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