void cImplemBlockCam::EstimCurOri(const cEstimateOrientationInitBlockCamera & anEOIB)
{
cLiaisonsSHC aLSHC;
for (int aKC=0 ; aKC<mNbCam ; aKC++)
{
if (anEOIB.Show().Val())
std::cout << "=================================================\n";
Pt3dr aSomTr(0,0,0);
double aSomP=0;
ElMatrix<double> aSomM(3,3,0.0);
for (int aKT=0 ; aKT<mNbTime ; aKT++)
{
cIBC_ImsOneTime * aTime = mNum2ITime[aKT];
cPoseCam * aP0 = aTime->Pose(0);
cPoseCam * aP1 = aTime->Pose(aKC);
if (aP0 && aP1)
{
ElRotation3D aR0toM = aP0->CurCam()->Orient().inv(); // CONV-ORI
ElRotation3D aR1toM = aP1->CurCam()->Orient().inv();
ElRotation3D aR1to0 = aR0toM.inv() * aR1toM; // CONV-ORI
if (anEOIB.Show().Val())
{
std::cout << " EstimCurOri " << aP0->Name() << " " << aP1->Name() << "\n";
std::cout << " " << aR1to0.ImAff(Pt3dr(0,0,0))
<< " " << aR1to0.teta01()
<< " " << aR1to0.teta02()
<< " " << aR1to0.teta12()
<< "\n";
}
aSomTr = aSomTr+ aR1to0.tr();
aSomM += aR1to0.Mat();
aSomP++;
}
}
if (aSomP)
{
aSomTr = aSomTr / aSomP;
aSomM *= 1.0/aSomP;
aSomM = NearestRotation(aSomM);
ElRotation3D aRMoy(aSomTr,aSomM,true);
std::cout << " ========== AVERAGE =========== \n";
std::cout << " " << aRMoy.ImAff(Pt3dr(0,0,0))
<< " tetas " << aRMoy.teta01()
<< " " << aRMoy.teta02()
<< " " << aRMoy.teta12()
<< "\n";
cParamOrientSHC aP;
aP.IdGrp() = mNum2Cam[aKC]->NameCam();
aP.Vecteur() = aRMoy.ImAff(Pt3dr(0,0,0));
aP.Rot() = ExportMatr(aSomM);
aLSHC.ParamOrientSHC().push_back(aP);
}
}
mEstimSBC.LiaisonsSHC().SetVal(aLSHC);
}
Data_Hdroite::Data_Hdroite():
_p0 (Pt3dr(0,0,0)),
_p1 (Pt3dr(0,0,0)),
_ray(Pt3dr(0,0,0)),
_nu(true),
_dist(0)
{
}
Pt3dr cGenSysSurResol::Pt3dSolInter(bool * aOk)
{
Im1D_REAL8 aSol = GSSR_Solve(aOk);
if (aOk && (! *aOk))
return Pt3dr(1e33,-1e44,0);
ELISE_ASSERT(aSol.tx()==3,"cGenSysSurResol::SolInter");
return Pt3dr(aSol.data()[0],aSol.data()[1],aSol.data()[2]);
}
void GenerateCodeEqOffsetGPS(bool aGL)
{
cSetEqFormelles aSet;
ElRotation3D aRot(Pt3dr(0,0,0),0,0,0);
cRotationFormelle * aRF = aSet.NewRotation (cNameSpaceEqF::eRotLibre,aRot);
aRF->SetGL(aGL);
cBaseGPS * aBase = aSet.NewBaseGPS(Pt3dr(0,0,0));
aSet.NewEqOffsetGPS(*aRF,*aBase,true);
}
void TestOriBundle()
{
for (int aT=0 ; aT<10000 ; aT++)
{
Pt3dr aPA1 = GrP3dRand(0,3);
Pt3dr aPA2 = GrP3dRand(1,3);
Pt3dr aPA3 = GrP3dRand(2,3);
Pt3dr aBase = Pt3dr(0,-1,0) + P3dRand() * 0.1;
double aT12 = 0.1*NRrandom3();
double aT13 = 0.1*NRrandom3();
double aT23 = 0.1*NRrandom3();
ElMatrix<double> aRot = ElMatrix<double>::Rotation(aT12,aT13,aT23);
std::cout << "TETAIN " << aT12 << " " << aT13 << " " << aT23 << "\n\n";
Pt3dr aPB1 = PInBCoord(aBase,aRot,aPA1);
Pt3dr aPB2 = PInBCoord(aBase,aRot,aPA2);
Pt3dr aPB3 = PInBCoord(aBase,aRot,aPA3);
/*
Pt3dr aP1 = Pt3dr(NRrandom3(),NRrandom3(),10+NRrandom3());
Pt3dr aP1 = Pt3dr(NRrandom3(),NRrandom3(),10+NRrandom3());
*/
std::list<ElMatrix<double> > aLR = OriFromBundle(aBase,aPA1,aPA2,aPA3,aPB1,aPB2,aPB3,30000,10,10);
std::cout << "================== " << aT << "\n"; getchar();
}
}
void GenCodeAppui(bool C2M,bool isFixe,bool isGL,bool isAFocal,bool wDist,const std::string & aType,bool EqDroite)
{
std::vector<double> aPAF;
if (isAFocal)
{
aPAF.push_back(0.0);
aPAF.push_back(0.0);
}
// std::cout << "Type Appui = " << aType << "ENTRE GETCHAR \n"; getchar();
cSetEqFormelles aSet;
cParamIntrinsequeFormel * aPIF = PIF_For_GC(C2M,aType,aSet,aPAF);
ElRotation3D aRot(Pt3dr(0,0,0),0,0,0);
// Genere auto les appuis fixe en X et Y
if (isFixe)
{
aPIF->NewCam(cNameSpaceEqF::eRotFigee,aRot,0,"toto",true,true);
}
else
{
cCameraFormelle * aCam = aPIF->NewCam(cNameSpaceEqF::eRotFigee,aRot,0,"toto",false,false);
if (isGL)
aCam->SetGL(true);
aCam->AddForUseFctrEqAppuisInc(true,false,wDist,EqDroite);
aCam->AddForUseFctrEqAppuisInc(true,true,wDist,EqDroite);
}
}
double cNewO_CombineCple::CostOneBase(const Pt3dr & aBase)
{
// mCurBase = mVCdtSel[anArc.x]->NormQ1Q2() ^ mVCdtSel[anArc.y]->NormQ1Q2();
mCurBase = aBase;
double aNormBase = euclid(mCurBase);
if (aNormBase<1e-9) return 1e9;
mCurBase = mCurBase / aNormBase;
double aRes = 0;
double aStepTeta= ((2*PI*mCurStep) / mNbStepTeta) / 2.0 ;
// std::cout << "aStepTeta= " << aStepTeta << "\n";
for (int aK =0 ; aK < int (mVCdtSel.size()) ; aK++)
{
ElSeg3D aS1(Pt3dr(0,0,0),mVCdtSel[aK]->mQ1);
ElSeg3D aS2(mCurBase,mCurBase+mVCdtSel[aK]->mQ2);
Pt3dr anI = aS1.PseudoInter(aS2);
double aDist = aS1.DistDoite(anI);
double aA1 = ElAbs(aS1.AbscOfProj(anI));
double aA2 = ElAbs(aS1.AbscOfProj(anI));
double anEc = aDist/aA1 + aDist/aA2;
aRes += (anEc*aStepTeta) / (anEc + aStepTeta);
}
return aRes / mVCdtSel.size();
}
void TraitementSpec_CdD (std::vector<cCibleCalib> & aVCC,tFINT aFCol)
{
std::vector<int> aVS1;
std::vector<Pt2dr> aVSP;
std::vector<Pt2dr> aVTgt;
for (int aK=0 ; aK<int(aVCC.size()) ; aK++)
{
int aCol = aFCol(aK);
ELISE_ASSERT(aCol>=0 || (aCol==0),"TraitementSpec_CdD");
while ( int(aVS1.size()) <= aCol)
{
aVS1.push_back(0);
aVSP.push_back(Pt2dr(0,0));
}
aVS1[aCol]++;
Pt3dr aP3 =aVCC[aK].Position();
aVSP[aCol] = aVSP[aCol]+Pt2dr(aP3.x,aP3.y);
}
for (int aK=0 ;aK<int(aVSP.size()) ; aK++)
{
aVSP[aK] = aVSP[aK] / aVS1[aK];
aVTgt.push_back(Pt2dr(0,0));
}
for (int aK=0 ; aK<int(aVCC.size()) ; aK++)
{
int aC0 = aFCol(aK);
Pt2dr aTgt(0,0);
if (aC0==0)
{
Pt2dr aD01 = vunit(aVSP[1]-aVSP[0]);
Pt2dr aD12 = vunit(aVSP[2]-aVSP[1]);
aTgt = vunit(aD01 + (aD01-aD12)/2.0);
}
else if (aC0 == int(aVSP.size())-1)
{
Pt2dr aDP1P0 = vunit(aVSP[aC0]-aVSP[aC0-1]);
Pt2dr aDP2P1 = vunit(aVSP[aC0-1]-aVSP[aC0-2]);
aTgt = vunit(aDP1P0 + (aDP1P0-aDP2P1)/2.0);
}
else
{
Pt2dr aDP0 = vunit(aVSP[aC0]-aVSP[aC0-1]);
Pt2dr aD0N = vunit(aVSP[aC0+1]-aVSP[aC0]);
aTgt = vunit((aDP0+aD0N)/2.0);
}
aVTgt[aC0] = aTgt;
Pt2dr aN = aTgt * Pt2dr(0,1);
aVCC[aK].Normale() = Pt3dr(aN.x,aN.y,0);
}
for (int aK=0 ;aK<int(aVSP.size()) ; aK++)
std::cout << aK << " :: " << aVTgt[aK] << "\n";
}
Pt3dr CentreAJ(const cAvionJauneDocument & anAJD)
{
return Pt3dr
(
anAJD.navigation().sommet().x(),
anAJD.navigation().sommet().y(),
anAJD.navigation().altitude().value()
);
}
ElMatrix<double> RotationVecAJ(const cAvionJauneDocument & anAJD)
{
double aCap = AJ2Radian(anAJD.navigation().capAvion());
// Envoie de la camera vers l'avion
ElMatrix<double> aRCam2Av = ElMatrix<double>::Rotation
(
Pt3dr(1, 0, 0),
Pt3dr(0,-1, 0),
Pt3dr(0, 0,-1)
);
ElMatrix<double> aMCap = ElMatrix<double>::Rotation(aCap,0,0);
ElMatrix<double> aRAv2Ter = aMCap;
return aRAv2Ter * aRCam2Av;
}
double cLiaisORPO::Residu()
{
SetOrCam();
if (! mProfCalc)
{
mProfCalc = true;
SetProf();
}
// ElRotation3D aC1toM = mCam1.Orientation().inv();
// ElRotation3D aMtoC2 = mCam2.Orientation();
ElRotation3D aC1toC2 = mCam2.Orient() * mCam1.Orient().inv();
Pt3dr aBaseInC2 = aC1toC2.ImAff(Pt3dr(0,0,0));
ElMatrix<double> aMat = aC1toC2.Mat() ;
Pt3dr aC0,aC1,aC2;
aMat.GetCol(0,aC0);
aMat.GetCol(1,aC1);
aMat.GetCol(2,aC2);
double sDist=0.0;
int aK = 0;
for (ElPackHomologue::const_iterator itP =mPack.begin() ; itP!=mPack.end(); itP++)
{
double aPds = itP->Pds();
double aLambda = mProfs.at(aK);
Pt2dr aPIm1 = itP->P1();
Pt3dr aRay1 = aC0 * aPIm1.x + aC1 * aPIm1.y + aC2;
Pt3dr aPTerA = aBaseInC2 + aRay1 * (aLambda *0.99);
Pt3dr aPTerB = aBaseInC2 + aRay1 * (aLambda *1.01);
Pt2dr aProjA(aPTerA.x/aPTerA.z,aPTerA.y/aPTerA.z);
Pt2dr aProjB(aPTerB.x/aPTerB.z,aPTerB.y/aPTerB.z);
Pt2dr aVAB (aProjB.y-aProjA.y,aProjA.x-aProjB.x);
double aD2 = ElAbs(scal(aVAB,aProjA-itP->P2()) / euclid(aVAB));
sDist += aPds*aD2;
aK++;
}
if (DEBUG_POWEL)
{
std::cout << sDist/mPack.size() << "\n"; // getchar();
}
return sDist * mPPL->Pds().Val();
}
void GenerateCodeEqRelativeGPS()
{
cSetEqFormelles aSet;
ElRotation3D aRot(Pt3dr(0,0,0),0,0,0);
cRotationFormelle * aRF1 = aSet.NewRotation (cNameSpaceEqF::eRotLibre,aRot);
cRotationFormelle * aRF2 = aSet.NewRotation (cNameSpaceEqF::eRotLibre,aRot);
new cEqRelativeGPS(*aRF1,*aRF2,true);
}
Pt3dr cNewO_CombineCple::PInt2Tetas(const Pt3di & aP) const
{
return Pt3dr
(
K2Teta(aP.x),
K2Teta(aP.y),
K2Teta(aP.z)
);
}
void GenCodeSurf()
{
if (1)
{
ElSeg3D aSeg(Pt3dr(0,0,0),Pt3dr(1,0,0));
cCylindreRevolution aCyl(true,aSeg,Pt3dr(0,1,0));
cSetEqFormelles aSet;
aSet.AllocCylindre(aCyl,true);
}
if (1)
{
cSetEqFormelles aSet;
Pt3dr aP(0,0,0);
cSolBasculeRig aSBR(aP,aP,ElMatrix<double>::Rotation(0,0,0),1);
aSet.NewEqObsBascult(aSBR,true);
}
}
Pt3dr cRawNuage::GetPt(const Pt2di & anI) const
{
return Pt3dr
(
mTX.get(anI),
mTY.get(anI),
mTZ.get(anI)
);
}
int LucasChCloud_main(int argc,char ** argv)
{
//===================== PARAMETRES EN DUR ==============
std::string aNameNuage,aNameOut;
ElInitArgMain
(
argc,argv,
LArgMain() << EAMC(aNameNuage,"Name input "),
LArgMain() << EAM(aNameOut,"Out",true)
);
if (! EAMIsInit(&aNameOut))
aNameOut = DirOfFile(aNameNuage) + "TestScale_" + NameWithoutDir(aNameNuage);
cXML_ParamNuage3DMaille aXML = StdGetObjFromFile<cXML_ParamNuage3DMaille>
(
aNameNuage,
StdGetFileXMLSpec("SuperposImage.xml"),
"XML_ParamNuage3DMaille",
"XML_ParamNuage3DMaille"
);
double aScale = 2;
cRepereCartesien aRep;
aRep.Ori() = Pt3dr(0,0,0);
aRep.Ox() = Pt3dr(aScale,0,0);
aRep.Oy() = Pt3dr(0,aScale,0);
aRep.Oz() = Pt3dr(0,0,aScale);
aXML.RepereGlob().SetVal(aRep);
MakeFileXML(aXML,aNameOut);
return 0;
}
void cOriFromBundle::AnalyseSol(const ElMatrix<double> & aMat) const
{
AnalyseSol(aMat,mDir1A,mDir1B);
AnalyseSol(aMat,mDir2A,mDir2B);
AnalyseSol(aMat,mDir3A,mDir3B);
ElRotation3D aR(Pt3dr(0,0,0),aMat,true);
std::cout << "TETA " << aR.teta01() << " " << aR.teta02() << " " << aR.teta12() << "\n\n";
}
Pt3dr cEqRelativeGPS::AddObs(const Pt3dr & aDif12,const Pt3dr & aPds)
{
std::vector<double> aVPds;
aVPds.push_back(aPds.x);
aVPds.push_back(aPds.y);
aVPds.push_back(aPds.z);
const std::vector<REAL> & aResidu = mSet->VAddEqFonctToSys(mFoncEqResidu,aVPds,false);
return Pt3dr(aResidu.at(0),aResidu.at(1),aResidu.at(2));
}
void GenCodeAppuiGrid()
{
cSetEqFormelles aSet;
ElRotation3D aRot(Pt3dr(0,0,0),0,0,0);
cTriangulFormelle * aTri = aSet.NewTriangulFormelleUnitaire(2);
cRotationFormelle * aRF = aSet.NewRotation (cNameSpaceEqF::eRotLibre,aRot);
aSet.NewEqAppuiGrid(*aTri,*aRF,true);
}
void GenCodeLiaison(const std::string & aType,cNameSpaceEqF::eModeResidu aMode)
{
std::vector<double> aNoPAF;
std::cout << "TYPE liason = " << aType << "\n";
cSetEqFormelles aSet;
ElRotation3D aRot(Pt3dr(0,0,0),0,0,0);
cParamIntrinsequeFormel * pPIF = PIF_For_GC(true,aType,aSet,aNoPAF); // Liaisons : tjs C2M
cCameraFormelle * pCam1 = pPIF->NewCam(cNameSpaceEqF::eRotFigee,aRot);
cCameraFormelle * pCam2 = pPIF->NewCam(cNameSpaceEqF::eRotBaseU,aRot);
aSet.NewCpleCam(*pCam1,*pCam2,aMode,true);
}
Pt3dr cElN3D_EpipGen<Type,TBase>::Loc_Euclid2ProfAndIndex(const Pt3dr & aPe) const
{
Pt2dr aPIm = this->mCam->R3toF2(aPe);
return Pt3dr(aPIm.x,aPIm.y,ProfOfPtE(aPe));
/*
double aProf = mIsSpherik ?
euclid(aPe-mCentre) :
scal(aPe-mCentre,mDirPl) ;
Pt2dr aPIm = this->mCam->R3toF2(aPe);
return Pt3dr(aPIm.x,aPIm.y,CorZInv(aProf));
*/
}
Pt3d<double> L2SysSurResol::Solve3x3Sym(bool * OK)
{
cMSymCoffact3x3<double> aMCS(mDatatLi_Li);
if (std::abs(aMCS.mDet<1e-30))
{
if (OK) *OK = false;
return Pt3dr(1e40,1e50,1e70);
}
if (OK) *OK =true;
return aMCS.CoffVecInv(mDatabi_Li);
}
void cMEPCoCentrik::OneItereRotPur(ElMatrix<REAL> & aMat,double & anErrStd)
{
L2SysSurResol aSysLin3(3);
aSysLin3.GSSR_Reset(false);
std::vector<double> aVRes;
double aSomP=0;
double aSomErr=0;
for (ElPackHomologue::const_iterator itP=mPack.begin() ; itP!=mPack.end() ; itP++)
{
Pt3dr aQ1 = vunit(PZ1(itP->P1()));
Pt3dr aQ2 = aMat * vunit(PZ1(itP->P2()));
double aVQ2[3],aVQ1[3];
aQ2.to_tab(aVQ2);
aQ1.to_tab(aVQ1);
double anEcart = euclid(aQ1-aQ2);
aVRes.push_back(anEcart);
double aPds = itP->Pds() / (1 + ElSquare(anEcart / (2*anErrStd)));
aSomP += aPds;
aSomErr += aPds * square_euclid(aQ1-aQ2);;
ElMatrix<REAL> aMQ2 = MatProVect(aQ2);
for (int aY=0 ; aY< 3 ; aY++)
{
double aCoeff[3];
for (int aX=0 ; aX< 3 ; aX++)
aCoeff[aX] = aMQ2(aX,aY);
aSysLin3.GSSR_AddNewEquation(aPds,aCoeff,aVQ2[aY]-aVQ1[aY],0);
}
}
double anErrRobut = MoyKPPVal(aVRes,NbForEcart(aVRes.size()));
double anErrQuad = sqrt(aSomErr/aSomP);
if (0)
{
std::cout << "ERR QUAD " << anErrQuad * mFoc << " Robust " << anErrRobut * mFoc << "\n";
}
anErrStd = anErrQuad;
Im1D_REAL8 aSol = aSysLin3.GSSR_Solve (0);
double * aData = aSol.data();
ElMatrix<double> aMPV = MatProVect(Pt3dr(aData[0],aData[1],aData[2]));
// std::cout << " aData " << aData[0] << " " << aData[1] << " " << aData[2] << "\n";
// aMat = NearestRotation(aMat * (ElMatrix<double>(3,true) +aMPV));
aMat = NearestRotation((ElMatrix<double>(3,true) +aMPV) * aMat);
}
void GenerateCodeBlockCam()
{
cSetEqFormelles aSet;
ElRotation3D aRot(Pt3dr(0,0,0),0,0,0);
cRotationFormelle * aRotRT0 = aSet.NewRotation (cNameSpaceEqF::eRotLibre,aRot);
cRotationFormelle * aRotLT0 = aSet.NewRotation (cNameSpaceEqF::eRotLibre,aRot);
cRotationFormelle * aRotRT1 = aSet.NewRotation (cNameSpaceEqF::eRotLibre,aRot);
cRotationFormelle * aRotLT1 = aSet.NewRotation (cNameSpaceEqF::eRotLibre,aRot);
cEqObsBlockCam * aEOBC = aSet.NewEqBlockCal (*aRotRT0,*aRotLT0,*aRotRT1,*aRotLT1,true);
DoNothingButRemoveWarningUnused(aEOBC);
}
void GenCodeGrid (cNameSpaceEqF::eModeResidu aMode)
{
cSetEqFormelles aSet;
ElRotation3D aRot(Pt3dr(0,0,0),0,0,0);
cTriangulFormelle * aTr1 = aSet.NewTriangulFormelleUnitaire(2);
cTriangulFormelle * aTr2 = aSet.NewTriangulFormelleUnitaire(2);
cRotationFormelle * aRF1 = aSet.NewRotation (cNameSpaceEqF::eRotLibre,aRot);
cRotationFormelle * aRF2 = aSet.NewRotation (cNameSpaceEqF::eRotLibre,aRot);
aSet.NewCpleGridEq(*aTr1,*aRF1,*aTr2,*aRF2,aMode,true);
}
void getPlyBoundingBox( const string &aFilename, Pt3dr &oP0, Pt3dr &oP1 )
{
#if ELISE_QT_VERSION >=4
//~ #if 0
GlCloud *ply = GlCloud::loadPly(aFilename);
if ( !ply) ELISE_ERROR_EXIT("cannot load ply file [" << aFilename << ']');
if (ply->size() == 0) return;
float min[3], max[3];
{
QVector3D p = ply->getVertex(0).getPosition();
min[0] = max[0] = p.x();
min[1] = max[1] = p.y();
min[2] = max[2] = p.z();
}
const int plySize = ply->size();
for (int i = 1; i < plySize; i++)
{
QVector3D p = ply->getVertex(i).getPosition();
if (p.x() < min[0]) min[0] = p.x();
if (p.y() < min[1]) min[1] = p.y();
if (p.z() < min[2]) min[2] = p.z();
if (p.x() > max[0]) max[0] = p.x();
if (p.y() > max[1]) max[1] = p.y();
if (p.z() > max[2]) max[2] = p.z();
}
oP0 = Pt3dr((REAL)min[0], (REAL)min[1], (REAL)min[2]);
oP1 = Pt3dr((REAL)max[0], (REAL)max[1], (REAL)max[2]);
#else
ELISE_ERROR_EXIT("getPlyBoundingBox: no Qt");
#endif
}
Data_Hplan::Data_Hplan()
{
_p0 = Pt3dr(0,0,0);
_p1 = Pt3dr(1,0,0);
_p2 = Pt3dr(0,1,0);
_r_x = Pt3dr(1,0,0);
_r_y = Pt3dr(0,1,0);
_r_x = Pt3dr(0,0,1);
_nu = false;
}
void cOriFromBundle::AnalyseSol(const ElMatrix<double> & aMat,const Pt3dr & aPA, const Pt3dr & aPB) const
{
Pt3dr aBRor = aMat*aPB;
ElSeg3D aSA(Pt3dr(0,0,0),aPA);
ElSeg3D aSB(mBase,mBase+aBRor);
Pt3dr anI = aSA.PseudoInter(aSB);
double aAbsA = aSA.AbscOfProj(anI);
double aAbsB = aSB.AbscOfProj(anI);
std::cout << " "
<< ((aAbsA >0) ? "+ " : "- ")
<< ((aAbsB >0) ? "+ " : "- ")
<< " Z " << aPA.z << " " << aBRor.z << " " << anI.z << "\n";
}
void GenCodeLaserImage(bool Normalize,bool Multi,INT aNb,INT aDegre)
{
tParamAFocal aNOPAF;
cout << "GenCodeLaserImage\n";
CamStenopeIdeale aCam(true,1.0,Pt2dr(0,0),aNOPAF);
cSetEqFormelles aSet;
Im2D_REAL4 aI(1,1);
cLIParam_Image * aP1 = aSet.NewLIParamImage(aI,1.0,aCam,cNameSpaceEqF::eRotFigee);
cLIParam_Image * aP2 = aSet.NewLIParamImage(aI,1.0,aCam,cNameSpaceEqF::eRotFigee);
cRotationFormelle * aRotPts = 0;
if (aDegre >=0)
aRotPts = aSet.NewRotationEvol(ElRotation3D(Pt3dr(0,0,0),0,0,0),aDegre);
aSet.NewLIEqVueLaserIm(aRotPts,Multi,Normalize,aNb,*aP1,*aP2,true);
}
Pt3dr cEqOffsetGPS::AddObs(const Pt3dr & aGPS,const Pt3dr & aPds)
{
mGPS.SetEtat(aGPS);
if (mRot->IsGL())
{
ELISE_ASSERT(false,"cEqOffsetGPS::AddObs GL to complete");
// mMatriceGL (isGL ? new cMatr_Etat_PhgrF("GL",3,3) : 0),
// mMatriceGL->SetEtat(mRot.MGL());
}
std::vector<double> aVPds;
aVPds.push_back(aPds.x);
aVPds.push_back(aPds.y);
aVPds.push_back(aPds.z);
const std::vector<REAL> & aResidu = mSet->VAddEqFonctToSys(mFoncEqResidu,aVPds,false);
return Pt3dr(aResidu.at(0),aResidu.at(1),aResidu.at(2));
}
