double cOneTestLSQ::Correl(const Pt2dr & aDec) const
{
if (! OK(aDec)) return -1;
double aSom2= 0;
double aSom22= 0;
double aSom12= 0;
int aCpt=0;
for(int aKx=-mNbW; aKx<=mNbW; aKx++)
{
for(int aKy=-mNbW; aKy<=mNbW; aKy++)
{
Pt2dr aP2 = aDec+mIm2->PVois(aKx,aKy);
double aV2 = mIm2->GetVal(aP2);
double aV1 = mValsIm1[aCpt];
aSom2 += aV2;
aSom12 += aV1*aV2;
aSom22 += ElSquare(aV2);
aCpt++;
}
}
aSom2 /= mSomP;
aSom12 /= mSomP;
aSom22 /= mSomP;
aSom12 -= mSom1 * aSom2;
aSom22 -= ElSquare(aSom2);
return aSom12 * sqrt(std::max(1e-5,aSom22*mSom11));
}
double cPonderateur::PdsOfError(double anEr) const
{
anEr = ElAbs(anEr);
if (anEr>mEcMax)
{
// std::cout << "ERR = " << anEr << "\n";
return 0;
}
switch (mMode)
{
case ePondL2 :
return mPdsStd;
case ePondL1 :
return mPdsStd / anEr;
case ePondLK :
return mPdsStd / pow(anEr,2-mExpoLK);
case ePondGauss :
return mPdsStd * exp(-ElSquare(anEr/mSigmaP));
case eL1Secured :
{
double aPds = mPdsStd * sqrt(1/(1+ElSquare(anEr/mSigmaP)));
//std::cout << anEr/mSigmaP << " " << mPdsStd << " " << aPds << "\n";
return aPds;
}
}
ELISE_ASSERT(false,"cPonderateur::PdsOfError");
return 0;
}
void BenchcDbleGrid
(
Pt2dr aP0In,Pt2dr aP1In,
REAL aStepDir,
ElDistortion22_Gen & aDist
)
{
//cDbleGrid aDGr(aP0In,aP1In,aStepDir,aDist);
Pt2dr stepDir2(aStepDir,aStepDir); // __NEW
cDbleGrid aDGr(false,aP0In,aP1In,stepDir2,aDist); // __NEW
for (REAL aX = aP0In.x ; aX<aP1In.x ; aX += aStepDir)
for (REAL aY = aP0In.y ; aY<aP1In.y ; aY += aStepDir)
{
REAL x = aX + NRrandom3() * aStepDir;
SetInRange(aP0In.x,x,aP1In.x);
REAL y = aY + NRrandom3() * aStepDir;
SetInRange(aP0In.y,y,aP1In.y);
Pt2dr aP(x,y);
Pt2dr aQ0 = aDist.Direct(aP);
Pt2dr aQ1 = aDGr.Direct(aP);
Pt2dr aR0 = aDist.Inverse(aQ0);
Pt2dr aR1 = aDist.Inverse(aQ1);
REAL aDQ = euclid(aQ0,aQ1);
REAL aDR = euclid(aR0,aP) + euclid(aR1,aP);
aDQ /= ElSquare(aStepDir);
aDR /= ElSquare(aStepDir);
BENCH_ASSERT(aDQ<0.1);
BENCH_ASSERT(aDR<0.1);
}
}
void cOneTestLSQ::InitP1()
{
mValsIm1.clear();
mDeps.clear();
mSomP=0;
mSom1=0;
mSom11=0;
for(int aKx=-mNbW; aKx<=mNbW; aKx++)
{
for(int aKy=-mNbW; aKy<=mNbW; aKy++)
{
Pt2dr aP1 = mIm1->PVois(aKx,aKy);
double aV1 = mIm1->GetVal(aP1);
mSom1 += aV1;
mSom11 += ElSquare(aV1);
mSomP++;
mValsIm1.push_back(aV1);
}
}
mSom1 /= mSomP;
mSom11 /= mSomP;
mSom11 -= ElSquare(mSom1);
}
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 cEqAppui_X_C2MDPol7::ComputeVal()
{
double tmp0_ = mCompCoord[72];
double tmp1_ = mCompCoord[73];
double tmp2_ = cos(tmp1_);
double tmp3_ = sin(tmp0_);
double tmp4_ = cos(tmp0_);
double tmp5_ = sin(tmp1_);
double tmp6_ = mCompCoord[74];
double tmp7_ = mCompCoord[75];
double tmp8_ = mLocXTer-tmp7_;
double tmp9_ = sin(tmp6_);
double tmp10_ = -(tmp9_);
double tmp11_ = -(tmp5_);
double tmp12_ = cos(tmp6_);
double tmp13_ = mCompCoord[76];
double tmp14_ = mLocYTer-tmp13_;
double tmp15_ = mCompCoord[77];
double tmp16_ = mLocZTer-tmp15_;
double tmp17_ = ElSquare(mLocYIm);
double tmp18_ = ElSquare(mLocXIm);
double tmp19_ = VCube(mLocYIm);
double tmp20_ = VCube(mLocXIm);
double tmp21_ = VPow4(mLocYIm);
double tmp22_ = VPow4(mLocXIm);
double tmp23_ = VPow5(mLocYIm);
double tmp24_ = VPow5(mLocXIm);
double tmp25_ = VPow6(mLocYIm);
double tmp26_ = VPow6(mLocXIm);
mVal[0] = (tmp4_*tmp2_*(tmp8_)+tmp3_*tmp2_*(tmp14_)+tmp5_*(tmp16_))/((-(tmp3_)*tmp10_+tmp4_*tmp11_*tmp12_)*(tmp8_)+(tmp4_*tmp10_+tmp3_*tmp11_*tmp12_)*(tmp14_)+tmp2_*tmp12_*(tmp16_))-(mCompCoord[0]+mCompCoord[1]*mLocYIm+mCompCoord[2]*mLocXIm+mCompCoord[3]*tmp17_+mCompCoord[4]*mLocXIm*mLocYIm+mCompCoord[5]*tmp18_+mCompCoord[6]*tmp19_+mCompCoord[7]*mLocXIm*tmp17_+mCompCoord[8]*tmp18_*mLocYIm+mCompCoord[9]*tmp20_+mCompCoord[10]*tmp21_+mCompCoord[11]*mLocXIm*tmp19_+mCompCoord[12]*tmp18_*tmp17_+mCompCoord[13]*tmp20_*mLocYIm+mCompCoord[14]*tmp22_+mCompCoord[15]*tmp23_+mCompCoord[16]*mLocXIm*tmp21_+mCompCoord[17]*tmp18_*tmp19_+mCompCoord[18]*tmp20_*tmp17_+mCompCoord[19]*tmp22_*mLocYIm+mCompCoord[20]*tmp24_+mCompCoord[21]*tmp25_+mCompCoord[22]*mLocXIm*tmp23_+mCompCoord[23]*tmp18_*tmp21_+mCompCoord[24]*tmp20_*tmp19_+mCompCoord[25]*tmp22_*tmp17_+mCompCoord[26]*tmp24_*mLocYIm+mCompCoord[27]*tmp26_+mCompCoord[28]*VPow7(mLocYIm)+mCompCoord[29]*mLocXIm*tmp25_+mCompCoord[30]*tmp18_*tmp23_+mCompCoord[31]*tmp20_*tmp21_+mCompCoord[32]*tmp22_*tmp19_+mCompCoord[33]*tmp24_*tmp17_+mCompCoord[34]*tmp26_*mLocYIm+mCompCoord[35]*VPow7(mLocXIm));
}
void cAppliApero::AddOneObservationsRelGPS(const cObsRelGPS & aXMLObs)
{
cCompiledObsRelGPS * aCObs = mMCORelGps[aXMLObs.Id()];
const cGpsRelativeWeighting & aPond = aXMLObs.Pond();
ELISE_ASSERT(aCObs!=0,"cAppliApero::AddObservationsRelGPS cannot find id");
const std::vector<cPoseCam *> & aVP = aCObs->VOrderedPose();
const std::vector<cEqRelativeGPS *> & aVR = aCObs->VObs();
for (int aKR = 0 ; aKR < int(aVR.size()) ; aKR++)
{
cPoseCam * aPC1 = aVP[aKR];
cPoseCam * aPC2 = aVP[aKR+1];
cEqRelativeGPS * anObs = aVR[aKR];
Pt3dr aC1 = aPC1->ObsCentre();
Pt3dr aC2 = aPC2->ObsCentre();
Pt3dr aDif21 = aC2-aC1;
Pt3dr aResidu = anObs->Residu(aDif21);
if ((! aPond.MaxResidu().IsInit()) || (euclid(aResidu) < aPond.MaxResidu().Val()))
{
double aT1 = aPC1->Time();
double aT2 = aPC2->Time();
double aSigma = aPond.SigmaMin() + aPond.SigmaPerSec() * ElAbs(aT1-aT2);
Pt3dr aPds(1/ElSquare(aSigma),1/ElSquare(aSigma),1/ElSquare(aSigma));
anObs->AddObs(aDif21,aPds);
std::cout << "RELGPS " << aPC1->Name() << " " << aResidu
<< " D=" << euclid(aResidu)
<< " Sig0 " << aSigma<< "\n";
}
}
}
void L2SysSurResol::GSSR_AddEquationPoint3D(const Pt3dr & aP,const Pt3dr & anInc)
{
if (anInc.x > 0)
GSSR_AddEquationFitOneVar(0,aP.x,ElSquare(1/anInc.x));
if (anInc.y > 0)
GSSR_AddEquationFitOneVar(1,aP.y,ElSquare(1/anInc.y));
if (anInc.z > 0)
GSSR_AddEquationFitOneVar(2,aP.z,ElSquare(1/anInc.z));
}
REAL Pow(REAL x,INT i)
{
if (i>0)
return (i&1) ? (x * ElSquare(Pow(x,i/2))) : ElSquare(Pow(x,i/2));
if (i<0)
return 1.0 / Pow(x,-i);
return 1.0;
}
REAL SegComp::square_dist_demi_droite(Pt2dr pt) const
{
REAL a = abscisse(pt);
REAL o2 = ElSquare(ordonnee(pt));
if (a < 0)
return o2 + ElSquare(a);
else
return o2;
}
EHFS_ScoreGrad::EHFS_ScoreGrad
(
REAL Step,
REAL Width,
REAL LentghMax,
Im2D_INT1 ImGlobGX,
Im2D_INT1 ImGlobGY,
REAL CostChg ,
REAL EcarTeta,
REAL EcarMaxLoc,
REAL SeuilGrad
) :
ElHoughFiltSeg (Step,Width,LentghMax,Inf(ImGlobGX.sz(),ImGlobGY.sz())),
mImLocGX (SzMax().x,SzMax().y),
mImLocGY (SzMax().x,SzMax().y),
mImLocGRho (SzMax().x,SzMax().y),
mDataLocRho (mImLocGRho.data()),
mImLocGTeta (SzMax().x,SzMax().y),
mDataLocTeta (mImLocGTeta.data()),
mImYMaxLoc (SzMax().x),
mDataYMaxLoc (mImYMaxLoc.data()),
mImGlobGX (ImGlobGX),
mImGlobGY (ImGlobGY),
mCostChgt (CostChg),
//mEcarTeta (EcarTeta),
mPdsTeta (256),
mDataPdsTeta (mPdsTeta.data()),
//mEcarMaxLoc (EcarMaxLoc),
mPdsMaxLoc (SzMax().y),
mDataPdsMaxLoc (mPdsMaxLoc.data()),
//mSeuilGrad (SeuilGrad),
mPdsRho (256),
mDataPdsRho (mPdsRho.data()),
mGainIfSeg (SzMax().x),
mDataGainIfSeg (mGainIfSeg.data())
{
for (INT iTeta=0; iTeta<256 ; iTeta++)
{
REAL DeltaTeta = (iTeta%128) *PI /128.0 - PI/2;
DeltaTeta /= EcarTeta;
mDataPdsTeta[iTeta] = pow(0.5,ElSquare(DeltaTeta));
}
for (INT y=0 ; y<SzMax().y ; y++)
{
REAL dY = (y-mNbYMax) * mStep;
dY /= EcarMaxLoc;
mDataPdsMaxLoc[y] = pow(0.5,ElSquare(dY));
}
for (INT iRho=0 ; iRho<256 ; iRho++)
{
mDataPdsRho[iRho] = ElMin(1.0,iRho/SeuilGrad);
}
}
REAL cIncEnsembleCamera::ScoreCurGen(bool WithFaux,bool CumulDeriv)
{
SetOpt();
REAL mMoyResidu = 0.0;
REAL mNbPts = 0.0;
for
(
std::list<cIncSetLiaison *>::iterator anIt = mListSl->begin();
anIt != mListSl->end();
anIt ++
)
{
cIncCpleCamera * aCple = (*anIt)->Cple();
ElPackHomologue & aPack = (*anIt)->Pack();
for
(
ElPackHomologue::iterator itHoms = aPack.begin();
itHoms != aPack.end();
itHoms++
)
{
Pt2dr aP1 = itHoms->P1();
Pt2dr aP2 = itHoms->P2();
REAL aPds = itHoms->Pds();
REAL Residu = CumulDeriv ?
aCple->DevL1AddEqCoPlan(aP1,aP2,aPds,*mSys):
aCple->ValEqCoPlan(aP1,aP2);
mMoyResidu += ElSquare(Residu )* aPds;
mNbPts += aPds;
}
}
if (WithFaux)
{
for (INT aK = 0 ; aK<INT(mFoncsAux.size()) ; aK++)
{
cFonctrPond aFP = mFoncsAux[aK];
if (CumulDeriv)
StdAddEq(aFP.mFctr,aFP.mPds);
else
aFP.mFctr->ComputeValAndSetIVC();
mMoyResidu += ElSquare(aFP.mFctr->Val(0)) * aFP.mPds;
mNbPts += aFP.mPds;
}
}
return mMoyResidu / mNbPts;
}
void cEqAppui_PTInc_C2MDPol5::ComputeVal()
{
double tmp0_ = mCompCoord[42];
double tmp1_ = mCompCoord[43];
double tmp2_ = cos(tmp1_);
double tmp3_ = sin(tmp0_);
double tmp4_ = cos(tmp0_);
double tmp5_ = sin(tmp1_);
double tmp6_ = mCompCoord[44];
double tmp7_ = mCompCoord[48];
double tmp8_ = mCompCoord[45];
double tmp9_ = tmp7_-tmp8_;
double tmp10_ = sin(tmp6_);
double tmp11_ = -(tmp10_);
double tmp12_ = -(tmp5_);
double tmp13_ = cos(tmp6_);
double tmp14_ = mCompCoord[49];
double tmp15_ = mCompCoord[46];
double tmp16_ = tmp14_-tmp15_;
double tmp17_ = mCompCoord[50];
double tmp18_ = mCompCoord[47];
double tmp19_ = tmp17_-tmp18_;
double tmp20_ = ElSquare(mLocYIm);
double tmp21_ = ElSquare(mLocXIm);
double tmp22_ = VCube(mLocYIm);
double tmp23_ = VCube(mLocXIm);
double tmp24_ = VPow4(mLocYIm);
double tmp25_ = VPow4(mLocXIm);
double tmp26_ = -(tmp3_);
double tmp27_ = tmp4_*tmp12_;
double tmp28_ = tmp3_*tmp12_;
double tmp29_ = tmp26_*tmp11_;
double tmp30_ = tmp27_*tmp13_;
double tmp31_ = tmp29_+tmp30_;
double tmp32_ = (tmp31_)*(tmp9_);
double tmp33_ = tmp4_*tmp11_;
double tmp34_ = tmp28_*tmp13_;
double tmp35_ = tmp33_+tmp34_;
double tmp36_ = (tmp35_)*(tmp16_);
double tmp37_ = tmp32_+tmp36_;
double tmp38_ = tmp2_*tmp13_;
double tmp39_ = tmp38_*(tmp19_);
double tmp40_ = tmp37_+tmp39_;
double tmp41_ = VPow5(mLocYIm);
double tmp42_ = VPow5(mLocXIm);
mVal[0] = (tmp4_*tmp2_*(tmp9_)+tmp3_*tmp2_*(tmp16_)+tmp5_*(tmp19_))/(tmp40_)-(mCompCoord[0]+mCompCoord[1]*mLocYIm+mCompCoord[2]*mLocXIm+mCompCoord[3]*tmp20_+mCompCoord[4]*mLocXIm*mLocYIm+mCompCoord[5]*tmp21_+mCompCoord[6]*tmp22_+mCompCoord[7]*mLocXIm*tmp20_+mCompCoord[8]*tmp21_*mLocYIm+mCompCoord[9]*tmp23_+mCompCoord[10]*tmp24_+mCompCoord[11]*mLocXIm*tmp22_+mCompCoord[12]*tmp21_*tmp20_+mCompCoord[13]*tmp23_*mLocYIm+mCompCoord[14]*tmp25_+mCompCoord[15]*tmp41_+mCompCoord[16]*mLocXIm*tmp24_+mCompCoord[17]*tmp21_*tmp22_+mCompCoord[18]*tmp23_*tmp20_+mCompCoord[19]*tmp25_*mLocYIm+mCompCoord[20]*tmp42_);
mVal[1] = ((tmp26_*tmp13_+tmp27_*tmp10_)*(tmp9_)+(tmp4_*tmp13_+tmp28_*tmp10_)*(tmp16_)+tmp2_*tmp10_*(tmp19_))/(tmp40_)-(mCompCoord[21]+mCompCoord[22]*mLocYIm+mCompCoord[23]*mLocXIm+mCompCoord[24]*tmp20_+mCompCoord[25]*mLocXIm*mLocYIm+mCompCoord[26]*tmp21_+mCompCoord[27]*tmp22_+mCompCoord[28]*mLocXIm*tmp20_+mCompCoord[29]*tmp21_*mLocYIm+mCompCoord[30]*tmp23_+mCompCoord[31]*tmp24_+mCompCoord[32]*mLocXIm*tmp22_+mCompCoord[33]*tmp21_*tmp20_+mCompCoord[34]*tmp23_*mLocYIm+mCompCoord[35]*tmp25_+mCompCoord[36]*tmp41_+mCompCoord[37]*mLocXIm*tmp24_+mCompCoord[38]*tmp21_*tmp22_+mCompCoord[39]*tmp23_*tmp20_+mCompCoord[40]*tmp25_*mLocYIm+mCompCoord[41]*tmp42_);
}
void cEqHomogrX::ComputeValDeriv()
{
double tmp0_ = mCompCoord[6];
double tmp1_ = tmp0_*mLocXL1;
double tmp2_ = mCompCoord[7];
double tmp3_ = tmp2_*mLocYL1;
double tmp4_ = tmp1_+tmp3_;
double tmp5_ = tmp4_+1;
double tmp6_ = ElSquare(tmp5_);
double tmp7_ = mCompCoord[0];
double tmp8_ = tmp7_*mLocXL1;
double tmp9_ = mCompCoord[1];
double tmp10_ = tmp9_*mLocYL1;
double tmp11_ = tmp8_+tmp10_;
double tmp12_ = mCompCoord[2];
double tmp13_ = tmp11_+tmp12_;
double tmp14_ = mCompCoord[14];
double tmp15_ = tmp14_*mLocXL2;
double tmp16_ = mCompCoord[15];
double tmp17_ = tmp16_*mLocYL2;
double tmp18_ = tmp15_+tmp17_;
double tmp19_ = tmp18_+1;
double tmp20_ = ElSquare(tmp19_);
double tmp21_ = mCompCoord[8];
double tmp22_ = tmp21_*mLocXL2;
double tmp23_ = mCompCoord[9];
double tmp24_ = tmp23_*mLocYL2;
double tmp25_ = tmp22_+tmp24_;
double tmp26_ = mCompCoord[10];
double tmp27_ = tmp25_+tmp26_;
mVal[0] = (tmp13_)/(tmp5_)-(tmp27_)/(tmp19_);
mCompDer[0][0] = (mLocXL1*(tmp5_))/tmp6_;
mCompDer[0][1] = (mLocYL1*(tmp5_))/tmp6_;
mCompDer[0][2] = (tmp5_)/tmp6_;
mCompDer[0][3] = 0;
mCompDer[0][4] = 0;
mCompDer[0][5] = 0;
mCompDer[0][6] = -((tmp13_)*mLocXL1)/tmp6_;
mCompDer[0][7] = -((tmp13_)*mLocYL1)/tmp6_;
mCompDer[0][8] = -((mLocXL2*(tmp19_))/tmp20_);
mCompDer[0][9] = -((mLocYL2*(tmp19_))/tmp20_);
mCompDer[0][10] = -((tmp19_)/tmp20_);
mCompDer[0][11] = 0;
mCompDer[0][12] = 0;
mCompDer[0][13] = 0;
mCompDer[0][14] = -(-((tmp27_)*mLocXL2)/tmp20_);
mCompDer[0][15] = -(-((tmp27_)*mLocYL2)/tmp20_);
}
Pt2d< Fonc_Num> cDistRadialeFormelle::operator ()(Pt2d< Fonc_Num> aP)
{
Pt2d<Fonc_Num> fEc = aP - mFCentre;
Fonc_Num fRho2 = ElSquare(fEc.x) +ElSquare(fEc.y);
Fonc_Num fSom = 1;
Fonc_Num fRho2N = 1;
for (int aK=0 ; aK<(INT) mFCoeffs.size() ; aK++)
{
fRho2N = fRho2N * fRho2;
fSom = fSom + fRho2N * mFCoeffs[aK];
}
return mFCentre + Pt2d<Fonc_Num>(fEc.x*fSom,fEc.y*fSom);
}
REAL SimilariteEllipse(REAL A1,REAL B1,REAL C1,REAL A2,REAL B2,REAL C2)
{
return sqrt
(
(ElSquare(A1-A2) +2 *ElSquare(B1-B2)+ElSquare(C1-C2))
/ (
ElSquare(A1)/2 +ElSquare(B1)+ElSquare(C1)/2
+ ElSquare(A2)/2 +ElSquare(B2)+ElSquare(C2)/2
)
);
}
void cEqHomogFormelle::StdRepondere(ElPackHomologue & aPack,REAL aCoeff)
{
cElHomographie H1 = HF1().HomCur();
cElHomographie H2 = HF2().HomCur();
ElDistRadiale_PolynImpair Dist(1e5,Pt2dr(0,0));
if (DRF())
Dist = DRF()->DistCur();
for
(
ElPackHomologue::iterator it=aPack.begin();
it!=aPack.end();
it++
)
{
Pt2dr aP1 = it->P1();
Pt2dr aP2 = it->P2();
if (mDRF)
{
aP1 = Dist.Direct(aP1);
aP2 = Dist.Direct(aP2);
}
Pt2dr Ec = H1.Direct(Dist.Direct(it->P1())) - H2.Direct(Dist.Direct(it->P2()));
REAL D = euclid(Ec);
REAL Pds = 1 / (1 + ElSquare(D/aCoeff));
it->Pds() = Pds;
}
}
void Optim_L1FormLin::BenchRand(INT NbVar,INT NbForm,INT NbTest,bool Comb)
{
Optim_L1FormLin OLF = RandOLF(NbVar,NbForm);
ElMatrix<REAL> Sol = OLF.Solve();
REAL ScS = OLF.score(Sol);
for (;NbTest>=0; NbTest --)
{
REAL eps = std::max(1e-3,0.2*ElSquare(NRrandom3()));
ElMatrix<REAL> D = Sol;
for (INT k=0 ; k<NbVar ; k++)
D(0,k) += eps * (NRrandom3()-0.5);
REAL sd = OLF.score(D);
if (ScS> sd)
{
if (Comb)
OLF.BenchCombin(ScS);
cout << ScS << " " << sd
<< " " << ((ScS-sd)/eps)
<< " " << eps << "\n";
BENCH_ASSERT(std::abs((ScS-sd)/eps) < 1e-7);
}
}
}
void bench_Proj32()
{
Pt2di aSz(200,300);
Pt2di aC = aSz/2;
double aR = 50;
Fonc_Num aF = Square(FX-aC.x)+Square(FY-aC.y) < ElSquare(aR);
// aF = 0;
Im2D_INT2 aI1(aSz.x,aSz.y);
ELISE_COPY
(
aI1.all_pts(),
aF * Polar_Def_Opun::polar(Virgule(FX-aC.x,FY-aC.y),0).v1()*(255.0/(2.0*PI))
+ (1-aF) * (frandr() * 255),
aI1.out()
);
Video_Win aW= Video_Win::WStd(aSz,1.0);
ELISE_COPY(aI1.all_pts(),aI1.in(),aW.ocirc());
getchar();
cResProj32 aR32 = Projection32(aF,aSz);
ELISE_COPY
(
aI1.all_pts(),
aI1.in()[Virgule(aR32.PX().in(),aR32.PY().in())],
aW.ocirc()
);
getchar();
}
void cEqBBCamThird::ComputeValDeriv()
{
double tmp0_ = mCompCoord[8];
double tmp1_ = mCompCoord[7];
double tmp2_ = mCompCoord[4];
double tmp3_ = mCompCoord[6];
double tmp4_ = mCompCoord[3];
double tmp5_ = tmp4_*tmp1_;
double tmp6_ = tmp2_*tmp3_;
double tmp7_ = tmp5_-tmp6_;
double tmp8_ = tmp0_+tmp7_;
double tmp9_ = mCompCoord[2];
double tmp10_ = tmp9_+mLocCInit3_z;
double tmp11_ = (tmp8_)-(tmp10_);
double tmp12_ = tmp2_*tmp0_;
double tmp13_ = mCompCoord[5];
double tmp14_ = tmp13_*tmp1_;
double tmp15_ = tmp12_-tmp14_;
double tmp16_ = tmp3_+tmp15_;
double tmp17_ = mCompCoord[0];
double tmp18_ = tmp17_+mLocCInit3_x;
double tmp19_ = (tmp16_)-(tmp18_);
double tmp20_ = -(1);
double tmp21_ = ElSquare(tmp11_);
double tmp22_ = tmp20_*(tmp11_);
double tmp23_ = (tmp22_)/tmp21_;
double tmp24_ = -(tmp23_);
double tmp25_ = tmp13_*tmp3_;
double tmp26_ = tmp4_*tmp0_;
double tmp27_ = tmp25_-tmp26_;
double tmp28_ = tmp1_+tmp27_;
double tmp29_ = mCompCoord[1];
double tmp30_ = tmp29_+mLocCInit3_y;
double tmp31_ = (tmp28_)-(tmp30_);
double tmp32_ = -(tmp3_);
double tmp33_ = -(tmp2_);
mVal[0] = mLocI3_x-(tmp19_)/(tmp11_);
mCompDer[0][0] = tmp24_;
mCompDer[0][1] = 0;
mCompDer[0][2] = -(-((tmp19_)*tmp20_)/tmp21_);
mCompDer[0][3] = -(-((tmp19_)*tmp1_)/tmp21_);
mCompDer[0][4] = -((tmp0_*(tmp11_)-(tmp19_)*tmp32_)/tmp21_);
mCompDer[0][5] = -((-(tmp1_)*(tmp11_))/tmp21_);
mCompDer[0][6] = -((tmp11_-(tmp19_)*tmp33_)/tmp21_);
mCompDer[0][7] = -((-(tmp13_)*(tmp11_)-(tmp19_)*tmp4_)/tmp21_);
mCompDer[0][8] = -((tmp2_*(tmp11_)-(tmp19_))/tmp21_);
mVal[1] = mLocI3_y-(tmp31_)/(tmp11_);
mCompDer[1][0] = 0;
mCompDer[1][1] = tmp24_;
mCompDer[1][2] = -(-((tmp31_)*tmp20_)/tmp21_);
mCompDer[1][3] = -((-(tmp0_)*(tmp11_)-(tmp31_)*tmp1_)/tmp21_);
mCompDer[1][4] = -(-((tmp31_)*tmp32_)/tmp21_);
mCompDer[1][5] = -((tmp3_*(tmp11_))/tmp21_);
mCompDer[1][6] = -((tmp13_*(tmp11_)-(tmp31_)*tmp33_)/tmp21_);
mCompDer[1][7] = -((tmp11_-(tmp31_)*tmp4_)/tmp21_);
mCompDer[1][8] = -((-(tmp4_)*(tmp11_)-(tmp31_))/tmp21_);
}
void ElImplemDequantifier::TraitCuv ( U_INT2 * aDA,U_INT2 * aDB)
{
static std::vector<INT> Pts;
Pts.reserve(3000);
for (INT aP0=0; aP0<mNbPts ; aP0++)
{
if ((aDA[aP0] == eMaxDist) && (lDQ[aP0] != eValOut) )
{
Pts.push_back(aP0);
aDA[aP0] = eMaxDist-1;
INT aK = 0;
INT NbV = 0;
INT SigmaDV = 0;
INT aDmaxB = 0;
while (aK != INT (Pts.size()))
{
INT aP = Pts[aK];
ElSetMax(aDmaxB,aDB[aP]);
for (INT aKV =0 ; aKV < mNbVYT ; aKV++)
{
INT aPV = aP + mVYT[aKV];
if (lDQ[aPV] == eValOut)
{
}
else if (aDA[aPV] == eMaxDist)
{
Pts.push_back(aPV);
aDA[aPV] = eMaxDist-1;
}
else if (aDA[aPV] < eMaxDist-1)
{
NbV++;
SigmaDV += aDB[aPV];
}
}
aK++;
}
REAL DMoy = SigmaDV/REAL( NbV);
for (aK =0 ; aK<INT(Pts.size()) ; aK++)
{
INT aP = Pts[aK];
INT aD = aDB[aP];
REAL aPds = (2*aD*aDmaxB - ElSquare(aD))/REAL (aDmaxB*(aDmaxB+DMoy));
aDB[aP] = round_ni(aPds*10000);
aDA[aP] = 10000 - aDB[aP];
}
Pts.clear();
}
}
}
Fonc_Num cImage_LumRas::FMoy(int aNbIter,int aSzW,Fonc_Num aF)
{
Fonc_Num aRes = aF;
for (int aK=0 ; aK<aNbIter; aK++)
aRes = rect_som(aF,aSzW) / ElSquare(1.0+2*aSzW);
return aRes;
}
REAL SegComp::square_dist_seg(Pt2dr pt) const
{
REAL a = abscisse(pt);
REAL o2 = ElSquare(ordonnee(pt));
if (a < 0)
{
return o2 + ElSquare(a);
}
else if (a < _a1)
{
return o2;
}
else
{
return o2 + ElSquare(a-_a1);
}
}
Fonc_Num gauss_noise_1(INT nb)
{
REAL nb_pts = ElSquare(2*nb+1);
REAL moy = 0.5;
REAL ect = 1 / sqrt(12*nb_pts);
return rect_som(frandr()-moy,nb)/(nb_pts*ect) ;
}
void cMA_AffineOrient::TestOri1Ori2(bool ShowAll,CamStenope & anOri1,CamStenope & anOri2)
{
double aSEc2 =0;
double aSEc =0;
double aSP =0;
for
(
ElPackHomologue::iterator iT = mPack.begin();
iT != mPack.end();
iT++
)
{
Pt2dr aQ1 = mCam1.F2toPtDirRayonL3(iT->P1());
Pt2dr aQ2 = mCam2.F2toPtDirRayonL3(iT->P2());
double aL = mCpl12->ResiduSigneP1P2(aQ1,aQ2);
aL = ElAbs(aL*mFocale);
double aD;
anOri1.PseudoInter(iT->P1(),anOri2,iT->P2(),&aD);
aSEc2 += ElSquare(aD)*iT->Pds();
aSEc += ElAbs(aD)*iT->Pds();
aSP += iT->Pds();
// double aRatio = aL/aD;
// Pt2dr aP1 = mGeoTer.R2ToRDisc(iT->P1());
// Pt2di aI1 = round_down(aP1/mPas);
/*
{
if (ShowAll) std::cout << aL << aP << "\n";
<< iT->Pds() << " "
<< iT->P1() << " "
<< iT->P2() << " "
<< " RATIO = " << aL/aD << " "
<< " Ec Ang " << aL << " "
<< " D INTER = " << aD << "\n";
}
*/
}
std::cout << "EC2 = " << sqrt(aSEc2/aSP)
<< " ECAbs = " << (aSEc/aSP) << "\n";
for
(
std::list<cListTestCpleHomol>::iterator itC=mAppli.ListTestCpleHomol().begin();
itC !=mAppli.ListTestCpleHomol().end();
itC++
)
{
// Pt2dr aP1 = itC->PtIm1();
// Pt2dr aP2 = itC->PtIm2();
double aD;
Pt3dr aP = anOri1.PseudoInter(itC->PtIm1(),anOri2,itC->PtIm2(),&aD);
std::cout << "VERIF " << aP << " " << aD << "\n";
}
}
Pt3dr ElSeg3D::L2InterFaisceaux
(
const std::vector<double> * aVPds,
const std::vector<ElSeg3D> & aVS,
bool * aOK,
const cRapOnZ * aRAZ ,
cResOptInterFaisceaux * aROIF,
const std::vector<Pt3dr> * aVPts
)
{
if (! aVPts)
{
ELISE_ASSERT(int(aVS.size())>=2,"ElSeg3D::L2InterFaisceaux pas assez de pts");
}
static L2SysSurResol aSys(3);
aSys.GSSR_Reset(false);
for (int aKS=0; aKS<int(aVS.size()); aKS++)
{
double aPds = ((aVPds!=0) ? (*aVPds)[aKS] : 1.0);
if (aPds > 0)
{
aSys.GSSR_Add_EqInterDroite3D (aVS[aKS].TgNormee(),aVS[aKS].P0(),aPds);
}
}
if (aRAZ)
{
double aCoeff[3]={0,0,1};
aSys.AddEquation(1/ElSquare(aRAZ->IncEstim()),aCoeff,aRAZ->Z());
}
if (aVPts)
{
int aNb = aVPts->size();
ELISE_ASSERT((aNb%2)==0,"ElSeg3D::L2InterFaisceaux aVPts impair !!!");
for (int aK=0 ; aK<aNb ; aK+=2)
{
aSys.GSSR_AddEquationPoint3D((*aVPts)[aK],(*aVPts)[aK+1]);
}
}
if (aROIF)
{
aROIF->Init(aSys.tLi_Li().ToMatrix());
}
return aSys.Solve3x3Sym(aOK);
/*
Pt3dr aP0 = aSys.Solve3x3Sym(aOK);
Pt3dr aP1 = aSys.Pt3dSolInter(aOK);
ELISE_ASSERT(euclid(aP0-aP1)<1e-5,"aSys.Solve3x3Sym/aSys.Pt3dSolInter");
return aSys.Pt3dSolInter(aOK);
*/
}
void cGen2DBundleAtRot_Deg3::ComputeVal()
{
double tmp0_ = mLocRotPt_y-mLocCentrAttR_y;
double tmp1_ = (tmp0_)/mLocAmplAttR;
double tmp2_ = mLocRotPt_x-mLocCentrAttR_x;
double tmp3_ = (tmp2_)/mLocAmplAttR;
double tmp4_ = ElSquare(tmp1_);
double tmp5_ = ElSquare(tmp3_);
double tmp6_ = VCube(tmp1_);
double tmp7_ = VCube(tmp3_);
double tmp8_ = mCompCoord[0];
double tmp9_ = mCompCoord[1];
double tmp10_ = mCompCoord[2];
mVal[0] = (mCompCoord[3]+mCompCoord[4]*(tmp1_)+mCompCoord[5]*(tmp3_)+mCompCoord[6]*tmp4_+mCompCoord[7]*(tmp3_)*(tmp1_)+mCompCoord[8]*tmp5_+mCompCoord[9]*tmp6_+mCompCoord[10]*(tmp3_)*tmp4_+mCompCoord[11]*tmp5_*(tmp1_)+mCompCoord[12]*tmp7_)-mLocDepR1_x*tmp8_-mLocDepR2_x*tmp9_-mLocDepR3_x*tmp10_;
mVal[1] = (mCompCoord[13]+mCompCoord[14]*(tmp1_)+mCompCoord[15]*(tmp3_)+mCompCoord[16]*tmp4_+mCompCoord[17]*(tmp3_)*(tmp1_)+mCompCoord[18]*tmp5_+mCompCoord[19]*tmp6_+mCompCoord[20]*(tmp3_)*tmp4_+mCompCoord[21]*tmp5_*(tmp1_)+mCompCoord[22]*tmp7_)-mLocDepR1_y*tmp8_-mLocDepR2_y*tmp9_-mLocDepR3_y*tmp10_;
}
void cEqTrianguApparImage::Close()
{
mSI1 /= mN;
mSI2 /= mN;
mSigmaI = sqrt(mSI2-ElSquare(mSI1));
mSJ1 /= mN;
mSJ2 /= mN;
mSigmaJ2 = mSJ2-ElSquare(mSJ1);
mSigmaJ = sqrt(mSigmaJ2);
mSigmaJ3 = mSigmaJ2 * mSigmaJ;
for (INT aK6=0 ; aK6<6 ; aK6++)
{
mDerSJ1[aK6] /= mN;
mDerSJ2[aK6] *= 2.0/ mN;
mDerSigmaJ2[aK6] = mDerSJ2[aK6] -2*mDerSJ1[aK6] *mSJ1;
}
}
template <class Type,class TBase> void cElNuage3DMaille_FromImProf<Type,TBase>::ProfBouchePPV()
{
Im2D<Type,TBase> aIPPV = BouchePPV(mIm,ImDef().in());
int aNbTest = 7;
for (int aK=0 ; aK< (aNbTest+2) ; aK++)
{
Symb_FNum aFMasq = ImDef().in();
int aSzV = ElMax(1,ElSquare(aNbTest-aK));
Fonc_Num aFLisse = rect_som(aIPPV.in_proj(),aSzV) / ElSquare(1+2*aSzV);
aFLisse = aFLisse*(! aFMasq) + mIm.in() * aFMasq;
ELISE_COPY(aIPPV.all_pts(),aFLisse,aIPPV.out());
}
ELISE_COPY(aIPPV.all_pts(),aIPPV.in(),mIm.out());
ELISE_COPY(ImDef().all_pts(),1,ImDef().out());
}
tSysCho cGenSysSurResol::SomQuad() const
{
tSysCho aRes = 0;
for (int aKx=0 ; aKx<NbVar(); aKx++)
{
for (int aKy=0 ; aKy<NbVar(); aKy++)
{
aRes = ElSquare(GetElemQuad(aKx,aKy));
}
}
return aRes;
}
