cElHomographie cElHomographie::RansacInitH(const ElPackHomologue & aPack,int aNbRansac,int aNbMaxPts)
{
if ((aPack.size()<10) || (aNbMaxPts<4))
return cElHomographie(aPack,false);
cRandNParmiQ aRand(aNbMaxPts,aPack.size());
std::vector<ElCplePtsHomologues> aVCH;
for (ElPackHomologue::tCstIter itH=aPack.begin() ; itH!=aPack.end() ; itH++)
{
if (aRand.GetNext())
{
aVCH.push_back(itH->ToCple());
}
}
double anEcMin = 1e30;
cElHomographie aHMin = cElHomographie::Id();
while (aNbRansac)
{
int aK1 = NRrandom3(aVCH.size());
int aK2 = NRrandom3(aVCH.size());
int aK3 = NRrandom3(aVCH.size());
int aK4 = NRrandom3(aVCH.size());
if (
(aK1!=aK2) && (aK1!=aK3) && (aK1!=aK4)
&& (aK2!=aK3) && (aK2!=aK4)
&& (aK3!=aK4)
)
{
aNbRansac--;
ElPackHomologue aP4;
aP4.Cple_Add(aVCH[aK1]);
aP4.Cple_Add(aVCH[aK2]);
aP4.Cple_Add(aVCH[aK3]);
aP4.Cple_Add(aVCH[aK4]);
cElHomographie aSol = cElHomographie(aP4,true);
double anEcart = 0;
for (int aKP=0 ; aKP<int(aVCH.size()) ; aKP++)
{
anEcart += euclid(aSol.Direct(aVCH[aKP].P1()),aVCH[aKP].P2());
}
anEcart /= aVCH.size();
if (anEcart<anEcMin)
{
anEcMin = anEcart;
aHMin = aSol;
}
}
}
std::cout << "ECART H " << anEcMin << "\n";
return aHMin;
}
cElHomographie cElHomographie::MapingChScale(REAL aChSacle) const
{
return cElHomographie
(
mHX.MulCste(aChSacle),
mHY.MulCste(aChSacle),
mHZ.MulXY(1.0/aChSacle)
);
}
cElHomographie cElHomographie::Homotie(Pt2dr aP,REAL aSc)
{
return cElHomographie
(
cElComposHomographie( aSc, 0, aP.x),
cElComposHomographie( 0, aSc, aP.y),
cElComposHomographie( 0, 0, 1)
);
}
cElHomographie cElHomographie::Id()
{
return cElHomographie
(
cElComposHomographie(1,0,0),
cElComposHomographie(0,1,0),
cElComposHomographie(0,0,1)
);
}
cElHomographie cElHomographie::FromMatrix(const ElMatrix<REAL> & M)
{
return cElHomographie
(
cElComposHomographie( M(0,0),M(1,0),M(2,0)),
cElComposHomographie( M(0,1),M(1,1),M(2,1)),
cElComposHomographie( M(0,2),M(1,2),M(2,2))
);
}
cElHomographie cElHomographie::read(ELISE_fp & aFile)
{
cElComposHomographie aHX(0,0,0);
cElComposHomographie aHY(0,0,0);
cElComposHomographie aHZ(0,0,0);
aHX = cElComposHomographie::read(aFile);
aHY = cElComposHomographie::read(aFile);
aHZ = cElComposHomographie::read(aFile);
return cElHomographie(aHX,aHY,aHZ);
}
cElHomographie cElHomographie::SomPondHom(const std::vector<cElHomographie> & aVH,const std::vector<double> & aVP)
{
ELISE_ASSERT(aVH.size()==aVP.size(),"SomPondHom");
double aSomHXx = 0;
double aSomHXy = 0;
double aSomHX1 = 0;
double aSomHYx = 0;
double aSomHYy = 0;
double aSomHY1 = 0;
double aSomHZx = 0;
double aSomHZy = 0;
double aSomHZ1 = 0;
double aSomPds = 0;
for (int aK=0 ; aK<int(aVH.size()) ; aK++)
{
double aPds = aVP[aK];
const cElComposHomographie & aHX = aVH[aK].HX();
const cElComposHomographie & aHY = aVH[aK].HY();
const cElComposHomographie & aHZ = aVH[aK].HZ();
double aMul = aPds / aHZ.Coeff1();
aSomHXx += aHX.CoeffX() * aMul;
aSomHXy += aHX.CoeffY() * aMul;
aSomHX1 += aHX.Coeff1() * aMul;
aSomHYx += aHY.CoeffX() * aMul;
aSomHYy += aHY.CoeffY() * aMul;
aSomHY1 += aHY.Coeff1() * aMul;
aSomHZx += aHZ.CoeffX() * aMul;
aSomHZy += aHZ.CoeffY() * aMul;
aSomHZ1 += aHZ.Coeff1() * aPds;
aSomPds += aPds;
}
return cElHomographie
(
cElComposHomographie(aSomHXx/aSomPds,aSomHXy/aSomPds,aSomHX1/aSomPds),
cElComposHomographie(aSomHYx/aSomPds,aSomHYy/aSomPds,aSomHY1/aSomPds),
cElComposHomographie(aSomHZx/aSomPds,aSomHZy/aSomPds,aSomHZ1/aSomPds)
);
}
cElHomographie cElHomographie::Inverse() const
{
/*
ElPackHomologue aPack;
AddForInverse(aPack,Pt2dr( 1, 1));
AddForInverse(aPack,Pt2dr(-1, 1));
AddForInverse(aPack,Pt2dr( 1,-1));
AddForInverse(aPack,Pt2dr(-1,-1));
return cElHomographie(aPack,true);
*/
REAL a = mHX.mX, b = mHX.mY, c = mHX.m1;
REAL d = mHY.mX, e = mHY.mY, f = mHY.m1;
REAL ALPHA = mHZ.mX, BETA = mHZ.mY;
cElComposHomographie IX ( e-BETA * f , -b+ BETA*c , b*f-e*c );
cElComposHomographie IY ( -d+ALPHA*f , a-ALPHA*c , d*c-a*f );
cElComposHomographie IZ ( BETA*d-ALPHA*e , ALPHA*b-BETA*a , a*e-b*d );
return cElHomographie(IX,IY,IZ);
}
void cModeleAnalytiqueComp::SolveHomographie(const ElPackHomologue & aPackHom)
{
mHomogr = cElHomographie(aPackHom,mModele.HomographieL2().Val());
mBoxPoly = Box2dr(Pt2dr(0,0),Pt2dr(0,0));
if (mDegrPolAdd >0)
{
ElPackHomologue aPck2 = aPackHom;
Pt2dr aPMin(1e5,1e5);
Pt2dr aPMax(-1e5,-1e5);
for
(
ElPackHomologue::iterator iT = aPck2.begin();
iT != aPck2.end();
iT++
)
{
iT->P1() = mHomogr.Direct(iT->P1());
aPMin.SetInf(iT->P1());
aPMax.SetSup(iT->P1());
}
double anAmpl = ElMax(dist8(aPMin),dist8(aPMax));
mBoxPoly = Box2dr(aPMin,aPMax);
bool aPL2 = mModele.PolynomeL2().Val();
mPolX = aPck2.FitPolynome(aPL2,mDegrPolAdd,anAmpl,true);
mPolY = aPck2.FitPolynome(aPL2,mDegrPolAdd,anAmpl,false);
}
// Polynome2dReal ElPackHomologue::FitPolynome
{
cElXMLFileIn aFileXML(mNameXML);
aFileXML.PutElHomographie(mHomogr,"Homographie");
if (mDegrPolAdd >0)
{
cElXMLFileIn::cTag aTag(aFileXML,"PolynomeCompl"); aTag.NoOp();
aFileXML.PutPoly(mPolX,"XPoly");
aFileXML.PutPoly(mPolY,"YPoly");
aFileXML.PutPt2dr(mBoxPoly._p0,"PMinBox");
aFileXML.PutPt2dr(mBoxPoly._p1,"PMaxBox");
}
}
MakeInverseModele();
// Verification de la correction du calcul de l'inverse
if (0)
{
int aNb=10;
double aEps = 0.05;
for (int aKx=0 ; aKx<= aNb ; aKx++)
{
double aPdsX = ElMax(aEps,ElMin(1-aEps,aKx/double(aNb)));
for (int aKy=0 ; aKy<= aNb ; aKy++)
{
double aPdsY = ElMax(aEps,ElMin(1-aEps,aKy/double(aNb)));
Pt2dr aPRas = Pt2dr (mSzGl.x*aPdsX, mSzGl.x*aPdsY);
Pt2dr aPTer = mGeoTer.RDiscToR2(aPRas);
Pt2dr aP1 = Direct(Pt2dr(aPTer));
Pt2dr aQ2 = Inverse(aP1);
double anEr = euclid(aPTer,aQ2);
if (anEr>0.05)
{
std::cout << "Erreur = " << anEr << "\n";
std::cout << aPRas << aPTer << "\n";
Pt2dr aP0 = aPTer;
Pt2dr aP1 = mGeom2.CorrigeDist1(aP0);
Pt2dr aP2 = CorrecDirecte(aP1);
Pt2dr aP3 = mGeom2.InvCorrDist2(aP2);
Pt2dr aQ0 = mGeom2.InvCorrDist1(aP1);
std::cout << "pq0= " << euclid(aP0,aQ0) << aP0 << aQ0 << "\n";
Pt2dr aQ1 = CorrecInverse(aP2);
std::cout << "pq1= " << euclid(aP1,aQ1) << aP1 << aQ1 << "\n";
Pt2dr aQ2 = mGeom2.CorrigeDist2(aP3);
std::cout << "pq2= " << euclid(aP2,aQ2) << aP2 << aQ2 << "\n";
std::cout << "P3 " << aP3 << "\n";
ELISE_ASSERT(false,"MakeInverseModele Pb!!");
}
}
}
}
if (mModele.ExportImage().Val() || mModele.ReuseResiduelle().Val() )
{
Im2D_REAL4 anImX(mSzGl.x,mSzGl.y);
TIm2D<REAL4,REAL8> aTImX(anImX);
Im2D_REAL4 anImY(mSzGl.x,mSzGl.y);
TIm2D<REAL4,REAL8> aTImY(anImY);
Pt2di aPRas;
double aPX0[2] = {0.0,0.0};
for (aPRas.x=0 ; aPRas.x<mSzGl.x ; aPRas.x++)
{
for (aPRas.y=0 ; aPRas.y<mSzGl.y ; aPRas.y++)
{
Pt2dr aPTer = mGeoTer.DiscToR2(aPRas);
Pt2dr aP1 = Direct(Pt2dr(aPTer));
Pt2dr aP2 ;
if (mModele.ReuseResiduelle().Val())
aP2 = mGeom2.Objet2ImageInit_Euclid(Pt2dr(aPTer),aPX0);
else
aP2 = mGeom2.InvCorrDist2(mGeom2.CorrigeDist1(aPTer));
double aPx[2];
aPx[0] = aP1.x- aP2.x;
aPx[1] = aP1.y- aP2.y;
mGeoTer.PxReel2PxDisc(aPx,aPx);
aTImX.oset(aPRas,aPx[0]);
aTImY.oset(aPRas,aPx[1]);
}
}
if (mModele.ExportImage().Val())
{
Tiff_Im::Create8BFromFonc(mNameImX,mSzGl,ToUC(anImX.in()));
Tiff_Im::Create8BFromFonc(mNameImY,mSzGl,ToUC(anImY.in()));
}
if (mModele.ReuseResiduelle().Val())
{
Tiff_Im::Create8BFromFonc(mNameResX,mSzGl,ToUC(anImX.in()-ImPx(0)));
Tiff_Im::Create8BFromFonc(mNameResY,mSzGl,ToUC(anImY.in()-ImPx(1)));
}
}
if (mExpModeleGlobal)
{
const cGeomDiscFPx & aG = mAppli.GeomDFPxInit() ;
Box2dr aBox = aG.BoxEngl();
double aME =mModele.MailleExport().Val();
cDbleGrid aGrid
(
true, // P0P1 Direct par defaut maintien du comp actuel
true,
aBox._p0,aBox._p1,
Pt2dr(aME,aME),
*this,
"toto"
);
std::string aNameXML = mAppli.ICNM()->Assoc1To2
(
mModele.FCND_ExportModeleGlobal().Val(),
mAppli.PDV1()->Name(),
mAppli.PDV2()->Name(),
true
);
aGrid.SaveXML(mAppli.FullDirResult() + aNameXML);
}
}
cElHomographie cElHomographie::RobustInit(double & aDMIn,double * aQuality,const ElPackHomologue & aPack,bool & Ok ,int aNbTestEstim, double aPerc,int aNbMaxPts)
{
cElHomographie aRes = cElHomographie::Id();
Ok = false;
Pt2dr aCdg(0,0);
for (ElPackHomologue::tCstIter itH=aPack.begin() ; itH!=aPack.end() ; itH++)
{
aCdg = aCdg + itH->P1();
}
aCdg = aCdg / double(aPack.size());
std::vector<std::pair<Pt2dr,Pt2dr> > aV00;
std::vector<std::pair<Pt2dr,Pt2dr> > aV01;
std::vector<std::pair<Pt2dr,Pt2dr> > aV10;
std::vector<std::pair<Pt2dr,Pt2dr> > aV11;
std::vector<std::pair<Pt2dr,Pt2dr> > aVAll;
int aNbPtsTot = aPack.size();
int aCpt = 0;
for (ElPackHomologue::tCstIter itH=aPack.begin() ; itH!=aPack.end() ; itH++)
{
Pt2dr aP1 = itH->P1();
Pt2dr aP2 = itH->P2();
std::pair<Pt2dr,Pt2dr> aPair(aP1,aP2);
if ( (((aCpt-1)*aNbMaxPts)/aNbPtsTot) != ((aCpt*aNbMaxPts)/aNbPtsTot))
{
aVAll.push_back(aPair);
}
if (aP1.x < aCdg.x)
{
if (aP1.y < aCdg.y) aV00.push_back(aPair);
else aV01.push_back(aPair);
}
else
{
if (aP1.y < aCdg.y) aV10.push_back(aPair);
else aV11.push_back(aPair);
}
aCpt++;
}
if (aV00.empty() || aV01.empty() || aV10.empty() || aV11.empty() )
return aRes;
aDMIn = 1e30;
int aNbPts = aVAll.size();
int aNbKth = ElMax(1,ElMin(aNbPts-1,round_ni((aPerc/100.0) * aNbPts)));
std::vector<double> aVDist;
if (aNbMaxPts<aNbPtsTot)
aNbTestEstim = (aNbTestEstim*aNbPtsTot) / aNbMaxPts;
// int aKMIN = -1;
std::vector<double> aVD; // For tuning and show in if(0) ...
while (aNbTestEstim)
{
int aK00 = NRrandom3(aV00.size());
int aK01 = NRrandom3(aV01.size());
int aK10 = NRrandom3(aV10.size());
int aK11 = NRrandom3(aV11.size());
ElPackHomologue aP4;
AddPair(aP4,aV00[aK00]);
AddPair(aP4,aV01[aK01]);
AddPair(aP4,aV10[aK10]);
AddPair(aP4,aV11[aK11]);
cElHomographie aSol = cElHomographie(aP4,true);
aVDist.clear();
for (int aK=0 ; aK< aNbPts ; aK++)
{
Pt2dr aP1 = aVAll[aK].first;
Pt2dr aP2 = aVAll[aK].second;
/*
Pt2dr aDif = aP2 -aSol.Direct(aP1);
double aDx = ElAbs(aDif.x);
double aDy = ElAbs(aDif.y);
double aDist = (aDx+aDy + ElMax(aDx,aDy))/ 2.0;
*/
double aDist = QuickDist(aP2 -aSol.Direct(aP1));
aVDist.push_back(aDist);
}
double aSom = MoyKPPVal(aVDist,aNbKth);
aVD.push_back(aSom);
//std::cout << "Robust:Hom:SOM = " << aDMIn << " " << aSom << "\n";
if (aSom <aDMIn)
{
aRes = aSol;
aDMIn = aSom;
}
aNbTestEstim--;
}
// double aDMinInit = aDMIn;
ElPackHomologue aPckPds;
for (int anIterL2 = 0 ; anIterL2 < 4 ; anIterL2++)
{
aPckPds = ElPackHomologue();
aVDist.clear();
int aCpt = 0;
for (ElPackHomologue::tCstIter itH=aPack.begin() ; itH!=aPack.end() ; itH++)
{
Pt2dr aP1 = itH->P1();
Pt2dr aP2 = itH->P2();
double aDist = QuickDist(aP2 -aRes.Direct(aP1));
aVDist.push_back(aDist);
double aPds = 1/ (1+ 4*ElSquare(aDist/aDMIn));
aPckPds.Cple_Add(ElCplePtsHomologues(aP1,aP2,aPds));
aCpt++;
}
ELISE_ASSERT(aNbPtsTot==aPack.size() ,"KKKKK ????");
int aKTh = round_ni(aNbPtsTot * (aPerc/100.0));
ELISE_ASSERT(int(aVDist.size())==aNbPtsTot,"Compat MoyKPPVal/SplitArrounKthValue");
aDMIn = MoyKPPVal(aVDist,aKTh);
aRes = cElHomographie(aPckPds,true);
}
if (aQuality)
{
std::vector<double> aVEstim;
int aNbTestValid = 71;
for (int aKTest = 0 ; aKTest <aNbTestValid ; aKTest++)
{
ElPackHomologue aPckPdsA;
ElPackHomologue aPckPdsB;
cRandNParmiQ aSelec(aNbPtsTot/2,aNbPtsTot);
for (ElPackHomologue::tCstIter itH=aPack.begin() ; itH!=aPack.end() ; itH++)
{
Pt2dr aP1 = itH->P1();
Pt2dr aP2 = itH->P2();
double aDist = QuickDist(aP2 -aRes.Direct(aP1));
aVDist.push_back(aDist);
double aPds = 1/ sqrt(1+ ElSquare(aDist/aDMIn));
// if (NRrandom3() > 0.5)
if (aSelec.GetNext())
aPckPdsA.Cple_Add(ElCplePtsHomologues(aP1,aP2,aPds));
else
aPckPdsB.Cple_Add(ElCplePtsHomologues(aP1,aP2,aPds));
}
cElHomographie aResA = cElHomographie(aPckPdsA,true);
cElHomographie aResB = cElHomographie(aPckPdsB,true);
double aSomDist = 0;
for (ElPackHomologue::tCstIter itH=aPack.begin() ; itH!=aPack.end() ; itH++)
{
Pt2dr aP1 = itH->P1();
Pt2dr aQ = aRes.Direct(aP1);
Pt2dr aQA = aResA.Direct(aP1);
Pt2dr aQB = aResB.Direct(aP1);
double aDist = (QuickDist(aQ-aQA) + QuickDist(aQ-aQB) + QuickDist(aQB-aQA)) / 3.0;
aSomDist += aDist;
}
aSomDist /= aNbPtsTot;
aVEstim.push_back(aSomDist);
}
*aQuality = MedianeSup(aVEstim);
}
Ok= true;
return aRes;
}