void OneTestNewMerge()
{
static int aCpt=0;
for (int aNb = 2 ; aNb < 500 ; aNb += 3)
{
// std::cout << "================ " << aNb << " =============================\n";
bool MemoArc = true;
aCpt++;
double aProbaPInt = NRrandom3();
double aProbaArcGlob = NRrandom3();
cStructMergeTieP<cVarSizeMergeTieP<Pt2df> > aVSMT(NbCamTest,MemoArc);
cStructMergeTieP<cFixedSizeMergeTieP<NbCamTest,Pt2df> > aFSMT(NbCamTest,MemoArc);
tGraphTestTieP aGr;
int aFlagOr = aGr.alloc_flag_arc();
tSubGraphTestTiepOr aSubOr(-1,aFlagOr);
std::map<Pt2df,tSomTestTieP *> aMapS;
for (int aKP=0 ; aKP<aNb ; aKP++)
{
double aProbaArc = aProbaArcGlob * NRrandom3();
std::vector<Pt2df> aVP;
std::vector<tSomTestTieP *> aVS;
for (int aKC = 0 ; aKC<NbCamTest ; aKC++)
{
Pt2df aP;
if (NRrandom3() < aProbaPInt)
{
aP = Pt2df(aKC,NRrandom3(aNb*NbCamTest));
}
else
{
aP = Pt2df(aKC,aKP) ; // Chaque point est different
}
aVP.push_back(aP);
if (aMapS[aP] == 0)
aMapS[aP] = &aGr.new_som(aP);
aVS.push_back(aMapS[aP]);
}
for (int aKC1=0 ; aKC1<NbCamTest ; aKC1++)
{
for (int aKC2=0 ; aKC2<NbCamTest ; aKC2++)
{
if ((NRrandom3() < aProbaArc) && (aKC1!=aKC2))
{
aVSMT.AddArc(aVP[aKC1],aKC1,aVP[aKC2],aKC2);
aFSMT.AddArc(aVP[aKC1],aKC1,aVP[aKC2],aKC2);
tSomTestTieP * aS1 = aVS[aKC1];
tSomTestTieP * aS2 = aVS[aKC2];
tArcTestTieP * anArc = aGr.arc_s1s2(*aS1,*aS2);
if (anArc==0)
anArc = &aGr.add_arc(*aS1,*aS2,0);
anArc->flag_set_kth_true(aFlagOr);
anArc->attr()++;
}
}
}
}
tSubGraphTestTieP aSub;
ElPartition<tSomTestTieP * > aPart;
PartitionCC(aPart,aGr,aSub);
int aNbCCOk=0;
int aNbArcOK=0;
cChkMerge aChkGr;
for (int aKSet=0 ; aKSet <aPart.nb() ; aKSet++)
{
ElSubFilo<tSomTestTieP *> aSet = aPart[aKSet];
if (aSet.size() != 1)
{
bool Ok=true;
std::vector<int> aCpt(NbCamTest,0);
std::vector<Pt2df> aVPts(NbCamTest,Pt2df(0,0));
int aNbArc = 0;
for (int aKSom=0 ; aKSom<aSet.size() ; aKSom++)
{
Pt2df aP = aSet[aKSom]->attr();
int aKC = round_ni(aP.x);
aVPts[aKC] = aP;
if (aCpt[aKC])
Ok= false;
aCpt[aKC]++;
for (tArtIterTestTieP itA= aSet[aKSom]->begin(aSub) ; itA.go_on() ; itA++)
aNbArc += (*itA).attr();
}
if (Ok)
{
aNbArcOK += aNbArc;
aNbCCOk++;
aChkGr.AddNbArc(aNbArc);
for (int aKC=0 ; aKC<NbCamTest ; aKC++)
{
if (aCpt[aKC])
aChkGr.AddSomIn(aKC,aVPts[aKC]);
else
aChkGr.AddSomAbs(aKC);
}
}
}
}
aVSMT.DoExport();
aFSMT.DoExport();
const std::list<cVarSizeMergeTieP<Pt2df> *> & aLVT = aVSMT.ListMerged();
const std::list<cFixedSizeMergeTieP<NbCamTest,Pt2df> *> & aLFT = aFSMT.ListMerged();
ELISE_ASSERT(int(aLVT.size())==aNbCCOk,"Tie Point CC Check");
int aNbA1,aNbA2;
double aChk1 = ChkSomMerge(aLVT,aNbA1,MemoArc);
double aChk2 = ChkSomMerge(aLFT,aNbA2,MemoArc);
// std::cout << aCpt << " ============== " << aLVT.size() << " " << aNbCCOk << " " << aNbArcOK << " " << " " << aNbA1 << "\n";
if (ElAbs(aChk1-aChk2)> 1E-5)
{
std::cout << "HHHHH " << aChk1 << " " << aChk2 << " " << "\n";
ELISE_ASSERT(false,"Chk TieP");
}
if (ElAbs(aChk1-aChkGr.mRes)> 1e-5)
{
std::cout << "UUuuUUUi " << aChk1 << " " << aChkGr.mRes << " " << "\n";
ELISE_ASSERT(false,"Chk TieP");
}
if (aNbArcOK != aNbA1)
{
std::cout << " NB ARC : " << aNbArcOK << " " << aNbA1 << "\n";
ELISE_ASSERT(aNbArcOK==aNbA1,"Tie Point CC Check");
}
// std::cout << "HHHHH " << ChkSomMerge(aLVT) << " " << ChkSomMerge(aLFT) << " " << ChkSomMerge(aLF) << " " << "\n";
aVSMT.Delete();
aFSMT.Delete();
}
}
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;
}
