void Craig_etal_L1 ( Im2D_REAL8 A, Im1D_REAL8 B, REAL TOLER, Im1D_REAL8 SOL, Im1D_REAL8 RESIDU ) { INT n = SOL.tx(); INT m = B.tx(); BENCH_ASSERT ( (A.tx() == n+2) && (A.ty() == m+2) && (B.tx() == m) && (SOL.tx() == n) && (RESIDU.tx() == m) ); Craig_Barrodale_Roberts_l1 ( m,n, A.data_lin(), B.data(), TOLER, SOL.data(), RESIDU.data() ); }
Im1D_REAL8 cManipOrdInc::ReordonneSol(Im1D_REAL8 aIm) { Im1D_REAL8 aRes (aIm.tx()); for (int aK=0 ; aK<aIm.tx() ; aK++) aRes.data()[aK] = aIm.data()[mAlloc2Solve[aK]]; return aRes; }
Pt3dr cGenSysSurResol::Pt3dSolInter(bool * aOk) { Im1D_REAL8 aSol = GSSR_Solve(aOk); if (aOk && (! *aOk)) return Pt3dr(1e33,-1e44,0); ELISE_ASSERT(aSol.tx()==3,"cGenSysSurResol::SolInter"); return Pt3dr(aSol.data()[0],aSol.data()[1],aSol.data()[2]); }
void cGenSysSurResol::GSSR_SolveEqFitDroite(REAL & aAx,REAL &aB,bool * aOk) { Im1D_REAL8 aSol = GSSR_Solve(aOk); if (aOk && (! *aOk)) return; ELISE_ASSERT(aSol.tx()==2,"cGenSysSurResol::GSSR_SolveEqFitDroite"); aAx = aSol.data()[0]; aB = aSol.data()[1]; }
void cGenSysSurResol::GSSR_SolveEqFitPlan(REAL & aAx,REAL &aBy,REAL & aC,bool * aOk) { Im1D_REAL8 aSol = GSSR_Solve(aOk); if (aOk && (! *aOk)) return; ELISE_ASSERT(aSol.tx()==3,"cGenSysSurResol::GSSR_SolveEqFiPlan"); aAx = aSol.data()[0]; aBy = aSol.data()[1]; aC = aSol.data()[2]; }
REAL SystLinSurResolu::Residu(Im1D_REAL8 anIm,INT iEq) const { AssertIndexEqValide(iEq); AssertIndexGoodNbVar(anIm.tx()); return Residu(anIm.data(),iEq); }
void cIncEnsembleCamera::OptLineaireOnDirL2 ( std::list<cIncSetLiaison *> * aListSL, const std::vector<cFonctrPond> & aFoncAux // bool CalcMatr ) { mListSl = aListSL; mFoncsAux = aFoncAux; SetOpt(); /* if (CalcMatr) { ResetEquation(); ScoreCurGen(true,true); } */ Im1D_REAL8 anIm = mSys->GSSR_Solve((bool *)0); SetImState0(); SetCurDir(anIm.data()); cIEC_OptimCurDir anOpti(*this); REAL aLambda = anOpti.Optim(0,1); SetLambda(aLambda); }
void cIncEnsembleCamera::OptimPowel ( std::list<cIncSetLiaison *> * aListSL, const std::vector<cFonctrPond> & aFoncAux, REAL tol, INT ItMax ) { mListSl = aListSL; mFoncsAux = aFoncAux; SetOpt(); Im1D_REAL8 I = CurVals(); cIEC_OptimPowel aOpt(*this); aOpt.powel(I.data(),tol,ItMax); SetPtCur(I.data()); }
void SystLinSurResolu::PushEquation ( Im1D_REAL8 aFormLin, REAL aValue, REAL aPds ) { AssertIndexGoodNbVar(aFormLin.tx()); PushEquation (aFormLin.data(),aValue,aPds); }
REAL SystLinSurResolu::L2SomResiduPond(Im1D_REAL8 aPt) const { AssertIndexGoodNbVar(aPt.tx()); REAL aRes = 0.0; REAL *aDataP = aPt.data(); for (INT iEq=0 ; iEq<mNbEqCur ; iEq++) aRes += mDataPds[iEq] * ElSquare(Residu(aDataP,iEq)); return aRes; }
void MakeFoncRepart(Im1D_REAL8 aH,int * aVMax=0) { double aNbP; ELISE_COPY(aH.all_pts(),aH.in(),sigma(aNbP)); REAL8 * aDH = aH.data(); for (int aK=1 ; aK<aNbH ; aK++) { if (aDH[aK] && aVMax) *aVMax = aK; aDH[aK] += aDH[aK-1]; } ELISE_COPY(aH.all_pts(),aH.in() * (255.0/aNbP),aH.out()); }
void cMEPCoCentrik::OneItereRotPur(ElMatrix<REAL> & aMat,double & anErrStd) { L2SysSurResol aSysLin3(3); aSysLin3.GSSR_Reset(false); std::vector<double> aVRes; double aSomP=0; double aSomErr=0; for (ElPackHomologue::const_iterator itP=mPack.begin() ; itP!=mPack.end() ; itP++) { Pt3dr aQ1 = vunit(PZ1(itP->P1())); Pt3dr aQ2 = aMat * vunit(PZ1(itP->P2())); double aVQ2[3],aVQ1[3]; aQ2.to_tab(aVQ2); aQ1.to_tab(aVQ1); double anEcart = euclid(aQ1-aQ2); aVRes.push_back(anEcart); double aPds = itP->Pds() / (1 + ElSquare(anEcart / (2*anErrStd))); aSomP += aPds; aSomErr += aPds * square_euclid(aQ1-aQ2);; ElMatrix<REAL> aMQ2 = MatProVect(aQ2); for (int aY=0 ; aY< 3 ; aY++) { double aCoeff[3]; for (int aX=0 ; aX< 3 ; aX++) aCoeff[aX] = aMQ2(aX,aY); aSysLin3.GSSR_AddNewEquation(aPds,aCoeff,aVQ2[aY]-aVQ1[aY],0); } } double anErrRobut = MoyKPPVal(aVRes,NbForEcart(aVRes.size())); double anErrQuad = sqrt(aSomErr/aSomP); if (0) { std::cout << "ERR QUAD " << anErrQuad * mFoc << " Robust " << anErrRobut * mFoc << "\n"; } anErrStd = anErrQuad; Im1D_REAL8 aSol = aSysLin3.GSSR_Solve (0); double * aData = aSol.data(); ElMatrix<double> aMPV = MatProVect(Pt3dr(aData[0],aData[1],aData[2])); // std::cout << " aData " << aData[0] << " " << aData[1] << " " << aData[2] << "\n"; // aMat = NearestRotation(aMat * (ElMatrix<double>(3,true) +aMPV)); aMat = NearestRotation((ElMatrix<double>(3,true) +aMPV) * aMat); }
void bench_triviale_opt_sous_contrainte() { // Miminise x2+y2, sous la contrainte x+y=2 L2SysSurResol aSys(2); double C[2] = {1,1}; aSys.GSSR_AddContrainte(C,3); double Fx[2] = {1,0}; aSys.GSSR_AddNewEquation(1.0,Fx,0); double Fy[2] = {0,1}; aSys.GSSR_AddNewEquation(1.0,Fy,0); Im1D_REAL8 aSol = aSys.GSSR_Solve(0); BENCH_ASSERT(std::abs(aSol.data()[0] -1.5)<epsilon); BENCH_ASSERT(std::abs(aSol.data()[1] -1.5)<epsilon); }
void cASAMG::ComputeIncidGradProf() { Im2DGen * aImProf = mStdN->ImProf(); L2SysSurResol aSys (3); Pt2di aP0; Im2D_Bits<1> aMasqTmp = ImMarqueurCC(mSz); TIm2DBits<1> aTMasqTmp(aMasqTmp); for (aP0.x=0 ; aP0.x<mSz.x ; aP0.x++) { for (aP0.y=0 ; aP0.y<mSz.y ; aP0.y++) { double Angle = 1.5; if (mTMasqN.get(aP0)) { cCalcPlanImage aCalcPl(CCDist(),aSys,aImProf); int aNb = OneZC(aP0,CCV4(),aTMasqTmp,1,0,mTMasqN,1,aCalcPl); ResetMarqueur(aTMasqTmp,aCalcPl.mVPts); if (aNb >= SeuimNbPtsCCDist()) { Im1D_REAL8 aSol = aSys.Solve((bool *)0); double * aDS = aSol.data(); Pt2dr aGrad (aDS[0],aDS[1]); Angle = euclid(aGrad); } /* */ } mTIncid.oset(aP0,ElMin(255,ElMax(0,round_ni(Angle*DynAng())))); } } }
void cIncEnsembleCamera::OptimJacobi ( std::list<cIncSetLiaison *> * aListSL, const std::vector<cFonctrPond> & aFoncAux ) { mListSl = aListSL; mFoncsAux = aFoncAux; SetOpt(); ELISE_ASSERT(mL2Opt,"Need L2 Sys for OptimJacobi"); mSysL2->GetMatr(mMatrL2,mMatrtB); jacobi_diag(mMatrL2,mMatrValP,mMatrVecP); mtBVecP = mMatrtB * mMatrVecP; cElRanGen aR; Im1D_REAL8 P0 = CurVals(); Im1D_REAL8 mImBest = CurVals(); REAL aScInit = ScoreCur(false); REAL aScMin = aScInit; REAL aScAmMin = aScInit; for (INT aTest =0 ; aTest < 100 ; aTest ++) { SetPtCur(P0.data()); Im1D_REAL8 aDir( NbVal(),0.0); for (INT aK = 0 ; aK < NbVal() ; aK++) { ELISE_ASSERT(mMatrValP(aK,aK) != 0,"Jcobi "); REAL Val = (mtBVecP(aK,0) / ElAbs(mMatrValP(aK,aK))) ; REAL aRan = aR.cNRrandom3() ; if (aRan < 0.25) { Val = 0; } else if (aRan < 0.5) ; /* { Val = Val; } */ else Val *= 3 * aR.cNRrandom3() -1; for (INT aY =0 ; aY< NbVal() ; aY++) aDir.data()[aY] += mMatrVecP(aK,aY) * Val; } ELISE_COPY(aDir.all_pts(),P0.in() + aDir.in(),aDir.out()); SetPtCur(aDir.data()); REAL aSc = ScoreCur(false); if (aSc < aScMin) { ElSetMin(aScMin,aSc); for (INT aK= 0 ; aK< 10 ; aK++) OneItereDevL1( aK < 3); REAL aSc = ScoreCur(false); if (aSc < aScAmMin) { aScAmMin = aSc; ELISE_COPY(mImBest.all_pts(),CurVals().in(),mImBest.out()); } } cout << aScMin << " " << aScInit << " " << aSc << " " << aScAmMin << "\n"; } SetPtCur(mImBest.data()); }
int Luc_main_XAlign(int argc,char ** argv) { //MMD_InitArgcArgv(argc,argv,3); std::string aFilePtsIn; //Reading the arguments ElInitArgMain ( argc,argv, LArgMain() << EAMC(aFilePtsIn,"Input file"), LArgMain() ); std::string aFilePtsOut="GCP_xAligned.xml"; std::ifstream file(aFilePtsIn.c_str(), ios::in); int nbIm; file >> nbIm; std::vector<Pt3dr> aVPts(nbIm); std::vector<Pt3dr> aVInc(nbIm); std::vector<std::string> aVName(nbIm,""); for(int i=0 ; i<nbIm ; i++) { string name; file >> aVName[i] >> aVPts[i].x >> aVPts[i].y >> aVPts[i].z >> aVInc[i].x >> aVInc[i].y >> aVInc[i].z; } file.close(); //Least Square // Create L2SysSurResol to solve least square equation with 3 unknown L2SysSurResol aSys(2); //For Each SIFT point double sumX=0, sumY=0; for(int i=0;i<int(aVPts.size());i++){ double aPds[2]={aVPts[i].x,1}; double poids=1; aSys.AddEquation(poids,aPds,aVPts[i].y); sumX=sumX+aVPts[i].x; sumY=sumY+aVPts[i].y; } Pt2dr aRotCenter; aRotCenter.x=sumX/aVPts.size();aRotCenter.y=sumY/aVPts.size(); bool Ok; Im1D_REAL8 aSol = aSys.GSSR_Solve(&Ok); double aAngle; if (Ok) { double* aData = aSol.data(); aAngle=atan(aData[0]); cout<<"Angle = "<<aAngle<<endl<<"Rot Center = "<<aRotCenter<<endl; for(int i=0;i<int(aVPts.size());i++){ Pt2dr aPt; aPt.x=aVPts[i].x; aPt.y=aVPts[i].y; aPt=Rot2D(aAngle, aPt, aRotCenter);aVPts[i].x=aPt.x;aVPts[i].y=aPt.y; } } //End Least Square cDicoAppuisFlottant aDico; for (int aKP=0 ; aKP<int(aVPts.size()) ; aKP++) { cOneAppuisDAF aOAD; aOAD.Pt() = aVPts[aKP]; aOAD.NamePt() = aVName[aKP]; aOAD.Incertitude() = aVInc[aKP]; aDico.OneAppuisDAF().push_back(aOAD); } MakeFileXML(aDico,aFilePtsOut); return 0; }
Pt3dr cMEPCoCentrik::ComputeNormBase() { // Valeur initiale par Ransac double aSomMin = 1e30; Pt3dr aBestNorm(0,0,0); for (int aCpt=0 ; aCpt<NbCpleBase ; ) { int aKA = NRrandom3(mVPlanBase.size()); int aKB = NRrandom3(mVPlanBase.size()); if (aKA!=aKB) { Pt3dr aN = mVPlanBase[aKA] ^ mVPlanBase[aKB]; if (euclid(aN) !=0) { aN = vunit(aN); double aSom = 0; for (int aK=0 ; aK< int(mVPlanBase.size()) ; aK++) { aSom += ElAbs(scal(aN,mVPlanBase[aK])); } aSom /= mVPlanBase.size(); if (aSom < aSomMin) { aSomMin = aSom; aBestNorm = aN; } aCpt++; } } } // Pt3dr aN0 = aBestNorm; // Affinaga par LSQ // Ort . (aBestNor + b B + c C) =0 for (int aCpt=0 ; aCpt < 3 ; aCpt++) { Pt3dr aB,aC; MakeRONWith1Vect(aBestNorm,aB,aC); L2SysSurResol aSysLin2(2); aSysLin2.GSSR_Reset(false); double aSomP = 0; double aSomE = 0; for (int aK=0 ; aK< int(mVPlanBase.size()) ; aK++) { double aCoeff[2]; Pt3dr aP = mVPlanBase[aK]; double aCste = scal(aP,aBestNorm); aCoeff[0] = scal(aP,aB); aCoeff[1] = scal(aP,aC); double aPds = 1 / (1+ElSquare(aCste/aSomMin)); aSysLin2.GSSR_AddNewEquation(aPds,aCoeff,-aCste,0); aSomE += aPds * ElSquare(aCste); aSomP += aPds; } Im1D_REAL8 aSol = aSysLin2.GSSR_Solve (0); double * aData = aSol.data(); aBestNorm = vunit(aBestNorm+aB*aData[0] + aC*aData[1]); // std::cout << "RESIDU " << sqrt(aSomE/aSomP) << "\n"; } if (mDoPly) { Pt3di aBleu(0,0,255); int aNb=1000; for (int aK=-aNb ; aK<= aNb ; aK++) { Pt3dr aN = aBestNorm * ((aK*1.2) / aNb); // std::cout << "AXE " << aN << "\n"; mPtsPly.push_back(aN); mColPly.push_back(aBleu); } Pt3dr aB,aC; MakeRONWith1Vect(aBestNorm,aB,aC); aNb=30; Pt3di aCyan(0,255,255); for (int aKb=-aNb ; aKb<= aNb ; aKb++) { for (int aKc=-aNb ; aKc<= aNb ; aKc++) { // Pt3dr aP = aB * (aKb*1.1)/aNb + aC * (aKc*1.1)/aNb; // mPtsPly.push_back(aP); // mColPly.push_back(aCyan); } } } return aBestNorm; }
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); }
int Luc_main(int argc, char ** argv) { //GET PSEUDO-RPC2D FOR ASTER FROM LATTICE POINTS std::string aTxtImage, aTxtCarto; std::string aFileOut = "RPC2D-params.xml"; //Reading the arguments ElInitArgMain ( argc, argv, LArgMain() << EAMC(aTxtImage, "txt file contaning the lattice point in the image coordinates", eSAM_IsPatFile) << EAMC(aTxtCarto, "txt file contaning the lattice point in the carto coordinates", eSAM_IsPatFile), LArgMain() << EAM(aFileOut, "Out", true, "Output xml file with RPC2D coordinates") ); //Reading the files vector<Pt2dr> aPtsIm, aPtsCarto; { std::ifstream fic(aTxtImage.c_str()); while (!fic.eof() && fic.good()) { double X, Y; fic >> X >> Y; Pt2dr aPt(X, Y); if (fic.good()) { aPtsIm.push_back(aPt); } } cout << "Read " << aPtsIm.size() << " points in image coordinates" << endl; //cout << aPtsIm << endl; std::ifstream fic2(aTxtCarto.c_str()); while (!fic2.eof() && fic2.good()) { double X, Y, Z; fic2 >> X >> Y >> Z; Pt2dr aPt(X, Y); if (fic2.good()) { aPtsCarto.push_back(aPt); } } cout << "Read " << aPtsCarto.size() << " points in cartographic coordinates" << endl; //cout << aPtsCarto << endl; } //Finding normalization parameters //divide Pts into X and Y vector<double> aPtsCartoX, aPtsCartoY, aPtsImX, aPtsImY; for (u_int i = 0; i < aPtsCarto.size(); i++) { aPtsCartoX.push_back(aPtsCarto[i].x); aPtsCartoY.push_back(aPtsCarto[i].y); aPtsImX.push_back(aPtsIm[i].x); aPtsImY.push_back(aPtsIm[i].y); } Pt2dr aPtCartoMin(*std::min_element(aPtsCartoX.begin(), aPtsCartoX.end()), *std::min_element(aPtsCartoY.begin(), aPtsCartoY.end())); Pt2dr aPtCartoMax(*std::max_element(aPtsCartoX.begin(), aPtsCartoX.end()), *std::max_element(aPtsCartoY.begin(), aPtsCartoY.end())); Pt2dr aPtImMin(*std::min_element(aPtsImX.begin(), aPtsImX.end()), *std::min_element(aPtsImY.begin(), aPtsImY.end())); Pt2dr aPtImMax(*std::max_element(aPtsImX.begin(), aPtsImX.end()), *std::max_element(aPtsImY.begin(), aPtsImY.end())); Pt2dr aCartoScale((aPtCartoMax.x - aPtCartoMin.x) / 2, (aPtCartoMax.y - aPtCartoMin.y) / 2); Pt2dr aImScale((aPtImMax.x - aPtImMin.x) / 2, (aPtImMax.y - aPtImMin.y) / 2); Pt2dr aCartoOffset(aPtCartoMin.x + (aPtCartoMax.x - aPtCartoMin.x) / 2, aPtCartoMin.y + (aPtCartoMax.y - aPtCartoMin.y) / 2); Pt2dr aImOffset(aPtImMin.x + (aPtImMax.x - aPtImMin.x) / 2, aPtImMin.y + (aPtImMax.y - aPtImMin.y) / 2); //Parameters too get parameters of P1 and P2 in --- Column=P1(X,Y)/P2(X,Y) --- where (X,Y) are Carto coordinates (idem for Row) //Function is 0=Poly1(X,Y)-Column*Poly2(X,Y) ==> Column*k=a+bX+cY+dXY+eX^2+fY^2+gX^2Y+hXY^2+iX^3+jY^3-Column(lX+mY+nXY+oX^2+pY^2+qX^2Y+rXY^2+sX^3+tY^3) //k=1 to avoid sol=0 L2SysSurResol aSysCol(19), aSysRow(19); //For all lattice points for (u_int i = 0; i<aPtsCarto.size(); i++){ //NORMALIZATION double X = (aPtsCarto[i].x - aCartoOffset.x) / aCartoScale.x; double Y = (aPtsCarto[i].y - aCartoOffset.y) / aCartoScale.y; double COL = (aPtsIm[i].x - aImOffset.x) / aImScale.x; double ROW = (aPtsIm[i].y - aImOffset.y) / aImScale.y; double aEqCol[19] = { (1), (X), (Y), (X*Y), (pow(X, 2)), (pow(Y, 2)), (pow(X, 2)*Y), (X*pow(Y, 2)), (pow(X, 3)), (pow(Y, 3)), //(COL), (-COL*X), (-COL*Y), (-COL*X*Y), (-COL*pow(X, 2)), (-COL*pow(Y, 2)), (-COL*pow(X, 2)*Y), (-COL*X*pow(Y, 2)), (-COL*pow(X, 3)), (-COL*pow(Y, 3)), }; aSysCol.AddEquation(1, aEqCol, COL); double aEqRow[19] = { (1), (X), (Y), (X*Y), (pow(X, 2)), (pow(Y, 2)), (pow(X, 2)*Y), (X*pow(Y, 2)), (pow(X, 3)), (pow(Y, 3)), //(ROW), (-ROW*X), (-ROW*Y), (-ROW*X*Y), (-ROW*pow(X, 2)), (-ROW*pow(Y, 2)), (-ROW*pow(X, 2)*Y), (-ROW*X*pow(Y, 2)), (-ROW*pow(X, 3)), (-ROW*pow(Y, 3)), }; aSysRow.AddEquation(1, aEqRow, ROW); } //Computing the result bool Ok; Im1D_REAL8 aSolCol = aSysCol.GSSR_Solve(&Ok); Im1D_REAL8 aSolRow = aSysRow.GSSR_Solve(&Ok); double* aDataCol = aSolCol.data(); double* aDataRow = aSolRow.data(); //Outputting results { std::ofstream fic(aFileOut.c_str()); fic << std::setprecision(15); fic << "<?xml version=\"1.0\" encoding=\"UTF-8\" standalone=\"no\"?>" << endl; fic << "<RPC2D>" << endl; fic << "\t<RFM_Validity>" << endl; fic << "\t\t<Direct_Model_Validity_Domain>" << endl; fic << "\t\t\t<FIRST_ROW>" << aPtImMin.x << "</FIRST_ROW>" << endl; fic << "\t\t\t<FIRST_COL>" << aPtImMin.y << "</FIRST_COL>" << endl; fic << "\t\t\t<LAST_ROW>" << aPtImMax.x << "</LAST_ROW>" << endl; fic << "\t\t\t<LAST_COL>" << aPtImMax.y << "</LAST_COL>" << endl; fic << "\t\t</Direct_Model_Validity_Domain>" << endl; fic << "\t\t<Inverse_Model_Validity_Domain>" << endl; fic << "\t\t\t<FIRST_X>" << aPtCartoMin.x << "</FIRST_X>" << endl; fic << "\t\t\t<FIRST_Y>" << aPtCartoMin.y << "</FIRST_Y>" << endl; fic << "\t\t\t<LAST_X>" << aPtCartoMax.x << "</LAST_X>" << endl; fic << "\t\t\t<LAST_Y>" << aPtCartoMax.y << "</LAST_Y>" << endl; fic << "\t\t</Inverse_Model_Validity_Domain>" << endl; fic << "\t\t<X_SCALE>" << aCartoScale.x << "</X_SCALE>" << endl; fic << "\t\t<X_OFF>" << aCartoOffset.x << "</X_OFF>" << endl; fic << "\t\t<Y_SCALE>" << aCartoScale.y << "</Y_SCALE>" << endl; fic << "\t\t<Y_OFF>" << aCartoOffset.y << "</Y_OFF>" << endl; fic << "\t\t<SAMP_SCALE>" << aImScale.x << "</SAMP_SCALE>" << endl; fic << "\t\t<SAMP_OFF>" << aImOffset.x << "</SAMP_OFF>" << endl; fic << "\t\t<LINE_SCALE>" << aImScale.y << "</LINE_SCALE>" << endl; fic << "\t\t<LINE_OFF>" << aImOffset.y << "</LINE_OFF>" << endl; fic << "\t</RFM_Validity>" << endl; for (int i = 0; i<10; i++) { fic << "<COL_NUMERATOR_" << i + 1 << ">" << aDataCol[i] << "</COL_NUMERATOR_" << i + 1 << ">" << endl; } fic << "<COL_DENUMERATOR_1>1</COL_DENUMERATOR_1>" << endl; for (int i = 10; i<19; i++) { fic << "<COL_DENUMERATOR_" << i - 8 << ">" << aDataCol[i] << "</COL_DENUMERATOR_" << i -8 << ">" << endl; } for (int i = 0; i<10; i++) { fic << "<ROW_NUMERATOR_" << i + 1 << ">" << aDataRow[i] << "</ROW_NUMERATOR_" << i + 1 << ">" << endl; } fic << "<ROW_DENUMERATOR_1>1</ROW_DENUMERATOR_1>" << endl; for (int i = 10; i<19; i++) { fic << "<ROW_DENUMERATOR_" << i - 8 << ">" << aDataRow[i] << "</ROW_DENUMERATOR_" << i - 8 << ">" << endl; } fic << "</RPC2D>" << endl; } cout << "Written functions in file " << aFileOut << endl; return 0; }
static cElComposHomographie ComputeHomFromHomologues ( const ElPackHomologue & aPack, bool aL2, cElComposHomographie& aHX, cElComposHomographie& aHY, cElComposHomographie& aHZ ) { ElPackHomologue::const_iterator anIt=aPack.begin(); // 0 Point : identite if (aPack.size() ==0) { aHX = cElComposHomographie(1,0,0); aHY = cElComposHomographie(0,1,0); aHZ = cElComposHomographie(0,0,1); return aHX; } // 1 Point : translation Pt2dr P1 = anIt->P1(); Pt2dr P2 = anIt->P2(); anIt++; if (aPack.size() ==1) { Pt2dr aTr = P2 - P1; aHX = cElComposHomographie(1,0,aTr.x); aHY = cElComposHomographie(0,1,aTr.y); aHZ = cElComposHomographie(0,0,1); return aHX; } // 2 Point : Similitude Pt2dr Q1 = anIt->P1(); Pt2dr Q2 = anIt->P2(); anIt++; if (aPack.size() ==2) { Pt2dr W = (Q2-P2) / (Q1-P1); Pt2dr aTr = P2 - W* P1; aHX = cElComposHomographie(W.x,-W.y,aTr.x); aHY = cElComposHomographie(W.y,W.x,aTr.y); aHZ = cElComposHomographie(0,0,1); return aHX; } // 3 Point : Affinite Pt2dr R1 = anIt->P1(); Pt2dr R2 = anIt->P2(); anIt++; if (aPack.size() ==3) { ElMatrix<REAL> M1(2,2); SetCol(M1,0,Q1-P1); SetCol(M1,1,R1-P1); ElMatrix<REAL> M2(2,2); SetCol(M2,0,Q2-P2); SetCol(M2,1,R2-P2); ElMatrix<REAL> M = M2 * gaussj(M1); Pt2dr aTr = P2 - M* P1; aHX = cElComposHomographie(M(0,0),M(1,0),aTr.x); aHY = cElComposHomographie(M(0,1),M(1,1),aTr.y); aHZ = cElComposHomographie(0,0,1); return aHX; } cGenSysSurResol * aSys = aL2 ? ( cGenSysSurResol *) new L2SysSurResol(8) : ( cGenSysSurResol *) new SystLinSurResolu(8,aPack.size()); aSys->SetPhaseEquation(0); for ( anIt=aPack.begin() ; anIt !=aPack.end(); anIt++) { AddCoeffSysHom(aSys,anIt->P1(),anIt->P2(),anIt->Pds(),true ); AddCoeffSysHom(aSys,anIt->P1(),anIt->P2(),anIt->Pds(),false); } Im1D_REAL8 aSol = aSys->GSSR_Solve((bool *)0); REAL * aS = aSol.data(); aHX = cElComposHomographie(aS[0],aS[1],aS[2]); aHY = cElComposHomographie(aS[3],aS[4],aS[5]); aHZ = cElComposHomographie(aS[6],aS[7], 1 ); delete aSys; return aHX; }
vector<double> Vignette_Solve(PtsHom aPtsHomol) { double distMax=sqrt(pow(float(aPtsHomol.SZ.x)/2,2)+pow(float(aPtsHomol.SZ.y)/2,2)); /*/Least Square // Create L2SysSurResol to solve least square equation with 3 unknown L2SysSurResol aSys(3); int nbPtsSIFT=aPtsHomol.size(); //For Each SIFT point for(int i=0;i<int(nbPtsSIFT);i++){ double aPds[3]={(aPtsHomol.Gr2[i]*pow(aPtsHomol.Dist2[i],2)-aPtsHomol.Gr1[i]*pow(aPtsHomol.Dist1[i],2)), (aPtsHomol.Gr2[i]*pow(aPtsHomol.Dist2[i],4)-aPtsHomol.Gr1[i]*pow(aPtsHomol.Dist1[i],4)), (aPtsHomol.Gr2[i]*pow(aPtsHomol.Dist2[i],6)-aPtsHomol.Gr1[i]*pow(aPtsHomol.Dist1[i],6)) }; double poids=1;//sqrt(max(aPtsHomol[1][i],aPtsHomol[0][i]));//sqrt(fabs(aPtsHomol[1][i]-aPtsHomol[0][i])); aSys.AddEquation(poids,aPds,aPtsHomol.Gr1[i]-aPtsHomol.Gr2[i]);//fabs(aPtsHomol[1][i]-aPtsHomol[0][i]) } //System has 3 unknowns and nbPtsSIFT equations (significantly more than enough) bool Ok; Im1D_REAL8 aSol = aSys.GSSR_Solve(&Ok); vector<double> aParam; if (Ok) { double* aData = aSol.data(); aParam.push_back(aData[0]); aParam.push_back(aData[1]); aParam.push_back(aData[2]); } //Erreur moyenne vector<double> erreur; for(int i=0;i<int(aPtsHomol[0].size());i++){ double aComputedVal=aData[0]*(aPtsHomol[3][i]*pow(aPtsHomol[1][i],2)-aPtsHomol[2][i]*pow(aPtsHomol[0][i],2)) +aData[1]*(aPtsHomol[3][i]*pow(aPtsHomol[1][i],4)-aPtsHomol[2][i]*pow(aPtsHomol[0][i],4)) +aData[2]*(aPtsHomol[3][i]*pow(aPtsHomol[1][i],6)-aPtsHomol[2][i]*pow(aPtsHomol[0][i],6)); double aInputVal=aPtsHomol[2][i]-aPtsHomol[3][i]; erreur.push_back(fabs(aComputedVal-aInputVal)*(min(aPtsHomol[0][i],aPtsHomol[1][i]))/(distMax)); } double sum = std::accumulate(erreur.begin(),erreur.end(),0.0); double ErMoy=sum/erreur.size(); cout<<"Mean error = "<<ErMoy<<endl; //End Least Square */ //RANSAC vector<double> aParam; double nbInliersMax=0,aScoreMax=0; //double ErMin = 10; int nbPtsSIFT=aPtsHomol.size(); int nbRANSACinitialised=0; int nbRANSACaccepted=0; int nbRANSACmax=10000; srand(time(NULL));//Initiate the rand value while(nbRANSACinitialised<nbRANSACmax || nbRANSACaccepted<500) { nbRANSACinitialised++; if(nbRANSACinitialised % 500==0 && nbRANSACinitialised<=nbRANSACmax){cout<<"RANSAC progress : "<<nbRANSACinitialised/100<<" %"<<endl;} L2SysSurResol aSys(3); //For 6-24 SIFT points for(int k=0;int(k)<3*((rand() % 8)+3);k++){ int i=rand() % nbPtsSIFT;//Rand choice of a point double aPds[3]={(aPtsHomol.Gr2[i]*pow(aPtsHomol.Dist2[i],2)-aPtsHomol.Gr1[i]*pow(aPtsHomol.Dist1[i],2)), (aPtsHomol.Gr2[i]*pow(aPtsHomol.Dist2[i],4)-aPtsHomol.Gr1[i]*pow(aPtsHomol.Dist1[i],4)), (aPtsHomol.Gr2[i]*pow(aPtsHomol.Dist2[i],6)-aPtsHomol.Gr1[i]*pow(aPtsHomol.Dist1[i],6)) }; double poids=1;//sqrt(max(aPtsHomol[1][i],aPtsHomol[0][i]));//sqrt(fabs(aPtsHomol[1][i]-aPtsHomol[0][i])); aSys.AddEquation(poids,aPds,aPtsHomol.Gr1[i]-aPtsHomol.Gr2[i]);//fabs(aPtsHomol[1][i]-aPtsHomol[0][i]) } //Computing the result bool Ok; Im1D_REAL8 aSol = aSys.GSSR_Solve(&Ok); double* aData = aSol.data(); //Filter if computed vignette is <0 in the corners and if param 1<0 (not a possible vignette) double valCoin=(1+aData[0]*pow(distMax,2)+aData[1]*pow(distMax,4)+aData[2]*pow(distMax,6)); if (Ok && aData[0]>0 && 1<=valCoin){ nbRANSACaccepted++; if (nbRANSACaccepted % 50==0 && nbRANSACinitialised>nbRANSACmax){cout<<"Difficult config RANSAC progress : "<<nbRANSACaccepted/5<<"%"<<endl;} //For Each SIFT point, test if in acceptable error field->compute score double nbInliers=0,aScore; vector<double> erreur; //Computing the distance from computed surface and data points for(int i=0;i<int(nbPtsSIFT);i++){ double aComputedVal=aData[0]*(aPtsHomol.Gr2[i]*pow(aPtsHomol.Dist2[i],2)-aPtsHomol.Gr1[i]*pow(aPtsHomol.Dist1[i],2)) +aData[1]*(aPtsHomol.Gr2[i]*pow(aPtsHomol.Dist2[i],4)-aPtsHomol.Gr1[i]*pow(aPtsHomol.Dist1[i],4)) +aData[2]*(aPtsHomol.Gr2[i]*pow(aPtsHomol.Dist2[i],6)-aPtsHomol.Gr1[i]*pow(aPtsHomol.Dist1[i],6)); double aInputVal=aPtsHomol.Gr1[i]-aPtsHomol.Gr2[i]; erreur.push_back(fabs(aComputedVal-aInputVal)*(min(aPtsHomol.Dist1[i],aPtsHomol.Dist2[i]))/(distMax)); //Selecting inliers if(fabs(aComputedVal-aInputVal)<5){ nbInliers++; } } double sum = std::accumulate(erreur.begin(),erreur.end(),0.0); double ErMoy=sum/erreur.size(); aScore=(nbInliers/nbPtsSIFT)/ErMoy; //if(nbInliers/nbPtsSIFT>0.20 && aScoreMax<aScore){ //if(nbInliers>nbInliersMax){ if(aScore>aScoreMax){ nbInliersMax=nbInliers; //ErMin=ErMoy; aScoreMax=aScore; cout<<"New Best Score (at "<<nbRANSACinitialised<<"th iteration) is : "<<aScoreMax<< " with " <<nbInliersMax/nbPtsSIFT*100<<"% of points used and Mean Error="<<ErMoy<< " and correction factor in corners = "<< valCoin << endl; aParam.clear(); aParam.push_back(aData[0]); aParam.push_back(aData[1]); aParam.push_back(aData[2]); } } } std::cout << "RANSAC score is : "<<aScoreMax<<endl; //end RANSAC if(aParam.size()==3){ std::cout << "Vignette parameters, with x dist from image center : (" << aParam[0] << ")*x^2+(" << aParam[1] << ")*x^4+(" << aParam[2] << ")*x^6"<<endl;} return aParam; }
Im1D_REAL8 cGenSysSurResol::GSSR_Solve(bool * aResOk) { Im1D_REAL8 aSol = V_GSSR_Solve(aResOk); if (true && (NbVar() >8)) { /* ElMatrix<double> aM2(NbVar(),NbVar()); ElMatrix<double> aL2(1,NbVar()); for (int aJ=0; aJ< NbVar() ; aJ++) { aL2(0,aJ) = GetElemLin(aJ); for (int aI=0; aI< NbVar() ; aI++) aM2(aI,aJ) = GetElemQuad(aI,aJ); } ElMatrix<double> aS2 = gaussj(aM2) * aL2; std::cout << "NBV " << NbVar() << "NB CONTRAINTE " << mNbContrainte << " Assumed : " << ContraintesAssumed() << "\n"; for (int aK=0 ; aK<NbVar() ; aK++) std::cout << "*************jjkk--- " << aK << " " << aSol.data()[aK] << " " << aS2(0,aK) << " M2 " << aM2(aK,aK) << "\n"; getchar(); */ if (0) { for (int aJ=0; aJ< NbVar() ; aJ++) { double aS0=0; for (int aK=0; aK< NbVar() ; aK++) aS0 += aSol.data()[aK] * GetElemQuad(aJ,aK); float aV = (float)GetElemLin(aJ); printf("%d %f %f %f :: ",aJ,aSol.data()[aJ],aV,aS0); for (int aK=0 ; aK< NbVar() ; aK++) { float aV = (float)GetElemQuad(aJ,aK); printf("%f ",aV); } printf("\n"); } } } if ((mNbContrainte==0) || mCstrAssumed) return aSol; for (INT y=0 ; y<NbVar() ; y++) mSol(0,y) = aSol.data()[y]; mSol += mE; mCSol.mul(mC,mSol); for (INT y=0 ; y<NbVar() ; y++) { aSol.data()[y] = mCSol(0,y); } return aSol; }