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