void cEqObsRotVect::AddObservation
(
Pt2dr aDir1,
Pt2dr aDir2,
double aPds,
bool WithD2
)
{
AddObservation(PZ1(aDir1),PZ1(aDir2),aPds,WithD2);
}
// Calcul les vecteur qui sont |_ dans un plan contenant la base
void cMEPCoCentrik::ComputePlanBase(const ElMatrix<REAL> & aMat)
{
Pt3di aRouge(255,0,0);
Pt3di aVert(0,255,0);
Pt3di aGrayFonc(64,64,64);
Pt3di aGrayClair(180,180,180);
double aMulP = 1.1;
std::vector<Pt3dr> aVpt;
for (int aKP =0 ; aKP< int(mVP1.size()) ; aKP++)
{
Pt3dr aQ1 = vunit(PZ1(mVP1[aKP]));
Pt3dr aQ2 = aMat * vunit(PZ1(mVP2[aKP]));
Pt3dr anOB = aQ1^aQ2;
if (euclid(anOB) != 0)
{
anOB = vunit(anOB);
if (mDoPly)
{
mColPly.push_back(aRouge);
mPtsPly.push_back(anOB);
mColPly.push_back(aGrayFonc);
mPtsPly.push_back(aQ1*aMulP);
mColPly.push_back(aGrayClair);
mPtsPly.push_back(aQ2*aMulP);
}
mVPlanBase.push_back(anOB);
}
}
if (mDoPly)
{
for (int aK=0 ; aK<10000 ; aK++)
{
int aKA = NRrandom3(mVPlanBase.size());
int aKB = NRrandom3(mVPlanBase.size());
Pt3dr aBase = mVPlanBase[aKA] ^ mVPlanBase[aKB];
if (euclid (aBase)>1e-6)
{
mColPly.push_back(aVert);
mPtsPly.push_back(vunit(aBase));
}
}
}
}
void cMEPCoCentrik::OneItereRotPur(ElMatrix<REAL> & aMat,double & anErrStd)
{
L2SysSurResol aSysLin3(3);
aSysLin3.GSSR_Reset(false);
std::vector<double> aVRes;
double aSomP=0;
double aSomErr=0;
for (ElPackHomologue::const_iterator itP=mPack.begin() ; itP!=mPack.end() ; itP++)
{
Pt3dr aQ1 = vunit(PZ1(itP->P1()));
Pt3dr aQ2 = aMat * vunit(PZ1(itP->P2()));
double aVQ2[3],aVQ1[3];
aQ2.to_tab(aVQ2);
aQ1.to_tab(aVQ1);
double anEcart = euclid(aQ1-aQ2);
aVRes.push_back(anEcart);
double aPds = itP->Pds() / (1 + ElSquare(anEcart / (2*anErrStd)));
aSomP += aPds;
aSomErr += aPds * square_euclid(aQ1-aQ2);;
ElMatrix<REAL> aMQ2 = MatProVect(aQ2);
for (int aY=0 ; aY< 3 ; aY++)
{
double aCoeff[3];
for (int aX=0 ; aX< 3 ; aX++)
aCoeff[aX] = aMQ2(aX,aY);
aSysLin3.GSSR_AddNewEquation(aPds,aCoeff,aVQ2[aY]-aVQ1[aY],0);
}
}
double anErrRobut = MoyKPPVal(aVRes,NbForEcart(aVRes.size()));
double anErrQuad = sqrt(aSomErr/aSomP);
if (0)
{
std::cout << "ERR QUAD " << anErrQuad * mFoc << " Robust " << anErrRobut * mFoc << "\n";
}
anErrStd = anErrQuad;
Im1D_REAL8 aSol = aSysLin3.GSSR_Solve (0);
double * aData = aSol.data();
ElMatrix<double> aMPV = MatProVect(Pt3dr(aData[0],aData[1],aData[2]));
// std::cout << " aData " << aData[0] << " " << aData[1] << " " << aData[2] << "\n";
// aMat = NearestRotation(aMat * (ElMatrix<double>(3,true) +aMPV));
aMat = NearestRotation((ElMatrix<double>(3,true) +aMPV) * aMat);
}
void cAppliOptimTriplet::ShowPoint(cImOfTriplet * aIm,Pt2df aP,int aCoul,int aRay)
{
Video_Win * aW = aIm->W();
if (aW)
{
Pt2dr aPr(aP.x,aP.y);
aPr = aIm->Im()->CS()->R3toF2(PZ1(aPr));
aW->draw_circle_abs(aPr,aRay,Line_St(aW->pdisc()(aCoul)));
}
}
ElMatrix<REAL> GlobMepRelCocentrique(double & anEcartMin,const ElPackHomologue & aPack, int aNbRansac,int aNbMaxPts)
{
aNbMaxPts = ElMin(aNbMaxPts,aPack.size());
std::vector<Pt3dr> aVDir1;
std::vector<Pt3dr> aVDir2;
cRandNParmiQ aRand(aNbMaxPts,aPack.size());
for (ElPackHomologue::tCstIter itH=aPack.begin() ; itH!=aPack.end() ; itH++)
{
if (aRand.GetNext())
{
aVDir1.push_back(vunit(PZ1(itH->P1())));
aVDir2.push_back(vunit(PZ1(itH->P2())));
}
}
ElMatrix<REAL> aRes(3,3);
anEcartMin = 1e60;
while (aNbRansac)
{
int aKA = NRrandom3(aVDir1.size());
int aKB = NRrandom3(aVDir2.size());
if (aKA!=aKB)
{
aNbRansac--;
ElMatrix<REAL> aMat = ComplemRotation(aVDir1[aKA],aVDir1[aKB],aVDir2[aKA],aVDir2[aKB]);
double anEc = SomEcartDist(aMat,aVDir1,aVDir2);
if (anEc<anEcartMin)
{
anEcartMin = anEc;
aRes = aMat;
}
}
}
return aRes;
}
