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

}
Пример #2
0
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());

}
Пример #5
0
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);


}