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