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