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