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