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);
}
