C++ (Cpp) ElMatrix::GetCol Examples

Example #1

File: PowelOptimize.cpp Project: xialang2012/micmac-archeos

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