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(); }
bool cModeleAnalytiqueComp::FiltragePointHomologues ( const ElPackHomologue & aPackInit, ElPackHomologue & aNewPack, double aTol, double aFiltre ) { bool GotOut = false; Box2dr aBox1 = mAppli.PDV1()->BoxIm().AddTol(aTol); Box2dr aBox2 = mAppli.PDV2()->BoxIm().AddTol(aTol); Box2dr aBoxF1 = mAppli.PDV1()->BoxIm().AddTol(aFiltre); Box2dr aBoxF2 = mAppli.PDV2()->BoxIm().AddTol(aFiltre); for ( ElPackHomologue::const_iterator iT = aPackInit.begin(); iT != aPackInit.end(); iT++ ) { if ( ( aBoxF1.inside(iT->P1())) && ( aBoxF2.inside(iT->P2())) ) { bool thisOut = (! aBox1.inside(iT->P1())) || (! aBox2.inside(iT->P2())); if (thisOut) { std::cout << aBox1._p1 << aBox2._p1 << "\n"; std::cout << "OUT " << iT->P1() << " " << iT->P2() << " " << iT->Pds()<< "\n"; } GotOut = GotOut || thisOut; aNewPack.Cple_Add(iT->ToCple()); } } return GotOut; }
CpleEpipolaireCoord * CpleEpipolaireCoord::PolynomialFromHomologue ( CpleEpipolaireCoord * aSolApprox, REAL aResiduMin, const ElPackHomologue & aPackH, INT aDegre, Pt2dr aDir1, Pt2dr aDir2 ) { StatElPackH aStat(aPackH); Polynome2dReal aPol1(aDegre,aStat.RMax1()); Polynome2dReal aPol2(aDegre,aStat.RMax2()); INT aNbInc =0; for (INT k=0; k<aPol1.NbMonome() ; k++) { const Monome2dReal & aMon = aPol1.KthMonome(k); if (aMon.DegreX() != 0) aNbInc++; aNbInc++; } SystLinSurResolu aSys (aNbInc,aStat.NbPts()) ; /* cGenSysSurResol * aSys = (aSolApprox == (CpleEpipolaireCoord *)0 ) ? new SystLinSurResolu(aNbInc,aStat.NbPts()) : new L2SysSurResol(aNbInc); */ Im1D_REAL8 aVecPds(aNbInc); REAL8 * aDVP = aVecPds.data(); for ( ElPackHomologue::const_iterator itC = aPackH.begin(); itC != aPackH.end(); itC++ ) { REAL aPdsResidu = 1; if (aSolApprox) { Pt2dr aQ1 = aSolApprox->EPI1().Direct(itC->P1()); Pt2dr aQ2 = aSolApprox->EPI2().Direct(itC->P2()); REAL aResidu = ElAbs(aQ1.y-aQ2.y); aPdsResidu = 1/sqrt(ElSquare(aResidu)+ElSquare(aResiduMin)); } Pt2dr aP1 = ( itC->P1() -aStat.Cdg1()) / aDir1; Pt2dr aP2 = ( itC->P2() -aStat.Cdg2()) / aDir2; aNbInc=0; for (INT k=0; k<aPol1.NbMonome() ; k++) { const Monome2dReal & aMon1 = aPol1.KthMonome(k); if (aMon1.DegreX() != 0) { aDVP[aNbInc++] = -aMon1(aP1); } const Monome2dReal & aMon2 = aPol2.KthMonome(k); aDVP[aNbInc++] = aMon2(aP2); } // aSys->GSSR_AddNewEquation(itC->Pds()*aPdsResidu,aDVP,aP1.y); aSys.PushEquation(aVecPds,aP1.y,itC->Pds()*aPdsResidu); } bool aOk; // Im1D_REAL8 aSol = aSys->GSSR_Solve(&aOk); Im1D_REAL8 aSol = (aSolApprox ? aSys.L2Solve(&aOk) : aSys.L1Solve()); aNbInc=0; { for (INT k=0; k<aPol1.NbMonome() ; k++) { const Monome2dReal & aMon1 = aPol1.KthMonome(k); if (aMon1.DegreX() != 0) { aPol1.SetCoeff(k,aSol.data()[aNbInc++]); } else aPol1.SetCoeff(k,(aMon1.DegreY() == 1)*aStat.RMax1()); aPol2.SetCoeff(k,aSol.data()[aNbInc++]); } } PolynomialEpipolaireCoordinate * anEpi1 = new PolynomialEpipolaireCoordinate ( aStat.Cdg1(), aDir1, aPol1, aStat.RMax1() ); PolynomialEpipolaireCoordinate * anEpi2 = new PolynomialEpipolaireCoordinate ( aStat.Cdg2(), aDir2, aPol2, aStat.RMax2() ); // delete aSys; return new CpleEpipolaireCoord(anEpi1,anEpi2); }