void bench_seg_mean_square()
{
for (int i = 0; i<100 ; i++)
{
INT nx = (INT) (10 +NRrandom3()*20);
INT ny = nx -5;
Pt2dr tr = Pt2dr((NRrandom3()-0.5)*1e4,(NRrandom3()-0.5)*1e4);
Pt2dr rot = Pt2dr::FromPolar(1.0,NRrandom3() *100);
RMat_Inertie m;
for (int x= -nx; x <= nx ; x++)
for (int y= -ny; y <= ny ; y++)
{
Pt2dr Z = tr+rot*Pt2dr(x,y);
m.add_pt_en_place(Z.x,Z.y);
}
Seg2d s = seg_mean_square(m,100.0);
Pt2dr cdg = s.p0();
Pt2dr all = (s.p1()-s.p0())/ 100.0;
BENCH_ASSERT
(
(euclid(cdg-tr) < BIG_epsilon)
&& (std::abs(all^rot) < BIG_epsilon)
);
// BENCH_ASSERT(Abs(d0 -d1) < BIG_epsilon);
}
}
float SimilByCorrel
(
int aSzW,
const cTD_Im & aIm1,const Pt2di & aP1,
const cTD_Im & aIm2,const Pt2di & aP2
)
{
if (! VignetteInImage(aSzW,aIm1,aP1)) return Beaucoup;
if (! VignetteInImage(aSzW,aIm2,aP2)) return Beaucoup;
RMat_Inertie aMat;
Pt2di aDP;
for (aDP.x= -aSzW ; aDP.x<= aSzW ; aDP.x++)
{
for (aDP.y= -aSzW ; aDP.y<= aSzW ; aDP.y++)
{
float aV1 = aIm1.GetVal(aP1+aDP);
float aV2 = aIm2.GetVal(aP2+aDP);
aMat.add_pt_en_place(aV1,aV2,1.0);
}
}
return 1-aMat.correlation();
}
void bench_dist_InerMat_seg_droite()
{
for (int i = 0; i<100 ; i++)
{
Pt2dr p1 = Pt2dr((NRrandom3()-0.5)*1e4,(NRrandom3()-0.5)*1e4);
Pt2dr p2 = p1;
while (euclid(p1-p2) < 1e2)
p2 = Pt2dr((NRrandom3()-0.5)*1e4,(NRrandom3()-0.5)*1e4);
SegComp s(p1,p2);
int nb = (int)(50 * NRrandom3());
REAL d0 = 0.0;
RMat_Inertie m;
for (int j =0; j<nb ; j++)
{
Pt2dr q = Pt2dr((NRrandom3()-0.5)*1e4,(NRrandom3()-0.5)*1e4);
REAL pds = NRrandom3();
m = m.plus_cple(q.x,q.y,pds);
d0 += pds * s.square_dist_droite(q);
}
REAL d1 = square_dist_droite(s,m);
BENCH_ASSERT(std::abs(d0 -d1) < BIG_epsilon);
}
}
float SimilByCorrel
(
const Pt2di & aP1,
const cTD_OneScale & aIm2,const Pt2di & aP2
) const
{
RMat_Inertie aMat;
FillInertie(aMat,aP1,aIm2,aP2);
return 1- aMat.correlation();
}
void TestNtt(const std::string &aName)
{
Tiff_Im aTF = Tiff_Im::StdConvGen(aName,1,true);
Pt2di aSz = aTF.sz();
Im2D_REAL4 aI0(aSz.x,aSz.y);
ELISE_COPY( aTF.all_pts(),aTF.in(),aI0.out());
int aWSz=2;
TIm2D<REAL4,REAL8> aTIm(aI0);
double aSomGlob=0.0;
double aNbGlob=0.0;
for (int aKdx=-aWSz ; aKdx<=aWSz ; aKdx+=aWSz)
{
printf("## ");
for (int aKdy=-aWSz ; aKdy<=aWSz ; aKdy+=aWSz)
{
int aDx = aKdx;
int aDy = aKdy;
Pt2di aDep(aDx,aDy);
Pt2di aP;
RMat_Inertie aMat;
for (aP.x = aWSz ; aP.x<aSz.x-aWSz ; aP.x++)
{
for (aP.y=aWSz ; aP.y<aSz.y-aWSz ; aP.y++)
{
aMat.add_pt_en_place(aTIm.get(aP),aTIm.get(aP+aDep));
}
}
double aC = aMat.correlation();
aC = 1-aC;
if (dist8(aDep) == aWSz)
{
aSomGlob += aC;
aNbGlob ++;
}
printf(" %4d",round_ni(10000*(aC)));
}
printf("\n");
}
aSomGlob /= aNbGlob;
std::cout << " G:" << aSomGlob << "\n";
printf("\n\n");
}
float SimilByCatCorrel
(
const Pt2di & aP1,
const cTD_PyrMultiScale & aPyr2,
const Pt2di & aP2
) const
{
if ((!InPyram(aP1)) || (!aPyr2.InPyram(aP2)))
return 2 ;
RMat_Inertie aMat;
for (int aK=0 ; aK<mNbIm ; aK++)
{
const cTD_OneScale& aSC1 = mVecIms[aK];
const cTD_OneScale& aSC2 = aPyr2.mVecIms[aK];
aSC1.FillInertie(aMat,aP1,aSC2,aP2);
}
return 1-aMat.correlation();
}
double cMC_PairIm::Correl(Pt2dr aPTER,int aSzW,double * aVPx)
{
RMat_Inertie aMat;
Pt2di aDP;
for (aDP.x=-aSzW ; aDP.x<=aSzW; aDP.x++)
{
for (aDP.y=-aSzW ; aDP.y<=aSzW; aDP.y++)
{
Pt2dr aQTer = aPTER + (Pt2dr(aDP) * double(mDZ*mResolZ1));
Pt2dr aPI1 = mPDV1.Geom().Objet2ImageInit_Euclid(aQTer,aVPx) / double(mDZ);
Pt2dr aPI2 = mPDV2.Geom().Objet2ImageInit_Euclid(aQTer,aVPx) / double(mDZ);
if (mTI1.Rinside_bilin(aPI1) && (mTI2.Rinside_bilin(aPI2)))
{
aMat.add_pt_en_place(mTI1.getr(aPI1),mTI2.getr(aPI2));
}
else
return -1;
}
}
return aMat.correlation();
}
void FillInertie
(
RMat_Inertie &aMat,
const Pt2di & aP1,
const cTD_OneScale & aIm2,const Pt2di & aP2
) const
{
Pt2di aDP;
for (aDP.x= -mSzW ; aDP.x<= mSzW ; aDP.x+=mStepW)
{
for (aDP.y= -mSzW ; aDP.y<= mSzW ; aDP.y+=mStepW)
{
float aV1 = this->mIm.GetVal(aP1+aDP);
float aV2 = aIm2.mIm.GetVal(aP2+aDP);
aMat.add_pt_en_place(aV1,aV2,mPdsByPix);
}
}
}
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";
*/
}
double APP_cout_square_droite(SomApproxPoly * s1,SomApproxPoly * s2,const SegComp& seg,const ArgAPP &)
{
RMat_Inertie m = s1->inert_dif(s2);
double d = square_dist_droite(seg,m);
return sqrt(std::max(0.0,d*m.s()));
}
cTrapuBat cElHJaArrangt::MakeSolTrapu
(
Pt2dr aDec,
eModeEvalZ aModeZ,
INT aValForbid,
Im2D_INT2 aMnt,
bool WithIndet
)
{
cTrapuBat aSol;
for (INT aKPl=0 ; aKPl<mNbPl; aKPl++)
{
std::vector<std::vector<Pt3dr> > aVFaces = mPlans[aKPl]->FacesSols(WithIndet);
for (INT aKF=0 ; aKF<INT(aVFaces.size()) ; aKF++)
{
aSol.AddFace(aVFaces[aKF],ElHJAEpsilon,aKPl);
}
}
mSysResolv = 0;
RMat_Inertie aMat = MInertCurSol();
switch (aModeZ)
{
case eNoCorrecZ : { } break;
case eCorrelEvalZ : { } break;
case eL2EvalZ :
{
mSysResolv = &mL2SysResolv;
}
break;
case eL1EvalZ :
{
mL1SysResolv.SetNbEquation(round_ni(aMat.S0()));
mSysResolv = &mL1SysResolv;
}
break;
};
if (aModeZ != eNoCorrecZ)
ELISE_ASSERT(!WithIndet,"cElHJaArrangt::MakeSolTrapu");
REAL aA,aB;
if (mSysResolv)
{
mSysResolv->GSSR_Reset(false);
for (tItFac itF=mFacettes.begin() ; itF!=mFacettes.end() ; itF++)
if ((*itF)->IsSure())
(*itF)->MakeInertie(aMnt,aValForbid,mSysResolv);
mSysResolv->GSSR_SolveEqFitDroite(aA,aB);
}
std::vector<Pt3dr> & aVSoms = aSol.Soms();
aVSoms.push_back(aSol.P0());
aVSoms.push_back(aSol.P1());
for (INT aK=0 ; aK<INT(aVSoms.size()) ; aK++)
{
if (aModeZ==eCorrelEvalZ)
aVSoms[aK].z = aMat.V2toV1(aVSoms[aK].z);
else if (mSysResolv)
{
aVSoms[aK].z = aB + aA *aVSoms[aK].z;
}
aVSoms[aK].x += aDec.x;
aVSoms[aK].y += aDec.y;
}
aSol.P1() = aVSoms.back(); aVSoms.pop_back();
aSol.P0() = aVSoms.back(); aVSoms.pop_back();
return aSol;
}
bool cOneTestLSQ::OneLSQItere()
{
if (!OK(Pt2dr(0,0))) return false;
ClearStat();
int aCpt=0;
RMat_Inertie aMat;
double aDif = 0;
for(int aKx=-mNbW; aKx<=mNbW; aKx++)
{
for(int aKy=-mNbW; aKy<=mNbW; aKy++)
{
Pt2dr aP2 = mIm2->PVois(aKx,aKy);
Pt3dr aGV2 = mIm2->GetValDer(aP2);
double aV1 = mValsIm1[aCpt];
// double aV1 = mIm1.GetVal(mIm1.PVois(aKx,aKy));
double aV2 = aGV2.z;
double aGx = aGV2.x;
double aGy = aGV2.y;
aDif += ElSquare(aV1-aV2);
aMat.add_pt_en_place(aV1,aV2);
// Pour verifier la justesse des moindres carres
if (0)
{
aV2 = 50 + 2 * aV1 - 0.5* aGx -0.25 * aGy;
}
mCov[IndK0][IndK0] += 1;
mCov[IndK0][IndK1] += aV1;
mCov[IndK0][IndKx] -= aGx;
mCov[IndK0][IndKy] -= aGy;
mCov[IndK1][IndK1] += aV1*aV1;
mCov[IndK1][IndKx] -= aV1*aGx;
mCov[IndK1][IndKy] -= aV1*aGy;
mCov[IndKx][IndKx] += aGx*aGx;
mCov[IndKx][IndKy] += aGx*aGy;
mCov[IndKy][IndKy] += aGy*aGy;
mSomI2[IndK0] += aV2;
mSomI2[IndK1] += aV1 * aV2;
mSomI2[IndKx] -= aV2 * aGx;
mSomI2[IndKy] -= aV2 * aGy;
aCpt++;
}
}
for(int aK1=0; aK1<NbInc; aK1++)
{
mMatI2(0,aK1) = mSomI2[aK1];
for(int aK2=0; aK2<=aK1; aK2++)
{
mMatCov(aK1,aK2) = mMatCov(aK2,aK1) = mCov[aK2][aK1];
}
}
jacobi_diag(mMatCov,mValP,mVecP);
double aVPMin = 1e10;
for(int aK1=0; aK1<NbInc; aK1++)
aVPMin = std::min(aVPMin,mValP(aK1,aK1));
if (aVPMin<1e-8) return false;
mSolLSQ = gaussj(mMatCov) * mMatI2;
mCorel0LSQ = aMat.correlation();
mDepLSQ = Pt2dr(mSolLSQ(0,2), mSolLSQ(0,3));
mDeps.push_back(mDepLSQ);
return true;
}
