cParamIFDistStdPhgr::cParamIFDistStdPhgr
(
bool isDC2M,
cCamStenopeModStdPhpgr * aCam,cSetEqFormelles & aSet,
int aDegFig
) :
cParamIFDistRadiale(isDC2M,aCam,aSet,aDegFig,aCam) ,
mDStd (mDRF.DistCurStd()),
mInitDStd (mDRF.DistInitStd()),
mParamDecentreFree (false),
mParamAffineFree (false),
mFP1 (aSet.Alloc().NewF(&(mDStd.P1()))),
mFP2 (aSet.Alloc().NewF(&(mDStd.P2()))),
mFb1 (aSet.Alloc().NewF(&(mDStd.b1()))),
mFb2 (aSet.Alloc().NewF(&(mDStd.b2()))),
mTol_Dec_PhgStdPIF (cContrainteEQF::theContrStricte),
mTol_Aff_PhgStdPIF (cContrainteEQF::theContrStricte),
mCurPIF (0)
{
// aSet.ShowVar(); std::cout <<"=======================\n";
//aSet.ShowVar(); std::cout << mDStd.P1() << " " << mDStd.P2() << " 11111\n"; getchar();
cDistModStdPhpgr aDistDeg = aCam->DModPhgrStd();
aDistDeg.DRad() = SetToDeg(aDistDeg.DRad(),5);
mDStd = aDistDeg;
mInitDStd = aDistDeg;
SetCDistPPLie();
NV_UpdateCurPIF();
//aSet.ShowVar(); std::cout << "22222\n"; getchar();
}
cHomogFormelle::cComp::cComp
(
cElComposHomographie & aComp,
cSetEqFormelles & aSet,
bool IsDenom
) :
mX (aSet.Alloc().NewF(&aComp.CoeffX())),
mY (aSet.Alloc().NewF(&aComp.CoeffY())),
m1 (IsDenom ?
Fonc_Num(1) :
aSet.Alloc().NewF(&aComp.Coeff1())
)
{
}
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();
}
}
cElemEqFormelle::cElemEqFormelle(cSetEqFormelles & aSet,bool isTmp) :
mSet (aSet),
mNumInc0 (aSet.Alloc().CurInc()),
mIncInterv (isTmp,"toto",aSet),
mClosed (false)
{
}
void cElCompiledFonc::InitBloc(const cSetEqFormelles & aSet)
{
int aSom=0;
for (int aKB=0 ; aKB<int(mBlocs.size()) ; aKB++)
{
mBlocs[aKB] = aSet.GetBlocOfI0Alloc(mBlocs[aKB].I0Brut(),mBlocs[aKB].I1Brut()); // ###
// aSom += mBlocs[aKB].I1Abs() - mBlocs[aKB].I0Abs();
aSom += mBlocs[aKB].Nb();
//std::cout << "IIIIBloc " << (mBlocs[aKB].mInt->NumBloc()) << "\n";
}
// cCmpNumBlocSolve aCmp;
// std::sort(mBlocs.begin(),mBlocs.end(),aCmp);
}
cParamIFHomogr::cParamIFHomogr
(
bool isDC2M,
cCamStenopeDistHomogr * aCam,
cSetEqFormelles & aSet,
eModeContrHom aMode
) :
cParamIntrinsequeFormel(isDC2M,aCam,aSet,false),
mHF (aSet.NewHomF(aCam->Hom(),aMode)),
mCurPIF (0)
{
NV_UpdateCurPIF();
}
cDistRadialeFormelle::cDistRadialeFormelle
(
bool asIntervBlock,
bool isCFige,
int aDegFig,
const ElDistRadiale_PolynImpair & aDist,
cSetEqFormelles & aSet,
cCamStenopeModStdPhpgr * aDP
) :
cElemEqFormelle(aSet,false),
mCentreFige (isCFige),
mDegreFige (aDegFig),
mDistInitStd (aDist),
mCurDistStd (aDist),
mDistInit (mDistInitStd.DRad()),
mCurDist (mCurDistStd.DRad()),
mIndCentre (aSet.Alloc().CurInc()),
mFCentre (aSet.Alloc().NewPt2(mCurDist.Centre())),
mTolCDist (cContrainteEQF::theContrStricte),
mTolCoeffs (cContrainteEQF::theContrStricte)
{
if (aDP)
{
mCurDistStd.P1() = aDP->DModPhgrStd().P1();
mCurDistStd.P2() = aDP->DModPhgrStd().P2();
mCurDistStd.b1() = aDP->DModPhgrStd().b1();
mCurDistStd.b2() = aDP->DModPhgrStd().b2();
}
for (INT aK=0 ; aK<INT(mCurDist.NbCoeff()) ; aK++)
{
mFCoeffs.push_back(aSet.Alloc().NewF(&(mCurDist.Coeff(aK))));
}
CloseEEF(asIntervBlock);
/// std::cout << "VPPPPPPPpp " << mCurDist.RMax() << "\n"; getchar();
}
void cChangeCamFormat::SetDRad(bool CDistPPLie,int aDegRadOut)
{
mCDistPPLie = CDistPPLie;
mDegRadOut = aDegRadOut;
ElDistRadiale_PolynImpair aDist = ElDistRadiale_PolynImpair::DistId(mRay,mPPInit,5);
cCamStenopeDistRadPol * aCamDRInit = new cCamStenopeDistRadPol(mC2M,mFInit,mPPInit,aDist,mCamInput->ParamAF());
mCamInit = aCamDRInit;
mPIF_DR = mSet.NewIntrDistRad (mC2M,aCamDRInit,0);
mPIF_DR->SetFocFree(false);
mPIF_DR->SetLibertePPAndCDist(false,false);
mPIF = mPIF_DR;
}
cPolynFormelXY::cPolynFormelXY
(
const Polynome2dReal & aPol,
cSetEqFormelles & aSet,
bool isX
) :
cElemEqFormelle(aSet,false),
mPolInit (aPol),
mPolCur (aPol),
mIsX (isX),
mPolXYTol (cContrainteEQF::theContrStricte)
{
for (INT aK = 0 ; aK<mPolCur.NbMonome() ; aK++)
{
vCOeffs.push_back(aSet.Alloc().NewF(&mPolCur.Coeff(aK)));
mFige.push_back(true);
}
CloseEEF();
}
cIncIntervale::cIncIntervale
(
bool isTmp,
const tId & anId,
cSetEqFormelles & aSet,
int aNbInc
) :
mI0Alloc (aSet.Alloc().CurInc()),
mI1Alloc ((aNbInc<=0) ? -1: (mI0Alloc+aNbInc)),
mI0Solve (-1),
mI1Solve (-1),
mSet (&aSet),
mId (anId),
mNumBlocAlloc (-1),
mNumBlocSolve (-1),
mFiged (false),
mOrder (-1),
mIsTmp (isTmp)
{
}
const std::vector<double> & cEqObsBlockCam::AddObs(const double & aPdsTr,const double & aPdsMatr)
{
#if (HandleGL)
mMatR0.SetEtat(mRotRT0->MGL());
mMatL0.SetEtat(mRotLT0->MGL());
mMatR1.SetEtat(mRotRT1->MGL());
mMatL1.SetEtat(mRotLT1->MGL());
#endif
// mGPS.SetEtat(aGPS);
std::vector<double> aVPds;
for (int aK=0 ; aK<3; aK++)
aVPds.push_back(aPdsTr);
for (int aK=0 ; aK<9; aK++)
aVPds.push_back(aPdsMatr);
// Compute the values and its derivative (in class cCodeBlockCam here)
// Link it to the real index of the variable
// fill the covariance stuff taking into account these values
return mSet->VAddEqFonctToSys(mFoncEqResidu,aVPds,false);
}
void cElCompiledFonc::SVD_And_AddEqSysSurResol
(
bool isCstr,
const std::vector<INT> & aVIndInit,
const std::vector<double> & aVPds,
REAL * Pts,
cGenSysSurResol & aSys,
cSetEqFormelles & aSet,
bool EnPtsCur
)
{
int aSzPds = (int)aVPds.size();
ELISE_ASSERT((aSzPds==1) || (aSzPds==mDimOut),"Taille Pds incohe in cElCompiledFonc::SVD_And_AddEqSysSurResol");
if (INT(aVIndInit.size())!=mNbCompVar)
{
std::cout << "SIZE " << aVIndInit.size() << " " << mNbCompVar << "\n";
ELISE_ASSERT
(
false,
"cElCompiledFonc::SVD_And_AddEqSysSurReol"
);
}
for (INT aK=0 ; aK< mNbCompVar ; aK++)
mCompCoord[aK] = Pts[aVIndInit[aK]];
static bool first = false;
ElTimer aChrono;
INT aNb = first ? 1 : 1;
for (INT aK=0 ; aK< aNb ; aK++)
ComputeValDeriv();
isValComputed = true;
isDerComputed = true;
bool UseMat = aSys.GSSR_UseEqMatIndexee() && false;
static std::vector<INT> aVInd;
static std::vector<double> aDer;
if (UseMat)
{
ELISE_ASSERT(false,"Use mat to implement");
}
else
{
for (INT aD= 0 ; aD < mDimOut ; aD++)
{
double aPdsCur = aVPds[ElMin(aD,aSzPds-1)];
aVInd.clear();
aDer.clear();
REAL aB = -mVal[aD];
bool GotCstr=false; GccUse(GotCstr);
for (INT aK=0 ; aK< mNbCompVar ; aK++)
{
double aDdk = mCompDer[aD][aK];
if (aDdk)
{
int anInd = aVIndInit[aK];
double aValForced;
/*
*/
// if (PermutIndex)
anInd = aSet.Alloc2Solve(anInd);
if(aSys.IsCstrUniv(anInd,aValForced) )
{
ELISE_ASSERT(!isCstr,"Multiple contrainte univ");
GotCstr=true;
aB -= aDdk * aValForced;
mCompDer[aD][aK] = 0;
}
else
{
if (! EnPtsCur)
{
aB += aDdk * mCompCoord[aK];
}
aVInd.push_back(anInd);
aDer.push_back(mCompDer[aD][aK]);
}
}
}
// if (GotCstr && isCstr && (aVInd.size()
if (isCstr)
{
aSys.GSSR_AddContrainteIndexee(aVInd,&(aDer[0]),aB);
}
else
{
aSys.GSSR_AddNewEquation_Indexe
(
&mBlocs,
&(mCompDer[aD][0]),
(int)aVIndInit.size(),
aVInd,
aPdsCur,
( ( aDer.size()==0 )?NULL:&(aDer[0]) ),
aB
);
}
}
}
}
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";
}
}
cParamIntrinsequeFormel::cParamIntrinsequeFormel
(
bool isDC2M,
CamStenope * aCamInit,
cSetEqFormelles & aSet,
bool ParamVar
) :
cElemEqFormelle (aSet,false),
isDistC2M (isDC2M),
isFocaleFree (false),
isPPFree (false),
isAFoc1Free (false),
isAFoc2Free (false),
mCamInit (aCamInit),
mCurPIF (0),
mCamGrid (0),
mFiged (false),
mFocaleInit (mCamInit->Focale()),
mCurFocale (mFocaleInit),
mPPInit (mCamInit->PP()),
mCurPP (mPPInit),
mAFocInit (mCamInit->ParamAF()),
mCurAFoc (mAFocInit),
mFFoc ( ParamVar ?
aSet.Alloc().NewF(&mCurFocale) :
Fonc_Num(mFocaleInit)
),
mIndPP (aSet.Alloc().CurInc()),
mFPP (
ParamVar ?
aSet.Alloc().NewPt2(mCurPP) :
Pt2d<Fonc_Num>(mPPInit.x,mPPInit.y)
),
mIndAF (aSet.Alloc().CurInc()),
mFAFoc (aSet.Alloc().NewVectInc(mCurAFoc)),
mParamVar (ParamVar),
mTolFoc (cContrainteEQF::theContrStricte),
mTolPP (cContrainteEQF::theContrStricte),
mTolAF1 (cContrainteEQF::theContrStricte),
mTolAF2 (cContrainteEQF::theContrStricte),
mProjStenF (mFFoc,mFPP.x,mFPP.y,mFAFoc),
mRegulDistDxyP1 (new cP2d_Etat_PhgrF("RegDistxy1")),
mRegulDistDxyP2 (new cP2d_Etat_PhgrF("RegDistxy2")),
mRegulDistDxyP3 (new cP2d_Etat_PhgrF("RegDistxy3")),
mRegulDistDxyP4 (new cP2d_Etat_PhgrF("RegDistxy4")),
mRegulDistDxxP1 (new cP2d_Etat_PhgrF("RegDistuu1")),
mRegulDistDxxP2 (new cP2d_Etat_PhgrF("RegDistuu2")),
mRegulDistDxxP3 (new cP2d_Etat_PhgrF("RegDistuu3")),
// mRegulDistDyyP1 (new cP2d_Etat_PhgrF("RegDistuu1")),
// mRegulDistDyyP2 (new cP2d_Etat_PhgrF("RegDistuu2")),
// mRegulDistDyyP3 (new cP2d_Etat_PhgrF("RegDistuu3")),
mRegulDistDxP1 (new cP2d_Etat_PhgrF("RegDistu1")),
mRegulDistDxP2 (new cP2d_Etat_PhgrF("RegDistu2")),
mRegulDistKnownDer (new cP2d_Etat_PhgrF("RegDistu3")),
// mRegulDistDyP1 (new cP2d_Etat_PhgrF("RegDistu1")),
// mRegulDistDyP2 (new cP2d_Etat_PhgrF("RegDistu2")),
mRegulDistValP1 (new cP2d_Etat_PhgrF("RegDistValP1")),
mRegulDistKnownVal (new cP2d_Etat_PhgrF("RegDistKnownVal")),
mFER_DxDy (0),
mFER_Dxx (0),
mFER_Dx (0),
mFER_Val (0),
mImPdsDef (1,1,1.0)
{
NV_UpdateCurPIF();
}
cSomBilin::cSomBilin(cSetEqFormelles & aSet,Pt2dr & aPt,const cIncIntervale & anInt) :
mPtF (aSet.Alloc().NewPt2(aPt)),
mInterv (anInt)
{
}
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);
}
