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