void cImplemBlockCam::EstimCurOri(const cEstimateOrientationInitBlockCamera & anEOIB)
{
cLiaisonsSHC aLSHC;
for (int aKC=0 ; aKC<mNbCam ; aKC++)
{
if (anEOIB.Show().Val())
std::cout << "=================================================\n";
Pt3dr aSomTr(0,0,0);
double aSomP=0;
ElMatrix<double> aSomM(3,3,0.0);
for (int aKT=0 ; aKT<mNbTime ; aKT++)
{
cIBC_ImsOneTime * aTime = mNum2ITime[aKT];
cPoseCam * aP0 = aTime->Pose(0);
cPoseCam * aP1 = aTime->Pose(aKC);
if (aP0 && aP1)
{
ElRotation3D aR0toM = aP0->CurCam()->Orient().inv(); // CONV-ORI
ElRotation3D aR1toM = aP1->CurCam()->Orient().inv();
ElRotation3D aR1to0 = aR0toM.inv() * aR1toM; // CONV-ORI
if (anEOIB.Show().Val())
{
std::cout << " EstimCurOri " << aP0->Name() << " " << aP1->Name() << "\n";
std::cout << " " << aR1to0.ImAff(Pt3dr(0,0,0))
<< " " << aR1to0.teta01()
<< " " << aR1to0.teta02()
<< " " << aR1to0.teta12()
<< "\n";
}
aSomTr = aSomTr+ aR1to0.tr();
aSomM += aR1to0.Mat();
aSomP++;
}
}
if (aSomP)
{
aSomTr = aSomTr / aSomP;
aSomM *= 1.0/aSomP;
aSomM = NearestRotation(aSomM);
ElRotation3D aRMoy(aSomTr,aSomM,true);
std::cout << " ========== AVERAGE =========== \n";
std::cout << " " << aRMoy.ImAff(Pt3dr(0,0,0))
<< " tetas " << aRMoy.teta01()
<< " " << aRMoy.teta02()
<< " " << aRMoy.teta12()
<< "\n";
cParamOrientSHC aP;
aP.IdGrp() = mNum2Cam[aKC]->NameCam();
aP.Vecteur() = aRMoy.ImAff(Pt3dr(0,0,0));
aP.Rot() = ExportMatr(aSomM);
aLSHC.ParamOrientSHC().push_back(aP);
}
}
mEstimSBC.LiaisonsSHC().SetVal(aLSHC);
}
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);
}
ElMatrix<double> cOriFromBundle::CalculRot(const cOFB_Sol1S1T & aSol,bool Show)
{
// R [mDir1B ...] = [mV1 ...]
ElMatrix<double> aMatB = MatFromCol(mDir1B,mDir2B,mDir3B);
ElMatrix<double> aMatBinA = MatFromCol(aSol.mV1,aSol.mV2,aSol.mV3);
ElMatrix<double> aMat = aMatBinA * gaussj(aMatB);
// std::cout << mDir1B << mDir2B << mDir3B << "\n";
// std::cout << aSol.mV1 << aSol.mV2 << aSol.mV3 << "\n";
// std::cout << "xxxxxxxxxxxx\n";
/*
for (int anY=0 ; anY<3 ; anY++)
{
for (int anX=0 ; anX<3 ; anX++)
{
std::cout << aMat(anX,anY) << " ";
}
std::cout << "\n";
}
*/
// getchar();
ElMatrix<double> aR = NearestRotation(aMat);
// std::cout << "yyyyyyyyyyyyyy\n";
if (Show)
{
std::cout << "DET " << aR.Det() -1.0 << " Eucl " << ElMatrix<double>(3,true).L2(aR*aR.transpose())<< "\n";
std::cout << " SCAL " << scal(mBase^mDir1A,aR*mDir1B) << " "
<< scal(mBase^mDir2A,aR*mDir2B) << " "
<< scal(mBase^mDir3A,aR*mDir3B) << "\n";
}
return aR;
}
