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);
}
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;
}
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];
}
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_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];
}
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 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 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()
);
}
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 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 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 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());
}
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_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;
}
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;
}
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;
}
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;
}
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;
}
