void cEqObsBlockCam::DoAMD(cAMD_Interf * anAMD) { std::vector<int> aNums; aNums.push_back(mRotRT0->IncInterv().NumBlocAlloc()); aNums.push_back(mRotLT0->IncInterv().NumBlocAlloc()); aNums.push_back(mRotRT1->IncInterv().NumBlocAlloc()); aNums.push_back(mRotLT1->IncInterv().NumBlocAlloc()); for (int aK1=0 ; aK1 <int(aNums.size()) ; aK1++) for (int aK2=aK1 ; aK2 <int(aNums.size()) ; aK2++) anAMD->AddArc(aNums[aK1],aNums[aK2],true); }
cEqOffsetGPS::cEqOffsetGPS(cRotationFormelle & aRF,cBaseGPS & aBase,bool doGenCode) : mSet (aRF.Set()), mRot (&aRF), mBase (&aBase), mGPS ("GPS"), mNameType ("cEqObsBaseGPS" + std::string(aRF.IsGL() ? "_GL" : "")), mResidu (mRot->C2M(mBase->BaseInc())- mGPS.PtF()), mFoncEqResidu (0) { /* ELISE_ASSERT ( (! aRF.IsGL()), "cEqOffsetGPS to complete in Gimbal Lock Mode" ); */ AllowUnsortedVarIn_SetMappingCur = true; ELISE_ASSERT ( mRot->Set()==mBase->Set(), "cEqOffsetGPS Rotation & Base do no belong to same set of unknown" ); mRot->IncInterv().SetName("Orient"); mBase->IncInterv().SetName("Base"); mLInterv.AddInterv(mRot->IncInterv()); mLInterv.AddInterv(mBase->IncInterv()); if (doGenCode) { GenCode(); return; } mFoncEqResidu = cElCompiledFonc::AllocFromName(mNameType); ELISE_ASSERT(mFoncEqResidu!=0,"Cannot allocate cEqObsBaseGPS"); mFoncEqResidu->SetMappingCur(mLInterv,mSet); // GL mGPS.InitAdr(*mFoncEqResidu); mSet->AddFonct(mFoncEqResidu); }
const std::vector<double> & cEqObsBlockCam::AddObs(const double & aPdsTr,const double & aPdsMatr) { #if (HandleGL) mMatR0.SetEtat(mRotRT0->MGL()); mMatL0.SetEtat(mRotLT0->MGL()); mMatR1.SetEtat(mRotRT1->MGL()); mMatL1.SetEtat(mRotLT1->MGL()); #endif // mGPS.SetEtat(aGPS); std::vector<double> aVPds; for (int aK=0 ; aK<3; aK++) aVPds.push_back(aPdsTr); for (int aK=0 ; aK<9; aK++) aVPds.push_back(aPdsMatr); // Compute the values and its derivative (in class cCodeBlockCam here) // Link it to the real index of the variable // fill the covariance stuff taking into account these values return mSet->VAddEqFonctToSys(mFoncEqResidu,aVPds,false); }
// Compute the parameter of the transformation of Point in L coordinate // to point in R coordinates void CalcParamEqRel ( Pt3d<Fonc_Num> & aTr, ElMatrix<Fonc_Num> & aMat, cRotationFormelle & aRotR, cRotationFormelle & aRotL, int aRNumGl, int aLNumGl ) { #if (HandleGL != 1) ELISE_ASSERT(! aRotR.IsGL(),"Guimbal lock in Eq Rig still unsupported"); ELISE_ASSERT(! aRotL.IsGL(),"Guimbal lock in Eq Rig still unsupported"); aRNumGl=-1; aLNumGl=-1; #endif ElMatrix<Fonc_Num> aRMat = aRotR.MatFGLComplete(aRNumGl); ElMatrix<Fonc_Num> aLMat = aRotL.MatFGLComplete(aLNumGl); ElMatrix<Fonc_Num> aRMatInv = aRMat.transpose(); aMat = aRMatInv * aLMat; aTr = aRMatInv * (aRotL.COpt() - aRotR.COpt()); }
cEqRelativeGPS::cEqRelativeGPS ( cRotationFormelle & aR1, cRotationFormelle & aR2, bool CodeGen ) : mSet (aR1.Set()), mR1 (&aR1), mR2 (&aR2), mDif21 ("Dif21") { ELISE_ASSERT(mSet==(mR2->Set()),"Different unknown in cEqRelativeGPS"); mR1->IncInterv().SetName("Ori1"); mR2->IncInterv().SetName("Ori2"); mLInterv.AddInterv(mR1->IncInterv()); mLInterv.AddInterv(mR2->IncInterv()); if (CodeGen) { Pt3d<Fonc_Num> aResidu = mR2->COpt() - mR1->COpt() - mDif21.PtF(); std::vector<Fonc_Num> aV; aV.push_back(aResidu.x); aV.push_back(aResidu.y); aV.push_back(aResidu.z); cElCompileFN::DoEverything ( DIRECTORY_GENCODE_FORMEL, // Directory ou est localise le code genere mNameType, // donne les noms de .cpp et .h de classe aV, // expressions formelles mLInterv // intervalle de reference ); return; } mFoncEqResidu = cElCompiledFonc::AllocFromName(mNameType); ELISE_ASSERT(mFoncEqResidu!=0,"Cannot allocate cEqObsBaseGPS"); mFoncEqResidu->SetMappingCur(mLInterv,mSet); // GL mDif21.InitAdr(*mFoncEqResidu); mSet->AddFonct(mFoncEqResidu); }
cEqObsBlockCam::cEqObsBlockCam ( cRotationFormelle & aRotRT0, cRotationFormelle & aRotLT0, cRotationFormelle & aRotRT1, cRotationFormelle & aRotLT1, bool doGenerateCode ) : mSet (aRotRT0.Set()), mRotRT0 (&aRotRT0), mRotLT0 (&aRotLT0), mRotRT1 (&aRotRT1), mRotLT1 (&aRotLT1), mNameType ("cCodeBlockCam"), mFoncEqResidu (0), #if (HandleGL) mMatR0("GL_MK0",3,3), mMatL0("GL_MK1",3,3), mMatR1("GL_MK2",3,3), mMatL1("GL_MK3",3,3) #endif { AllowUnsortedVarIn_SetMappingCur = true; ELISE_ASSERT(mSet==mRotRT0->Set(),"Different sets incEqObsBlockCam"); ELISE_ASSERT(mSet==mRotLT0->Set(),"Different sets incEqObsBlockCam"); ELISE_ASSERT(mSet==mRotRT1->Set(),"Different sets incEqObsBlockCam"); ELISE_ASSERT(mSet==mRotLT1->Set(),"Different sets incEqObsBlockCam"); mRotRT0->IncInterv().SetName("OriR0"); mRotLT0->IncInterv().SetName("OriL0"); mRotRT1->IncInterv().SetName("OriR1"); mRotLT1->IncInterv().SetName("OriL1"); mLInterv.AddInterv(mRotRT0->IncInterv()); mLInterv.AddInterv(mRotLT0->IncInterv()); mLInterv.AddInterv(mRotRT1->IncInterv()); mLInterv.AddInterv(mRotLT1->IncInterv()); if (doGenerateCode) { GenerateCode(); return; } mFoncEqResidu = cElCompiledFonc::AllocFromName(mNameType); ELISE_ASSERT(mFoncEqResidu!=0,"Cannot allocate cEqObsBlockCam"); mFoncEqResidu->SetMappingCur(mLInterv,mSet); #if (HandleGL) mMatR0.InitAdr(*mFoncEqResidu); mMatL0.InitAdr(*mFoncEqResidu); mMatR1.InitAdr(*mFoncEqResidu); mMatL1.InitAdr(*mFoncEqResidu); #endif mSet->AddFonct(mFoncEqResidu); }