void ImRON2ParmEllipse
(
REAL & A,
REAL & B,
REAL & C,
const Pt2dr & aV0,
const Pt2dr & aV1
)
{
ElMatrix<REAL> aMat(2,2);
SetCol(aMat,0,aV0);
SetCol(aMat,1,aV1);
aMat = gaussj(aMat);
aMat = aMat.transpose() * aMat;
ElMatrix<REAL> aVecP(2,2);
ElMatrix<REAL> aValP(2,2);
jacobi_diag(aMat,aValP,aVecP);
aValP(0,0) = sqrt(aValP(0,0));
aValP(1,1) = sqrt(aValP(1,1));
ElMatrix<REAL> aABC = aVecP * aValP * aVecP.transpose();
A = aABC(0,0);
B = aABC(1,0);
C = aABC(1,1);
}
bool EllipseEq2ParamPhys
(
REAL & V1,
REAL & V2,
REAL & teta,
REAL A,
REAL B,
REAL C
)
{
ElMatrix<REAL> aMat(2,2);
aMat(0,0) = A;
aMat(1,0) = B;
aMat(0,1) = B;
aMat(1,1) = C;
ElMatrix<REAL> aVecP(2,2);
ElMatrix<REAL> aValP(2,2);
jacobi_diag(aMat,aValP,aVecP);
if ((aValP(0,0) <=0) || (aValP(1,1) <= 0))
return false;
V1 = 1 / aValP(0,0);
V2 = 1 / aValP(1,1);
teta = angle(Pt2dr(aVecP(0,0),aVecP(0,1)));
if (V1 < V2)
{
ElSwap(V1,V2);
teta += PI/2;
if (teta> PI)
teta -= PI;
}
return true;
}
void cIncEnsembleCamera::OptimJacobi
(
std::list<cIncSetLiaison *> * aListSL,
const std::vector<cFonctrPond> & aFoncAux
)
{
mListSl = aListSL;
mFoncsAux = aFoncAux;
SetOpt();
ELISE_ASSERT(mL2Opt,"Need L2 Sys for OptimJacobi");
mSysL2->GetMatr(mMatrL2,mMatrtB);
jacobi_diag(mMatrL2,mMatrValP,mMatrVecP);
mtBVecP = mMatrtB * mMatrVecP;
cElRanGen aR;
Im1D_REAL8 P0 = CurVals();
Im1D_REAL8 mImBest = CurVals();
REAL aScInit = ScoreCur(false);
REAL aScMin = aScInit;
REAL aScAmMin = aScInit;
for (INT aTest =0 ; aTest < 100 ; aTest ++)
{
SetPtCur(P0.data());
Im1D_REAL8 aDir( NbVal(),0.0);
for (INT aK = 0 ; aK < NbVal() ; aK++)
{
ELISE_ASSERT(mMatrValP(aK,aK) != 0,"Jcobi ");
REAL Val = (mtBVecP(aK,0) / ElAbs(mMatrValP(aK,aK))) ;
REAL aRan = aR.cNRrandom3() ;
if (aRan < 0.25)
{
Val = 0;
}
else if (aRan < 0.5)
;
/*
{
Val = Val;
}
*/
else
Val *= 3 * aR.cNRrandom3() -1;
for (INT aY =0 ; aY< NbVal() ; aY++)
aDir.data()[aY] += mMatrVecP(aK,aY) * Val;
}
ELISE_COPY(aDir.all_pts(),P0.in() + aDir.in(),aDir.out());
SetPtCur(aDir.data());
REAL aSc = ScoreCur(false);
if (aSc < aScMin)
{
ElSetMin(aScMin,aSc);
for (INT aK= 0 ; aK< 10 ; aK++)
OneItereDevL1( aK < 3);
REAL aSc = ScoreCur(false);
if (aSc < aScAmMin)
{
aScAmMin = aSc;
ELISE_COPY(mImBest.all_pts(),CurVals().in(),mImBest.out());
}
}
cout << aScMin << " " << aScInit << " " << aSc << " " << aScAmMin << "\n";
}
SetPtCur(mImBest.data());
}
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;
}
