static Pt2d<Fonc_Num> P1_EDD
(
cParamIntrinsequeFormel & aPIF,
Pt2d<Fonc_Num> aP2 ,
cNameSpaceEqF::eTypeEqDisDirecre Usage
)
{
if ( Usage== cNameSpaceEqF::eTEDD_Reformat)
return aPIF.DistC2M(aP2,0);
if (Usage == cNameSpaceEqF::eTEDD_Bayer)
return aPIF.DistC2M(aP2,0) + (aP2-aPIF.FPP()).mul(aPIF.FFoc());
// eTEDD_Interp
Pt3d<Fonc_Num> aP3 = aPIF.Cam2DirRayMonde(aP2,0);
return Pt2d<Fonc_Num>(aP3.x,aP3.y);
}
cEqDirecteDistorsion:: cEqDirecteDistorsion
(
cParamIntrinsequeFormel & aPIF,
eTypeEqDisDirecre Usage,
bool Code2Gen
) :
mUsage (Usage),
mSet (*aPIF.Set()),
mPIF (aPIF),
mP1 ("PIm1"),
mP2 ("PIm2"),
mResidu (P1_EDD(mPIF,mP1.PtF(),Usage) - mP2.PtF()),
mNameType ( "cEqDirectDist"
+ mPIF.NameType()
+ (Usage2Str(Usage))
+"_CodGen"
)
{
mPIF.IncInterv().SetName("Calib");
mLInterv.AddInterv(mPIF.IncInterv());
mFoncEqResidu = cElCompiledFonc::AllocFromName(mNameType);
if (Code2Gen)
{
GenCode();
return;
}
if (mFoncEqResidu==0)
{
std::cout << "Name = " << mNameType << "\n";
ELISE_ASSERT(false,"Can Get Code Comp for cEqCalibCroisee");
mFoncEqResidu = cElCompiledFonc::DynamicAlloc(mLInterv,Fonc_Num(0));
}
mFoncEqResidu->SetMappingCur(mLInterv,&mSet);
mP1.InitAdr(*mFoncEqResidu);
mP2.InitAdr(*mFoncEqResidu);
aPIF.InitStateOfFoncteur(mFoncEqResidu,0);
mSet.AddFonct(mFoncEqResidu);
}
void cChangeCamFormat::DoAll()
{
mEq = mSet.NewEqCalibCroisee(mC2M,*mPIF);
mSet.SetClosed();
for (int aKet=0 ; aKet< 30 ; aKet++)
{
if (mPIF_DR)
{
if (aKet== 1)
mPIF_DR->SetFocFree(true);
if (aKet >= 3)
{
int aDegre = ElMin(mDegRadOut,1+ (aKet-3)/3);
mPIF_DR->SetDRFDegreFige(aDegre);
}
if (aKet >= 10)
mPIF_DR->SetCDistPPLie();
if ((aKet >= 15) && (! mCDistPPLie))
mPIF_DR->SetLibertePPAndCDist(true,true);
}
else
{
ELISE_ASSERT(false,"Unknown Cam Modele");
}
int aNBP = 20;
mSet.AddContrainte(mPIF->StdContraintes(),true);
mSet.AddContrainte(mEq->RotF().StdContraintes(),true);
for (int aKx=0 ; aKx<= aNBP ; aKx++)
{
double aPdsX = aKx / double(aNBP);
for (int aKy=0 ; aKy<= aNBP ; aKy++)
{
double aPdsY = aKy / double(aNBP);
Pt2dr aP0 (mSz.x*aPdsX,mSz.y*aPdsY);
Pt3dr aDir = mCamInput->F2toDirRayonL3(aP0);
Pt2dr aPNew = ToNewCoord(aP0);
mEq->AddObservation(aPNew,aDir);
}
}
mSet.SolveResetUpdate();
}
}
void cMA_AffineOrient::OneItere(bool isLast)
{
ElRotation3D aR1 = mCamF1->RF().CurRot();
ElRotation3D aR2 = mCamF2->RF().CurRot();
std::cout << "TR = " << (aR2.tr()-aR1.tr()) << "\n";
std::cout << "I = " << aR2.ImVect(Pt3dr(1,0,0)) << euclid(aR2.ImVect(Pt3dr(1,0,0))) << "\n";
std::cout << "J = " << aR2.ImVect(Pt3dr(0,1,0)) << euclid(aR2.ImVect(Pt3dr(0,1,0))) << "\n";
std::cout << "K = " << aR2.ImVect(Pt3dr(0,0,1)) << euclid(aR2.ImVect(Pt3dr(0,0,1))) << "\n";
mSetEq.AddContrainte(mPIF->StdContraintes(),true);
mSetEq.AddContrainte(mCamF1->RF().StdContraintes(),true);
mSetEq.AddContrainte(mCamF2->RF().StdContraintes(),true);
double aSEc2 =0;
double aSEc =0;
double aSP =0;
int aNbP=0;
for
(
ElPackHomologue::iterator iT = mPack.begin();
iT != mPack.end();
iT++
)
{
// double aD = mCam1.EcartProj(iT->P1(),mCam2,iT->P2());
Pt2dr aQ1 = mCam1.F2toPtDirRayonL3(iT->P1());
Pt2dr aQ2 = mCam2.F2toPtDirRayonL3(iT->P2());
//double aL = mCpl12->ResiduSigneP1P2(aQ1,aQ2);
double aL2 = mCpl12->AddLiaisonP1P2(aQ1,aQ2,iT->Pds(),false);
aSEc2 += ElSquare(aL2)*iT->Pds();
aSEc += ElAbs(aL2)*iT->Pds();
aSP += iT->Pds();
Pt2dr aP1 = mGeoTer.R2ToRDisc(iT->P1());
Pt2di aI1 = round_down(aP1/mPas);
mTImResidu.oset(aI1,aL2*mFocale);
mTImPds.oset(aI1,iT->Pds());
if (iT->Pds() > 1e-5)
aNbP++;
}
if (aNbP < mNbPtMin)
{
mAppli.MicMacErreur
(
eErrNbPointInEqOriRel,
"Pas assez de point pour equation d'orientation relative",
"Zone de recouvrement trop faible"
);
}
mSetEq.SolveResetUpdate();
// if (isLast)
{
std::cout
<< aSEc2 << " " << aSP << " " << mPack.size()
<< " EQuad = " << (sqrt(aSEc2/aSP)*mFocale)
<< " EAbs = " << ((aSEc/aSP)*mFocale)
<< "\n";
}
}
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);
}
