cNewO_CombineCple::cNewO_CombineCple(const cStructMergeTieP< cFixedSizeMergeTieP<2,Pt2dr> > & aMap,ElRotation3D * aTestSol) : mCurStep (1<<NbPow2), mNbStepTeta (4 * NbDecoup0PIS2 * mCurStep), mCurRot (3,3), mW (0) { // REDONDANT AVEC FONCTION GLOBALES FAITE APRES .... PackReduit /******************************************************/ /* */ /* A- Selection des sommets */ /* */ /******************************************************/ //------------------------------------------------------------------------ // A- 1- Preselrection purement aleatoire d'un nombre raisonnable depoints //------------------------------------------------------------------------ const std::list<tMerge *> & aLM = aMap.ListMerged(); RMat_Inertie aMat; { cRandNParmiQ aSelec(NbMaxInit, (int)aLM.size()); for (std::list<tMerge *>::const_iterator itM=aLM.begin() ; itM!=aLM.end() ; itM++) { if (aSelec.GetNext()) { mVAllCdt.push_back(cCdtCombTiep(*itM)); Pt2dr aP1 = (*itM)->GetVal(0); aMat.add_pt_en_place(aP1.x,aP1.y); } } } aMat = aMat.normalize(); int aNbSomTot = int(mVAllCdt.size()); double aSurfType = sqrt (aMat.s11()* aMat.s22() - ElSquare(aMat.s12())); double aDistType = sqrt(aSurfType/aNbSomTot); double aSzW = 800; if (1) { mP0W = aMap.ValInf(0); Pt2dr aP1 = aMap.ValSup(0); Pt2dr aSz = aP1-mP0W; mP0W = mP0W - aSz * 0.1; aP1 = aP1 + aSz * 0.1; aSz = aP1-mP0W; mScaleW = aSzW /ElMax(aSz.x,aSz.y) ; mW = Video_Win::PtrWStd(round_ni(aSz*mScaleW)); } //------------------------------------------------------------------------ // A-2 Calcul d'une fonction de deponderation //------------------------------------------------------------------------ for (int aKS1 = 0 ; aKS1 <aNbSomTot ; aKS1++) { for (int aKS2 = aKS1 ; aKS2 <aNbSomTot ; aKS2++) { // sqrt pour attenuer la ponderation double aDist = sqrt(dist48( mVAllCdt[aKS1].mP1-mVAllCdt[aKS2].mP1) / 2.0); // aDist=1; // double aDist = (dist48( mVAllCdt[aKS1].mP1-mVAllCdt[aKS2].mP1) / 2.0); double aPds = 1 / (aDistType+aDist); mVAllCdt[aKS1].mPdsOccup += aPds; mVAllCdt[aKS2].mPdsOccup += aPds; } if (mW) mW->draw_circle_abs(ToW( mVAllCdt[aKS1].mP1),2.0,mW->pdisc()(P8COL::blue)); } for (int aKSom = 0 ; aKSom <aNbSomTot ; aKSom++) { cCdtCombTiep & aCdt = mVAllCdt[aKSom]; aCdt.mPdsOccup *= ElSquare(aCdt.mMerge->NbArc()); } int aNbSomSel = ElMin(aNbSomTot,NbTieP); //------------------------------------------------------------------------ // A-3 Calcul de aNbSomSel points biens repartis //------------------------------------------------------------------------ ElTimer aChrono; for (int aKSel=0 ; aKSel<aNbSomSel ; aKSel++) { // Recherche du cdt le plus loin double aMaxDMin = 0; cCdtCombTiep * aBest = 0; for (int aKSom = 0 ; aKSom <aNbSomTot ; aKSom++) { cCdtCombTiep & aCdt = mVAllCdt[aKSom]; double aDist = aCdt.mDMin * aCdt.mPdsOccup; if ((!aCdt.mTaken) && (aDist > aMaxDMin)) { aMaxDMin = aDist; aBest = & aCdt; } } ELISE_ASSERT(aBest!=0,"cNewO_CombineCple"); for (int aKSom = 0 ; aKSom <aNbSomTot ; aKSom++) { cCdtCombTiep & aCdt = mVAllCdt[aKSom]; aCdt.mDMin = ElMin(aCdt.mDMin,dist48(aCdt.mP1-aBest->mP1)); } aBest->mQ1 = vunit(Pt3dr(aBest->mP1.x,aBest->mP1.y,1.0)); Pt2dr aP2 = aBest->mMerge->GetVal(1); aBest->mQ2Init = vunit(Pt3dr(aP2.x,aP2.y,1.0)); mVCdtSel.push_back(aBest); if (mW) mW->draw_circle_abs(ToW( aBest->mP1),3.0,mW->pdisc()(P8COL::red)); } /******************************************************/ /* */ /* B- Calcul des arcs */ /* */ /******************************************************/ // B-1 Au max le nombre d'arc possible int aNbA = NbCple; while (aNbA > ((aNbSomSel * (aNbSomSel-1)) /2)) aNbA--; int aNbIter = (aNbA-1) / aNbSomSel + 1; cRandNParmiQ aSelec(aNbA- (aNbIter-1) * aNbSomSel,aNbSomSel); int aNbAMaj = aNbIter * aNbSomSel; std::vector<int> aPermut = RandPermut(aNbA); // B-2 Recherche des arsc int aKA=0; for (int aCptAMaj = 0 ; aCptAMaj < aNbAMaj ; aCptAMaj++) { // Tous les sommets sont equi repartis, sauf a la fin on choisit a hasard bool aSelK = true; if ( (aCptAMaj/aNbSomSel)== (aNbIter-1)) // Si derniere iter, test special { aSelK = aSelec.GetNext(); } if (aSelK) { int aKP1 = (aCptAMaj%aNbSomSel); double aTeta = (aPermut[aKA] * 2 * PI) / aNbA; Pt2dr aDir = Pt2dr::FromPolar(1.0,aTeta); // std::cout << "teta " << aTeta << "\n"; double aBestSc=-1.0; int aBestK=-1; for (int aKP2 = 0 ; aKP2 < aNbSomSel ; aKP2++) { if (aKP2!=aKP1) { Pt2dr aV = (mVCdtSel[aKP2]->mP1- mVCdtSel[aKP1]->mP1) / aDir; Pt2dr aU = vunit(aV); // Favorise les llongs arc et homogeneise les directions double aSc = NRrandom3() * euclid(aV) * (1/(1+ElSquare(5.0*aU.y))); if ((aSc>aBestSc) && (aKP2!=aKP1)) { aBestSc= aSc; aBestK = aKP2; } } } ELISE_ASSERT((aBestK>=0),"No Best Arc"); mLArcs.push_back(Pt2di(aKP1,aBestK)); if (mW) { mW->draw_seg(ToW( mVCdtSel[aKP1]->mP1),ToW( mVCdtSel[aBestK]->mP1),mW->pdisc()(P8COL::green)); } aKA++; } } /******************************************************/ /* */ /* */ /* */ /******************************************************/ if (mW) mW->clik_in(); if (aTestSol) { ElRotation3D aR = * aTestSol; // Le sens corret a ete retabli (j'espere !!) // SetCurRot(aR.Mat()); // std::cout << "Test Externe : " << CalculCostCur() <<"\n"; // aR = aR.inv(); SetCurRot(aR.Mat()); std::cout << "Test Externe I : " << CalculCostCur() <<"\n"; std::cout << "CostBase " << CostOneBase(aR.tr()) << "\n"; // ElRotation3D * } std::cout << "cNewO_CombineCple::cNewO_CombineCple " << aNbSomTot << "\n"; Pt3di aP; std::list<Pt3di> aLPMin; double aCostMin = 1e10; Pt3di aPMin(1000,1000,1000); for (aP.x = -NbDecoup0PIS2 ; aP.x <= NbDecoup0PIS2 ; aP.x ++) { std::cout << "DECx " << aP.x << "\n"; for (aP.y = -NbDecoup0PIS2 ; aP.y <= NbDecoup0PIS2 ; aP.y ++) { for (aP.z = - (2*NbDecoup0PIS2) ; aP.z < (2*NbDecoup0PIS2) ; aP.z ++) { double aVC = GetCost(aP*mCurStep); bool IsMinLoc = (aVC < GetCost((aP+Pt3di( 1,0,0)) * mCurStep)) && (aVC < GetCost((aP+Pt3di(-1,0,0)) * mCurStep)) && (aVC < GetCost((aP+Pt3di(0, 1,0)) * mCurStep)) && (aVC < GetCost((aP+Pt3di(0,-1,0)) * mCurStep)) && (aVC < GetCost((aP+Pt3di(0,0, 1)) * mCurStep)) && (aVC < GetCost((aP+Pt3di(0,0,-1)) * mCurStep)); int aDelta = 2; for (int aDx=-aDelta ; (aDx<=aDelta) && IsMinLoc ; aDx++) { for (int aDy=-aDelta ; (aDy<=aDelta) && IsMinLoc ; aDy++) { for (int aDz=-aDelta ; (aDz<=aDelta) && IsMinLoc ; aDz++) { if ((aDx!=0) || (aDy!=0) || (aDz!=0)) { IsMinLoc = IsMinLoc && (aVC<GetCost( (aP+Pt3di(aDx,aDy,aDz))*mCurStep)); } } } } if (IsMinLoc) { std::cout << " IisssMinn " << aP << " " << aVC << "\n"; aLPMin.push_back(aP*mCurStep); } if (aVC<aCostMin) { aPMin = aP*mCurStep; aCostMin = aVC; } } } } std::cout << "COST " << aCostMin << " PMIN " << PInt2Tetas(aPMin ) << " NbMinLoc " << aLPMin.size() << "\n"; Pt3dr aTeta = PInt2Tetas(aPMin); ElMatrix<double> aR = ElMatrix<double>::Rotation(aTeta.z,aTeta.y,aTeta.x); for (int aY=0 ; aY<3 ; aY++) { for (int aX=0 ; aX<3 ; aX++) { std::cout << aR(aX,aY) << " "; } std::cout << "\n"; } /* std::cout << "Sssz " << aLPMin.size() << "\n"; if ( aLPMin.size()>0) std::cout << "PP00 " << *(aLPMin.begin()) << "\n"; */ }
cElHomographie cElHomographie::RobustInit(double & aDMIn,double * aQuality,const ElPackHomologue & aPack,bool & Ok ,int aNbTestEstim, double aPerc,int aNbMaxPts) { cElHomographie aRes = cElHomographie::Id(); Ok = false; Pt2dr aCdg(0,0); for (ElPackHomologue::tCstIter itH=aPack.begin() ; itH!=aPack.end() ; itH++) { aCdg = aCdg + itH->P1(); } aCdg = aCdg / double(aPack.size()); std::vector<std::pair<Pt2dr,Pt2dr> > aV00; std::vector<std::pair<Pt2dr,Pt2dr> > aV01; std::vector<std::pair<Pt2dr,Pt2dr> > aV10; std::vector<std::pair<Pt2dr,Pt2dr> > aV11; std::vector<std::pair<Pt2dr,Pt2dr> > aVAll; int aNbPtsTot = aPack.size(); int aCpt = 0; for (ElPackHomologue::tCstIter itH=aPack.begin() ; itH!=aPack.end() ; itH++) { Pt2dr aP1 = itH->P1(); Pt2dr aP2 = itH->P2(); std::pair<Pt2dr,Pt2dr> aPair(aP1,aP2); if ( (((aCpt-1)*aNbMaxPts)/aNbPtsTot) != ((aCpt*aNbMaxPts)/aNbPtsTot)) { aVAll.push_back(aPair); } if (aP1.x < aCdg.x) { if (aP1.y < aCdg.y) aV00.push_back(aPair); else aV01.push_back(aPair); } else { if (aP1.y < aCdg.y) aV10.push_back(aPair); else aV11.push_back(aPair); } aCpt++; } if (aV00.empty() || aV01.empty() || aV10.empty() || aV11.empty() ) return aRes; aDMIn = 1e30; int aNbPts = aVAll.size(); int aNbKth = ElMax(1,ElMin(aNbPts-1,round_ni((aPerc/100.0) * aNbPts))); std::vector<double> aVDist; if (aNbMaxPts<aNbPtsTot) aNbTestEstim = (aNbTestEstim*aNbPtsTot) / aNbMaxPts; // int aKMIN = -1; std::vector<double> aVD; // For tuning and show in if(0) ... while (aNbTestEstim) { int aK00 = NRrandom3(aV00.size()); int aK01 = NRrandom3(aV01.size()); int aK10 = NRrandom3(aV10.size()); int aK11 = NRrandom3(aV11.size()); ElPackHomologue aP4; AddPair(aP4,aV00[aK00]); AddPair(aP4,aV01[aK01]); AddPair(aP4,aV10[aK10]); AddPair(aP4,aV11[aK11]); cElHomographie aSol = cElHomographie(aP4,true); aVDist.clear(); for (int aK=0 ; aK< aNbPts ; aK++) { Pt2dr aP1 = aVAll[aK].first; Pt2dr aP2 = aVAll[aK].second; /* Pt2dr aDif = aP2 -aSol.Direct(aP1); double aDx = ElAbs(aDif.x); double aDy = ElAbs(aDif.y); double aDist = (aDx+aDy + ElMax(aDx,aDy))/ 2.0; */ double aDist = QuickDist(aP2 -aSol.Direct(aP1)); aVDist.push_back(aDist); } double aSom = MoyKPPVal(aVDist,aNbKth); aVD.push_back(aSom); //std::cout << "Robust:Hom:SOM = " << aDMIn << " " << aSom << "\n"; if (aSom <aDMIn) { aRes = aSol; aDMIn = aSom; } aNbTestEstim--; } // double aDMinInit = aDMIn; ElPackHomologue aPckPds; for (int anIterL2 = 0 ; anIterL2 < 4 ; anIterL2++) { aPckPds = ElPackHomologue(); aVDist.clear(); int aCpt = 0; for (ElPackHomologue::tCstIter itH=aPack.begin() ; itH!=aPack.end() ; itH++) { Pt2dr aP1 = itH->P1(); Pt2dr aP2 = itH->P2(); double aDist = QuickDist(aP2 -aRes.Direct(aP1)); aVDist.push_back(aDist); double aPds = 1/ (1+ 4*ElSquare(aDist/aDMIn)); aPckPds.Cple_Add(ElCplePtsHomologues(aP1,aP2,aPds)); aCpt++; } ELISE_ASSERT(aNbPtsTot==aPack.size() ,"KKKKK ????"); int aKTh = round_ni(aNbPtsTot * (aPerc/100.0)); ELISE_ASSERT(int(aVDist.size())==aNbPtsTot,"Compat MoyKPPVal/SplitArrounKthValue"); aDMIn = MoyKPPVal(aVDist,aKTh); aRes = cElHomographie(aPckPds,true); } if (aQuality) { std::vector<double> aVEstim; int aNbTestValid = 71; for (int aKTest = 0 ; aKTest <aNbTestValid ; aKTest++) { ElPackHomologue aPckPdsA; ElPackHomologue aPckPdsB; cRandNParmiQ aSelec(aNbPtsTot/2,aNbPtsTot); for (ElPackHomologue::tCstIter itH=aPack.begin() ; itH!=aPack.end() ; itH++) { Pt2dr aP1 = itH->P1(); Pt2dr aP2 = itH->P2(); double aDist = QuickDist(aP2 -aRes.Direct(aP1)); aVDist.push_back(aDist); double aPds = 1/ sqrt(1+ ElSquare(aDist/aDMIn)); // if (NRrandom3() > 0.5) if (aSelec.GetNext()) aPckPdsA.Cple_Add(ElCplePtsHomologues(aP1,aP2,aPds)); else aPckPdsB.Cple_Add(ElCplePtsHomologues(aP1,aP2,aPds)); } cElHomographie aResA = cElHomographie(aPckPdsA,true); cElHomographie aResB = cElHomographie(aPckPdsB,true); double aSomDist = 0; for (ElPackHomologue::tCstIter itH=aPack.begin() ; itH!=aPack.end() ; itH++) { Pt2dr aP1 = itH->P1(); Pt2dr aQ = aRes.Direct(aP1); Pt2dr aQA = aResA.Direct(aP1); Pt2dr aQB = aResB.Direct(aP1); double aDist = (QuickDist(aQ-aQA) + QuickDist(aQ-aQB) + QuickDist(aQB-aQA)) / 3.0; aSomDist += aDist; } aSomDist /= aNbPtsTot; aVEstim.push_back(aSomDist); } *aQuality = MedianeSup(aVEstim); } Ok= true; return aRes; }