ElRotation3D cOneRotPowelOptimize::Param2Rot(const double * aP)
{
if (DEBUG_POWEL)
{
for (int aK=0 ; aK<5 ; aK++)
std::cout << aP[aK] << " ";
std::cout << "\n";
}
if (mContr==ePoseFigee)
return ElRotation3D(mU,mMatr,true);
aP+= mI0;
double aF = 0.05;
Pt3dr aTr = mU*cos(sqrt(ElSquare(aP[0]*aF)+ElSquare(aP[1]*aF)))
+ mV*sin(aP[0]*aF)
+ mW*sin(aP[1]*aF);
aTr = vunit(aTr) ;
double aN = mNorm;
if (mContr==ePoseLibre)
aN *= 1+aF*aP[5];
return ElRotation3D
(
vunit(aTr) * aN,
mMatr * ElMatrix<double>::Rotation(aP[2]*aF,aP[3]*aF,aP[4]*aF),
true
);
}
void GenCodeLaserImage(bool Normalize,bool Multi,INT aNb,INT aDegre)
{
tParamAFocal aNOPAF;
cout << "GenCodeLaserImage\n";
CamStenopeIdeale aCam(true,1.0,Pt2dr(0,0),aNOPAF);
cSetEqFormelles aSet;
Im2D_REAL4 aI(1,1);
cLIParam_Image * aP1 = aSet.NewLIParamImage(aI,1.0,aCam,cNameSpaceEqF::eRotFigee);
cLIParam_Image * aP2 = aSet.NewLIParamImage(aI,1.0,aCam,cNameSpaceEqF::eRotFigee);
cRotationFormelle * aRotPts = 0;
if (aDegre >=0)
aRotPts = aSet.NewRotationEvol(ElRotation3D(Pt3dr(0,0,0),0,0,0),aDegre);
aSet.NewLIEqVueLaserIm(aRotPts,Multi,Normalize,aNb,*aP1,*aP2,true);
}
cPairOfTriplet::cPairOfTriplet(cImOfTriplet * aI1,cImOfTriplet *aI2,cImOfTriplet *aI3) :
mAppli (aI1->Appli()),
mIm1 (aI1),
mIm2 (aI2),
mIm3 (aI3),
mR12Pair (ElRotation3D::Id)
{
mAppli.NM()->LoadHomFloats(mIm1->Im(),mIm2->Im(),&mFullVP1,&mFullVP2);
mFullHoms.push_back(&mFullVP1);
mFullHoms.push_back(&mFullVP2);
if (mAppli.Show())
std::cout << "cPairOfTriplet " << mFullVP1.size() << " " << mFullVP2.size() << "\n";
std::string aNameOri = mAppli.NM()->NameXmlOri2Im(mIm1->Im()->Name(),mIm2->Im()->Name(),true);
cXml_Ori2Im aXmlO = mAppli.NM()->GetOri2Im(mIm1->Im()->Name(),mIm2->Im()->Name());
const cXml_O2IRotation & aXO = aXmlO.Geom().Val().OrientAff();
mR12Pair = ElRotation3D (aXO.Centre(),ImportMat(aXO.Ori()),true);
}
void cMEPCoCentrik::Test(const ElPackHomologue & aPack,const ElMatrix<REAL> & aMat,const ElRotation3D * aRef,double anEcart)
{
if (mShow)
{
std::cout << " ============== ROTATION PURE =============\n";
}
ComputePlanBase(aMat);
Pt3dr aN1 = ComputeBase();
Pt3dr aN2 = ComputeNormBase();
Pt3dr aN3 = vunit(aN1^aN2);
ElRotation3D aRInit(aN3,aMat,true);
mSolVraiR = OneTestMatr(aMat,aN3,anEcart);
mCostVraiR = ProjCostMEP(aPack,mSolVraiR,0.1);
mPMed = MedianNuage(aPack,mSolVraiR);
if (mPMed.z < 0)
{
if (mShow) std::cout << " ------------- Z Neg in Pur rot : swap -------------\n";
mSolVraiR = ElRotation3D(-mSolVraiR.tr(),mSolVraiR.Mat(),true);
mPMed = MedianNuage(aPack,mSolVraiR);
aN3 = -aN3;
}
if ( mShow)
{
if (aRef)
{
std::cout << "Ref " << vunit(aRef->tr()) << " Norm " << aN3 << "\n";
std::cout << "COST REF " << ProjCostMEP(aPack,*aRef,0.1)* mFoc << "\n";
ElRotation3D aSol2 = OneTestMatr(aRef->Mat(),aRef->tr(),anEcart);
std::cout << "C REFOPT " << ProjCostMEP(aPack,aSol2,0.1)* mFoc << "\n";
// ShowMatr("REF/Mat",aRef->Mat()*aMat.transpose());
}
std::cout << " ###### Co-Centrik COST-Init " << ProjCostMEP(aPack,aRInit,0.1)* mFoc << "\n";
std::cout << "Sol , Cost " << mCostVraiR * mFoc << " Tr " << mSolVraiR.tr() << " Med " << mPMed << "\n";
}
// ShowMatr("REF/Sol",aRef->Mat()*aSol.Mat().transpose());
if (mDoPly)
{
std::list<std::string> aVCom;
std::vector<const cElNuage3DMaille *> aVNuage;
cElNuage3DMaille::PlyPutFile
(
"Base.ply",
aVCom,
aVNuage,
&mPtsPly,
&mColPly,
true
);
}
}
ElRotation3D AJ2R3d(const cAvionJauneDocument & anAJD)
{
return ElRotation3D(CentreAJ(anAJD),RotationVecAJ(anAJD),true);
}
void cAppliOptimTriplet::TestOPA(cPairOfTriplet & aPair)
{
ElRotation3D aR1 = ElRotation3D::Id;
std::list<Appar23> aL32;
int aI1 = aPair.Im1().Num();
int aI2 = aPair.Im2().Num();
int aI3 = aPair.Im3().Num();
const std::vector<Pt2df> & aVP1 = *(mRedH123[aI1]);
const std::vector<Pt2df> & aVP2 = *(mRedH123[aI2]);
const std::vector<Pt2df> & aVP3 = *(mRedH123[aI3]);
ElRotation3D aR2 = aPair.R12Pair();
if ( aVP1.size() < 4) return;
for (int aKP=0 ; aKP<int(aVP1.size()) ; aKP++)
{
std::vector<Pt3dr> aVA;
std::vector<Pt3dr> aVB;
const Pt2df & aP1 = aVP1[aKP];
aVA.push_back(Pt3dr(0,0,0));
aVB.push_back(Pt3dr(aP1.x,aP1.y,1.0));
const Pt2df & aP2 = aVP2[aKP];
aVA.push_back(aR2.ImAff(Pt3dr(0,0,0)));
aVB.push_back(aR2.ImAff(Pt3dr(aP2.x,aP2.y,1.0)));
bool OkI;
Pt3dr anInter = InterSeg(aVA,aVB,OkI);
if (OkI)
{
const Pt2df & aP3 = aVP3[aKP];
aL32.push_back(Appar23(Pt2dr(aP3.x,aP3.y),anInter));
}
}
ElTimer aChrono;
CamStenopeIdeale aCSI = CamStenopeIdeale::CameraId(true,ElRotation3D::Id);
double anEcart;
int aNbRansac = mQuick ? QuickNbRansac : StdNbRansac;
ElRotation3D aR3 = aCSI.RansacOFPA(true,aNbRansac,aL32,&anEcart);
aR3 = aR3.inv();
std::vector<ElRotation3D> aVR(3,ElRotation3D::Id);
aVR[aI1] = aR1;
aVR[aI2] = aR2;
aVR[aI3] = aR3;
ElRotation3D aR0 = aVR[0];
for (int aK=0 ; aK<3 ; aK++)
{
aVR[aK] = aR0.inv() * aVR[aK] ;
// aVR[aK] = aVR[aK] * aR0.inv() ;
}
double aFact = euclid(aVR[1].tr());
aVR[1] = ElRotation3D(aVR[1].tr()/aFact,aVR[1].Mat(),true);
aVR[2] = ElRotation3D(aVR[2].tr()/aFact,aVR[2].Mat(),true);
double aResidu = ResiduGlob(aVR[0],aVR[1],aVR[2]);
if (aResidu < mBestResidu)
{
mBestResidu = aResidu;
for (int aK=0 ; aK<3 ; aK++)
mIms[aK]->SetOri(aVR[aK]);
}
}
cEqCalibCroisee::cEqCalibCroisee
(
bool SensC2M,
cParamIntrinsequeFormel & aPIF,
cRotationFormelle * aRot,
bool Code2Gen
) :
mSet (*aPIF.Set()),
mPIF (aPIF),
mRotCalc ( aRot ?
aRot :
mSet.NewRotation(eRotCOptFige,ElRotation3D(Pt3dr(0,0,0),0,0,0))
),
mRot2Destr (aRot ? 0 : mRotCalc),
mP1 ("PIm1"),
mN2 ("DirEsp2"),
mResidu ( SensC2M ?
( PointNorm1(mPIF.Cam2DirRayMonde(mP1.PtF(),0))
- mRotCalc->VectM2C(mN2.PtF())
) :
AdZ0(mP1.PtF() -mPIF.DirRayMonde2Cam(mRotCalc->VectM2C(mN2.PtF()),0))
),
mNameType ("cEqCalibCroisee_" + mPIF.NameType() +"_CodGen" + std::string(SensC2M ? "C2M" : "M2C"))
{
// Test de coherence, si rotation vient de l'exterieur
ELISE_ASSERT
(
(&mSet==mRotCalc->Set()),
"Set incoherence in cEqObsRotVect::cEqObsRotVect"
);
// En mode utilisation, assurele lien les numeros d'inconnu de mRotCalc et
// la numerotation interne; en mode generation de code cree l'intervale de ref
mRotCalc->IncInterv().SetName("Orient");
mPIF.IncInterv().SetName("Calib");
mLInterv.AddInterv(mRotCalc->IncInterv());
mLInterv.AddInterv(mPIF.IncInterv());
// Cette etape va creer un objet de type "cEqCalibCroisee_Calib_CodeGen",
// tant que le code n'a pas ete genere et linke, il n'y a aucune
// raison pour que cet objet puisse etre cree, la valeur 0 sera alors
// retournee
mFoncEqResidu = cElCompiledFonc::AllocFromName(mNameType);
if (Code2Gen)
{
GenCode();
// En phase de generation il n'y a pas grand chose de plus a faire
return;
}
// En theorie le mecanisme permet de fonctionner meme sans generation
// de code par un mode degrade "interprete" ou toute les evaluation sont
// faite directement a partir des Fonc_Num ; cette fonctionnalite n'est
// pas vraiment maintenue (par exemple elle ne gere par les contraintes
// multiple); on conserve l'architecture de code "au cas ou "mais
// on en interdit l'usage
if (mFoncEqResidu==0)
{
std::cout << "Name = " << mNameType << "\n";
ELISE_ASSERT(false,"Can Get Code Comp for cEqCalibCroisee");
mFoncEqResidu = cElCompiledFonc::DynamicAlloc(mLInterv,Fonc_Num(0));
}
// Cree dans l'objet la lut vers les numero d'inconnues
mFoncEqResidu->SetMappingCur(mLInterv,&mSet);
// Le code genere fait reference au valeur de P1 et P2;
// pour pouvoir repidement faire l'initialisation on memorise
// leur adresse (on envoie un nom, on recupere une adresse)
// ceci est encapsule dans cP3d_Etat_PhgrF qui gere les variables
// d'etat triple correspondant des points 3D
mP1.InitAdr(*mFoncEqResidu);
mN2.InitAdr(*mFoncEqResidu);
aPIF.InitStateOfFoncteur(mFoncEqResidu,0);
// Il est necessaire que mSet connaisse l'ensemble de ses "foncteur"
// pour :
// 1- assurer la mise a jour des inconnues dans le cas d'un processus iteratif
// 2- assurer la destruction
mSet.AddFonct(mFoncEqResidu);
}
cEqObsRotVect::cEqObsRotVect
(
cSetEqFormelles & aSet,
cRotationFormelle * aRot,
bool Code2Gen
) :
mSet (aSet),
mRotCalc ( aRot ?
aRot :
aSet.NewRotation(eRotCOptFige,ElRotation3D(Pt3dr(0,0,0),0,0,0))
),
mRot2Destr (aRot ? 0 : mRotCalc),
mN1 ("P1"),
mN2 ("P2"),
mResidu (mRotCalc->VectC2M(mN1.PtF())-mN2.PtF()),
mNameType ("cEqObsRotVect_CodGen")
{
ELISE_ASSERT
(
(&mSet==mRotCalc->Set()),
"Set incoherence in cEqObsRotVect::cEqObsRotVect"
);
// En mode utilisation, assurele lien les numeros d'inconnu de mRotCalc et
// la numerotation interne; en mode generation de code cree l'intervale de ref
mRotCalc->IncInterv().SetName("Orient");
mLInterv.AddInterv(mRotCalc->IncInterv());
// Cette etape va creer un objet de type "cEqObsRotVect_CodGen",
// tant que le code n'a pas ete genere et linke, il n'y a aucune
// raison pour que cet objet puisse etre cree, la valeur 0 sera alors
// retournee
mFoncEqResidu = cElCompiledFonc::AllocFromName(mNameType);
if (Code2Gen)
{
GenCode();
// En phase de generation il n'y a pas grand chose de plus a faire
return;
}
// En theorie le mecanisme permet de fonctionner meme sans generation
// de code par un mode degrade "interprete" ou toute les evaluation sont
// faite directement a partir des Fonc_Num ; cette fonctionnalite n'est
// pas vraiment maintenue (par exemple elle ne gere par les contraintes
// multiple); on conserve l'architecture de code "au cas ou "mais
// on en interdit l'usage
if (mFoncEqResidu==0)
{
ELISE_ASSERT(false,"Can Get Code Comp for cCameraFormelle::cEqAppui");
mFoncEqResidu = cElCompiledFonc::DynamicAlloc(mLInterv,Fonc_Num(0));
}
// Cree dans l'objet la lut vers les numero d'inconnues
// reduits (ici [0,3[)
mFoncEqResidu->SetMappingCur(mLInterv,&mSet);
// Le code genere fait reference au valeur de P1 et P2;
// pour pouvoir repidement faire l'initialisation on memorise
// leur adresse (on envoie un nom, on recupere une adresse)
// ceci est encapsule dans cP3d_Etat_PhgrF qui gere les variables
// d'etat triple correspondant des points 3D
mN1.InitAdr(*mFoncEqResidu);
mN2.InitAdr(*mFoncEqResidu);
// Il est necessaire que mSet connaisse l'ensemble de ses "foncteur"
// pour :
// 1- assurer la mise a jour des inconnues dans le cas d'un processus iteratif
// 2- assurer la destruction
mSet.AddFonct(mFoncEqResidu);
}