ElPackHomologue PackFromCplAPF(const cMesureAppuiFlottant1Im & aMes, const cMesureAppuiFlottant1Im & aRef)
{
ElPackHomologue aRes;
for
(
std::list<cOneMesureAF1I>::const_iterator iT=aMes.OneMesureAF1I().begin();
iT!=aMes.OneMesureAF1I().end();
iT++
)
{
const cOneMesureAF1I * aPt = PtsOfName(aRef,iT->NamePt());
if (aPt!=0)
{
aRes.Cple_Add(ElCplePtsHomologues(iT->PtIm(),aPt->PtIm()));
}
else
{
std::cout << "For name " << iT->NamePt() << "\n";
ELISE_ASSERT(false,"Cannot get name in merging two cMesureAppuiFlottant1Im");
}
}
return aRes;
}
REAL cLEqHomOneDist::AddAllEquationsToSet()
{
mSet.AddContrainte(mDRF->StdContraintes(),true);
for (std::vector<cHomogFormelle *>::iterator itH=mHomFs.begin();itH!=mHomFs.end() ;itH++)
{
mSet.AddContrainte((*itH)->StdContraintes(),true);
}
REAL Res=0,Cpt=0;
for (INT aK=0; aK<INT(mEqFs.size()) ; aK++)
{
cEqHomogFormelle * pEq = mEqFs[aK];
ElPackHomologue * pPack = mLiaisons[aK];
for
(
ElPackHomologue::const_iterator it= pPack->begin();
it!= pPack->end();
it++
)
{
REAL Ecart = pEq->AddLiaisonP1P2(it->P1(),it->P2(),it->Pds(),mDerSec) ;
Res += std::abs(Ecart);
Cpt ++;
}
}
return Res/Cpt;
}
void NewOri_Info1Cple
(
const ElCamera & aCam1,
const ElPackHomologue & aPack12,
const ElCamera & aCam2,const ElPackHomologue & aPack21
)
{
cStructMergeTieP< cFixedSizeMergeTieP<2,Pt2dr> > aMap2(2,false);
NOMerge_AddPackHom(aMap2,aPack12,aCam1,0,aCam2,1);
NOMerge_AddPackHom(aMap2,aPack21,aCam2,1,aCam1,0);
aMap2.DoExport();
std::list<cFixedSizeMergeTieP<2,Pt2dr> *> aRes = aMap2.ListMerged();
int aNb1=0;
int aNb2=0;
for (std::list<cFixedSizeMergeTieP<2,Pt2dr> *>::const_iterator itR=aRes.begin() ; itR!=aRes.end() ; itR++)
{
if ((*itR)->NbArc() ==1)
{
aNb1++;
}
else if ((*itR)->NbArc() ==2)
{
aNb2++;
}
else
{
ELISE_ASSERT(false,"NO_MergeTO_Test2_0");
}
}
std::cout << "INPUT " << aPack12.size() << " " << aPack21.size() << " Exp " << aNb1 << " " << aNb2 << "\n";
}
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;
}
}
template <const int TheNb> void NOMerge_AddPackHom
(
cStructMergeTieP< cFixedSizeMergeTieP<TheNb,Pt2dr> > & aMap,
const ElPackHomologue & aPack,
const ElCamera * aCam1,int aK1,
const ElCamera * aCam2,int aK2
)
{
for
(
ElPackHomologue::tCstIter itH=aPack.begin();
itH !=aPack.end();
itH++
)
{
ElCplePtsHomologues aCple = itH->ToCple();
Pt2dr aP1 = aCple.P1();
Pt2dr aP2 = aCple.P2();
if (aCam1)
{
aP1 = ProjStenope(aCam1->F2toDirRayonL3(aP1));
}
if (aCam2)
{
aP2 = ProjStenope(aCam2->F2toDirRayonL3(aP2));
}
aMap.AddArc(aP1,aK1,aP2,aK2);
}
}
std::string cInterfChantierNameManipulateur::NamePackWithAutoSym
(
const std::string & aKey,
const std::string & aName1,
const std::string & aName2,
bool aSVP
)
{
std::string aN12_SsDir = Assoc1To2(aKey,aName1,aName2,true);
std::string aN12 = mDir+aN12_SsDir;
if (! ELISE_fp::exist_file(aN12))
{
std::string aN21 =mDir+ Assoc1To2(aKey,aName2,aName1,true);
if (! ELISE_fp::exist_file(aN21))
{
if (aSVP)
return aN12_SsDir;
std::cout << "For K=" << aKey
<< " N1=" << aName1
<< " N2=" << aName2 << "\n";
std::cout << aN12 << "\n";
std::cout << aN21 << "\n";
ELISE_ASSERT(false,"Ni fichier homoloque ni symetrique n'existe");
}
ElPackHomologue aPack = ElPackHomologue::FromFile(aN21);
aPack.SelfSwap();
aPack.StdPutInFile(aN12);
}
return aN12_SsDir;
}
void cAppliApero::SimuleOneLiaison
(
const cGenerateLiaisons & aGL,
const std::list<Pt3dr> & aLP3,
cPoseCam & aCam1,
cPoseCam & aCam2
)
{
const CamStenope * aCS1 = aCam1.CurCam();
const CamStenope * aCS2 = aCam2.CurCam();
ElPackHomologue aPack;
for(std::list<Pt3dr>::const_iterator itP3=aLP3.begin();itP3!=aLP3.end();itP3++)
{
Pt2dr aPIm1 = aCS1->R3toF2(*itP3)+Pt2dr(NRrandC(),NRrandC())*aGL.BruitIm1();
Pt2dr aPIm2 = aCS2->R3toF2(*itP3)+Pt2dr(NRrandC(),NRrandC())*aGL.BruitIm2();
aPack.Cple_Add(ElCplePtsHomologues(aPIm1,aPIm2));
}
std::string aName = mDC+mICNM->Assoc1To2(aGL.KeyAssoc(),aCam1.Name(),aCam2.Name(),true);
cElXMLFileIn aFileXML(aName);
aFileXML.PutPackHom(aPack);
}
REAL cEqFPtLiaison::AddPackLiaisonP1P2
(
const ElPackHomologue & aPack,
bool WithD2,
cElStatErreur * aStat,
REAL aPdsGlob ,
REAL * SomPdsTot
)
{
REAL aRes = 0.0;
REAL aSPds = 0.0;
for
(
ElPackHomologue::const_iterator it=aPack.begin();
it!=aPack.end();
it++
)
{
REAL aP = aPdsGlob * it->Pds();
if (SomPdsTot)
*SomPdsTot += aP;
REAL Ecart = ElAbs(AddLiaisonP1P2(it->P1(),it->P2(),aP,WithD2));
aRes += Ecart * aP;
aSPds += aP;
if (aStat)
aStat->AddErreur(Ecart);
}
return aRes / aSPds;
}
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";
}
}
cElHomographie cElHomographie::RansacInitH(const ElPackHomologue & aPack,int aNbRansac,int aNbMaxPts)
{
if ((aPack.size()<10) || (aNbMaxPts<4))
return cElHomographie(aPack,false);
cRandNParmiQ aRand(aNbMaxPts,aPack.size());
std::vector<ElCplePtsHomologues> aVCH;
for (ElPackHomologue::tCstIter itH=aPack.begin() ; itH!=aPack.end() ; itH++)
{
if (aRand.GetNext())
{
aVCH.push_back(itH->ToCple());
}
}
double anEcMin = 1e30;
cElHomographie aHMin = cElHomographie::Id();
while (aNbRansac)
{
int aK1 = NRrandom3(aVCH.size());
int aK2 = NRrandom3(aVCH.size());
int aK3 = NRrandom3(aVCH.size());
int aK4 = NRrandom3(aVCH.size());
if (
(aK1!=aK2) && (aK1!=aK3) && (aK1!=aK4)
&& (aK2!=aK3) && (aK2!=aK4)
&& (aK3!=aK4)
)
{
aNbRansac--;
ElPackHomologue aP4;
aP4.Cple_Add(aVCH[aK1]);
aP4.Cple_Add(aVCH[aK2]);
aP4.Cple_Add(aVCH[aK3]);
aP4.Cple_Add(aVCH[aK4]);
cElHomographie aSol = cElHomographie(aP4,true);
double anEcart = 0;
for (int aKP=0 ; aKP<int(aVCH.size()) ; aKP++)
{
anEcart += euclid(aSol.Direct(aVCH[aKP].P1()),aVCH[aKP].P2());
}
anEcart /= aVCH.size();
if (anEcart<anEcMin)
{
anEcMin = anEcart;
aHMin = aSol;
}
}
}
std::cout << "ECART H " << anEcMin << "\n";
return aHMin;
}
double cLiaisORPO::Residu()
{
SetOrCam();
if (! mProfCalc)
{
mProfCalc = true;
SetProf();
}
// ElRotation3D aC1toM = mCam1.Orientation().inv();
// ElRotation3D aMtoC2 = mCam2.Orientation();
ElRotation3D aC1toC2 = mCam2.Orient() * mCam1.Orient().inv();
Pt3dr aBaseInC2 = aC1toC2.ImAff(Pt3dr(0,0,0));
ElMatrix<double> aMat = aC1toC2.Mat() ;
Pt3dr aC0,aC1,aC2;
aMat.GetCol(0,aC0);
aMat.GetCol(1,aC1);
aMat.GetCol(2,aC2);
double sDist=0.0;
int aK = 0;
for (ElPackHomologue::const_iterator itP =mPack.begin() ; itP!=mPack.end(); itP++)
{
double aPds = itP->Pds();
double aLambda = mProfs.at(aK);
Pt2dr aPIm1 = itP->P1();
Pt3dr aRay1 = aC0 * aPIm1.x + aC1 * aPIm1.y + aC2;
Pt3dr aPTerA = aBaseInC2 + aRay1 * (aLambda *0.99);
Pt3dr aPTerB = aBaseInC2 + aRay1 * (aLambda *1.01);
Pt2dr aProjA(aPTerA.x/aPTerA.z,aPTerA.y/aPTerA.z);
Pt2dr aProjB(aPTerB.x/aPTerB.z,aPTerB.y/aPTerB.z);
Pt2dr aVAB (aProjB.y-aProjA.y,aProjA.x-aProjB.x);
double aD2 = ElAbs(scal(aVAB,aProjA-itP->P2()) / euclid(aVAB));
sDist += aPds*aD2;
aK++;
}
if (DEBUG_POWEL)
{
std::cout << sDist/mPack.size() << "\n"; // getchar();
}
return sDist * mPPL->Pds().Val();
}
void cModeleAnalytiqueComp::SauvHomologues(const ElPackHomologue & aPack)
{
TifSauvHomologues(aPack);
if (mModele.AutomNamesExportHomXml().IsInit())
{
AssertAutomSelExportOriIsInit(mModele);
std::string aNameXML = StdNameFromCple
(
mAutomExport,
mModele.AutomSelExportOri().Val(),
mModele.AutomNamesExportHomXml().Val(),
"@",
mAppli.PDV1()->Name(),
mAppli.PDV2()->Name()
);
cElXMLFileIn aFileXML(mAppli.FullDirResult()+aNameXML);
aFileXML.PutPackHom(aPack);
}
if (mModele.KeyNamesExportHomXml().IsInit())
{
std::string aNameXML = mAppli.ICNM()->Assoc1To2
(
mModele.KeyNamesExportHomXml().Val(),
mAppli.PDV1()->Name(),
mAppli.PDV2()->Name(),
true
);
// cElXMLFileIn aFileXML(mAppli.WorkDir()+aNameXML);
// aFileXML.PutPackHom(aPack);
aPack.StdPutInFile(mAppli.WorkDir()+aNameXML);
}
if (mModele.AutomNamesExportHomBin().IsInit())
{
AssertAutomSelExportOriIsInit(mModele);
std::string aNameBin = mAppli.FullDirResult()
+ StdNameFromCple
(
mAutomExport,
mModele.AutomSelExportOri().Val(),
mModele.AutomNamesExportHomBin().Val(),
"@",
mAppli.PDV1()->Name(),
mAppli.PDV2()->Name()
);
ELISE_fp aFP (aNameBin.c_str(),ELISE_fp::WRITE);
aPack.write(aFP);
aFP.close();
}
}
void cLiaisORPO::SetProf()
{
mProfs.clear();
Pt3dr aC1 = mCam1.PseudoOpticalCenter();
for(ElPackHomologue::iterator itP=mPack.begin();itP!=mPack.end();itP++)
{
Pt3dr aPTer = mCam1.PseudoInter(itP->P1(),mCam2,itP->P2());
mProfs.push_back
(
scal(aPTer-aC1,mCam1.F2toDirRayonR3(itP->P1()))
);
}
}
void cEqFPtLiaison::PondereFromResidu
(ElPackHomologue & aPack,REAL Ecart,REAL anEcCoupure)
{
for
(
ElPackHomologue::iterator it=aPack.begin();
it!=aPack.end();
it++
)
{
REAL aResidu = ResiduNonSigneP1P2(it->P1(),it->P2());
it->Pds() = 1 / (1+ElSquare(aResidu/Ecart));
if ((anEcCoupure > 0) && (aResidu > anEcCoupure))
it->Pds() = 0;
}
}
void cOneRotPowelOptimize::InitPts()
{
for
(
std::list<cPowPointLiaisons>::const_iterator itP=mOP->PowPointLiaisons().begin();
itP!=mOP->PowPointLiaisons().end();
itP++
)
{
std::list<cPoseCam *> aLCam = mAppli->ListCamInitAvecPtCom(itP->Id(),mCam);
// Pour l'insntant la selec se fait par simple tirage aleatoire
int aNbPtsTot = 0;
for (std::list<cPoseCam *>::iterator itC=aLCam.begin();itC!=aLCam.end();itC++)
{
ElPackHomologue aPack ;
mAppli->InitPack(itP->Id(),aPack,mCam->Name(),(*itC)->Name());
aNbPtsTot += aPack.size();
}
int aNbCible = itP->NbTot();
for (std::list<cPoseCam *>::iterator itC=aLCam.begin();itC!=aLCam.end();itC++)
{
mLIais.push_back(cLiaisORPO(*itP,mCam,*itC));
ElPackHomologue aPack ;
mAppli->InitPackPhgrm
(
itP->Id(),aPack,
mCam->Name(),&mCam->Calib()->CamInit(),
(*itC)->Name(),&(*itC)->Calib()->CamInit()
);
for (ElPackHomologue::iterator itPt=aPack.begin(); itPt!=aPack.end() ; itPt++)
{
if (NRrandom3() < double(aNbCible)/aNbPtsTot)
{
mLIais.back().Pack().Cple_Add(itPt->ToCple());
mNbMes++;
aNbCible--;
}
aNbPtsTot--;
}
}
}
}
Pt2dr cApply_CreateEpip_main::DirEpipIm2(cBasicGeomCap3D * aG1,cBasicGeomCap3D * aG2,ElPackHomologue & aPack,bool AddToP1)
{
Pt2dr aSz = Pt2dr(aG1->SzBasicCapt3D());
Pt2dr aSomTens2(0,0);
double aIProf = aG1->GetVeryRoughInterProf();
double aEps = 5e-4;
double aLenghtSquare = ElMin(mLengthMin,sqrt((aSz.x*aSz.y) / (mNbXY*mNbXY)));
int aNbX = ElMax(1+3*mDegre,round_up(aSz.x /aLenghtSquare));
int aNbY = ElMax(1+3*mDegre,round_up(aSz.y /aLenghtSquare));
std::cout << "NBBBB " << aNbX << " " << aNbY << "\n";
for (int aKX=0 ; aKX<= aNbX ; aKX++)
{
double aPdsX = ElMax(aEps,ElMin(1-aEps,aKX /double(aNbX)));
for (int aKY=0 ; aKY<= aNbY ; aKY++)
{
double aPdsY = ElMax(aEps,ElMin(1-aEps,aKY/double(aNbY)));
Pt2dr aPIm1 = aSz.mcbyc(Pt2dr(aPdsX,aPdsY));
if (aG1->CaptHasData(aPIm1))
{
Pt3dr aPT1;
Pt3dr aC1;
aG1->GetCenterAndPTerOnBundle(aC1,aPT1,aPIm1);
std::vector<Pt2dr> aVPIm2;
for (int aKZ = -mNbZ ; aKZ <= mNbZ ; aKZ++)
{
Pt3dr aPT2 = aC1 + (aPT1-aC1) * (1+(aIProf*aKZ) / mNbZ);
if (aG1->PIsVisibleInImage(aPT2) && aG2->PIsVisibleInImage(aPT2))
{
aVPIm2.push_back(aG2->Ter2Capteur(aPT2));
ElCplePtsHomologues aCple(aPIm1,aVPIm2.back(),1.0);
if (! AddToP1)
aCple.SelfSwap();
aPack.Cple_Add(aCple);
}
}
if (aVPIm2.size() >=2)
{
Pt2dr aDir2 = vunit(aVPIm2.back()-aVPIm2[0]);
aSomTens2 = aSomTens2 + aDir2 * aDir2; // On double l'angle pour en faire un tenseur
}
}
}
}
Pt2dr aRT = Pt2dr::polar(aSomTens2,0.0);
return Pt2dr::FromPolar(1.0,aRT.y/2.0);
}
Pt3dr cMA_AffineOrient::CalcPtMoy(CamStenope & anOri1,CamStenope & anOri2)
{
Pt3dr aSPt(0,0,0);
double aSPds =0.0;
for
(
ElPackHomologue::iterator iT = mPack.begin();
iT != mPack.end();
iT++
)
{
double aD;
Pt3dr aP = anOri1.PseudoInter(iT->P1(),anOri2,iT->P2(),&aD);
aSPds += iT->Pds();
aSPt = aSPt + aP * iT->Pds();
}
return aSPt/aSPds;
}
void cAppli_GenPTripleOneImage::AddOnePackOneSens(cFixedMergeStruct<2,Pt2df> & aMap,cNewO_OneIm * anI1,int anIndI1,cNewO_OneIm * anI2)
{
ElPackHomologue aPack = mNM->PackOfName(anI1->Name(),anI2->Name());
CamStenope * aCS1 = anI1->CS();
CamStenope * aCS2 = anI2->CS();
for (ElPackHomologue::const_iterator itP=aPack.begin(); itP!=aPack.end() ; itP++)
{
Pt2dr aP1 = aCS1->F2toPtDirRayonL3(itP->P1());
Pt2dr aP2 = aCS2->F2toPtDirRayonL3(itP->P2());
Pt2df aQ1(aP1.x,aP1.y);
Pt2df aQ2(aP2.x,aP2.y);
// if (aSwap) ElSwap(aQ1,aQ2);
aMap.AddArc(aQ1,anIndI1,aQ2,1-anIndI1);
}
}
bool cPackObsLiaison::InitPack
(
ElPackHomologue & aPack,
const std::string& aNN1,
const std::string& aNN2
)
{
std::string aN1 = mAppli.PoseFromName(aNN1)->Name();
std::string aN2 = mAppli.PoseFromName(aNN2)->Name();
if (mIsMult)
{
if (DicBoolFind(mDicoMul,aN1) && mDicoMul[aN1]->InitPack(aPack,aN2))
{
return false;
}
if (DicBoolFind(mDicoMul,aN2) && mDicoMul[aN2]->InitPack(aPack,aN1))
{
aPack.SelfSwap();
return true;
}
}
else
{
cObservLiaison_1Cple * aO2 = mDicObs[aN1][aN2];
if (aO2)
{
aPack = aO2->Pack();
return false;
}
aO2 = mDicObs[aN2][aN1];
if (aO2)
{
aPack = aO2->Pack();
aPack.SelfSwap();
return true;
}
}
std::cout << " For : " << mId << " " << aN1 << " " << aN2 << "\n";
std::cout << " Mult " << mIsMult << "\n";
ELISE_ASSERT(false,"Cannot find liaison");
return false;
}
bool cModeleAnalytiqueComp::FiltragePointHomologues
(
const ElPackHomologue & aPackInit,
ElPackHomologue & aNewPack,
double aTol,
double aFiltre
)
{
bool GotOut = false;
Box2dr aBox1 = mAppli.PDV1()->BoxIm().AddTol(aTol);
Box2dr aBox2 = mAppli.PDV2()->BoxIm().AddTol(aTol);
Box2dr aBoxF1 = mAppli.PDV1()->BoxIm().AddTol(aFiltre);
Box2dr aBoxF2 = mAppli.PDV2()->BoxIm().AddTol(aFiltre);
for
(
ElPackHomologue::const_iterator iT = aPackInit.begin();
iT != aPackInit.end();
iT++
)
{
if ( ( aBoxF1.inside(iT->P1()))
&& ( aBoxF2.inside(iT->P2()))
)
{
bool thisOut = (! aBox1.inside(iT->P1()))
|| (! aBox2.inside(iT->P2()));
if (thisOut)
{
std::cout << aBox1._p1 << aBox2._p1 << "\n";
std::cout << "OUT " << iT->P1() << " " << iT->P2() << " " << iT->Pds()<< "\n";
}
GotOut = GotOut || thisOut;
aNewPack.Cple_Add(iT->ToCple());
}
}
return GotOut;
}
void cLEqHomOneDist::PondereFromErreur(REAL aDCut)
{
for (INT aK=0; aK<INT(mEqFs.size()) ; aK++)
{
cEqHomogFormelle * pEq = mEqFs[aK];
ElPackHomologue * pPack = mLiaisons[aK];
ElDistRadiale_PolynImpair aDist(1e5,Pt2dr(0,0));
if (pEq->DRF())
aDist = pEq->DRF()->DistCur();
cElHomographie aH1 = pEq->HF1().HomCur();
cElHomographie aH2 = pEq->HF2().HomCur();
for
(
ElPackHomologue::iterator it= pPack->begin();
it!= pPack->end();
it++
)
{
Pt2dr aQ1 = it->P1();
Pt2dr aQ2 = it->P2();
if (pEq->DRF())
{
aQ1 = aDist.Direct(aQ1);
aQ2 = aDist.Direct(aQ2);
}
Pt2dr aP1 = aH1.Direct(aQ1);
Pt2dr aP2 = aH2.Direct(aQ2);
REAL aD = euclid(aP1,aP2) * mDiag;
REAL aPds = 1 / (1+ElSquare(aD/aDCut));
it->Pds() = aPds;
}
}
}
ElMatrix<REAL> GlobMepRelCocentrique(double & anEcartMin,const ElPackHomologue & aPack, int aNbRansac,int aNbMaxPts)
{
aNbMaxPts = ElMin(aNbMaxPts,aPack.size());
std::vector<Pt3dr> aVDir1;
std::vector<Pt3dr> aVDir2;
cRandNParmiQ aRand(aNbMaxPts,aPack.size());
for (ElPackHomologue::tCstIter itH=aPack.begin() ; itH!=aPack.end() ; itH++)
{
if (aRand.GetNext())
{
aVDir1.push_back(vunit(PZ1(itH->P1())));
aVDir2.push_back(vunit(PZ1(itH->P2())));
}
}
ElMatrix<REAL> aRes(3,3);
anEcartMin = 1e60;
while (aNbRansac)
{
int aKA = NRrandom3(aVDir1.size());
int aKB = NRrandom3(aVDir2.size());
if (aKA!=aKB)
{
aNbRansac--;
ElMatrix<REAL> aMat = ComplemRotation(aVDir1[aKA],aVDir1[aKB],aVDir2[aKA],aVDir2[aKB]);
double anEc = SomEcartDist(aMat,aVDir1,aVDir2);
if (anEc<anEcartMin)
{
anEcartMin = anEc;
aRes = aMat;
}
}
}
return aRes;
}
void cLEqHomOneDist::AddCple(const ElPackHomologue & aPack)
{
cElHomographie aH12(aPack,true);
cElHomographie aH2 = aH12.Inverse();
cHomogFormelle * aHF1 = mSet.NewHomF(cElHomographie::Id(),cNameSpaceEqF::eHomFigee);
cHomogFormelle * aHF2 = mSet.NewHomF(aH2,cNameSpaceEqF::eHomLibre);
ELISE_ASSERT(mDRF !=0,"LEqHomOneDist::AddCple");
AddEqF(mSet.NewEqHomog(false,*aHF1,*aHF2,mDRF));
for
(
ElPackHomologue::const_iterator it= aPack.begin();
it!= aPack.end();
it++
)
{
AddLiaisonOnTop(it->P1(),it->P2(),it->Pds());
}
}
ElPackHomologue ToStdPack(const tMergeLPackH * aMPack,bool PondInvNorm,double aPdsSingle)
{
ElPackHomologue aRes;
const tLMCplP & aLM = aMPack->ListMerged();
for ( tLMCplP::const_iterator itC=aLM.begin() ; itC!=aLM.end() ; itC++)
{
const Pt2dr & aP0 = (*itC)->GetVal(0);
const Pt2dr & aP1 = (*itC)->GetVal(1);
double aPds = ((*itC)->NbArc() == 1) ? aPdsSingle : 1.0;
if (PondInvNorm)
{
aPds /= NormPt2Ray(aP0) * NormPt2Ray(aP1);
}
ElCplePtsHomologues aCple(aP0,aP1,aPds);
aRes.Cple_Add(aCple);
}
return aRes;
}
void cCameraTiepRed::SaveHom(cCameraTiepRed* aCam2,const std::list<int> & aLBox)
{
std::pair<CamStenope*,CamStenope*> aPC (0,0);
if (mAppli.VerifNM())// (this != aCam2)
{
aPC = mAppli.NM().CamOriRel(NameIm(),aCam2->NameIm());
}
CamStenope* aCS1 = aPC.first;
CamStenope* aCS2 = aPC.second;
ElPackHomologue aRes;
for (std::list<int>::const_iterator itI=aLBox.begin(); itI!=aLBox.end() ; itI++)
{
std::string aName = mAppli.NameHomol(NameIm(),aCam2->NameIm(),*itI);
if (ELISE_fp::exist_file(aName))
{
ElPackHomologue aPack = ElPackHomologue::FromFile(aName);
aRes.Add(aPack);
// Verif
if (aCS2)
{
std::vector<double> aVD;
for (ElPackHomologue::const_iterator itP=aPack.begin(); itP!=aPack.end(); itP++)
{
double aDist;
aCS1->PseudoInterPixPrec(itP->P1(),*aCS2,itP->P2(),aDist);
aVD.push_back(aDist);
}
if (aVD.size())
std::cout << "Verif CamOriRel " << MedianeSup(aVD) << "\n";
}
}
}
if (aRes.size())
{
std::string aKeyH = "NKS-Assoc-CplIm2Hom@"+ mAppli.StrOut() + "@dat";
std::string aNameH = mAppli.ICNM()->Assoc1To2(aKeyH,NameIm(),aCam2->NameIm(),true);
aRes.StdPutInFile(aNameH);
// std::string aNameH = mAppli
}
}
cMEPCoCentrik::cMEPCoCentrik(const ElPackHomologue & aPack,double aFoc,bool aShow,const ElRotation3D * aRef,bool Quick) :
mPack (aPack),
mRef (aRef),
mFoc (aFoc),
mCostMin (1e9),
mDoPly (true),
mEcartCo (1e9),
mMatRPur (1,1),
mShow (aShow),
mSolVraiR (ElRotation3D::Id),
mCostVraiR (1e20),
mQuick (Quick)
{
InitPackME(mVP1,mVP2,mVPds,aPack);
ElTimer aChrono;
mMatRPur = GlobMepRelCocentrique(mEcartCo,mPack,NbRanCoCInit,aPack.size());
mMatRPur = mMatRPur.transpose() ; // Retour aux convention 2 = > 1
// Car un peu optimiste comme estim init
mEcartCo *= 1.5;
if (0) // OK la descente est validee
{
double aNoise = 1e-2;
ElMatrix<double> aMP = ElMatrix<double>::Rotation(aNoise*NRrandC(),aNoise*NRrandC(),aNoise*NRrandC());
mMatRPur = mMatRPur * aMP;
}
for (int aK=0 ; aK<10 ; aK++)
{
OneItereRotPur(mMatRPur,mEcartCo);
}
if (!mQuick)
Test(aPack,mMatRPur,aRef,mEcartCo);
}
void cOneImageOfLayer::SplitLayer
(
cOneImageOfLayer& aL2,
const std::string & aNameH,
const cSplitLayer & aSL
)
{
ElPackHomologue aPck = ElPackHomologue::FromFile(mAppli.DC()+aNameH);
int aLMin = ElMax(mLabMin,aL2.mLabMin);
int aLMax = ElMin(mLabMax,aL2.mLabMax);
for (int aLab = aLMin ; aLab<=aLMax ; aLab++)
{
ElPackHomologue aPackR;
for
(
ElPackHomologue::iterator itP=aPck.begin();
itP!=aPck.end();
itP++
)
{
if (
(LayerOfPt(itP->P1()) == aLab)
&& (aL2.LayerOfPt(itP->P2()) == aLab)
)
{
aPackR.Cple_Add(ElCplePtsHomologues(itP->P1(),itP->P2()));
}
}
std::string aNameRed = mAppli.DC()+ mAppli.ICNM()->Assoc1To2
(
aSL.KeyCalHomSplit(),
aNameH,
ToString(aLab),
true
);
aPackR.StdPutInFile(aNameRed);
// std::cout << "RRRReedd === " << aNameRed << "\n";
}
}
void cAppliReduc::AmelioHomLocal(cImagH & anIm)
{
double aPdsLVMStd = 0.1;
double aPdsFreezC = 100;
double aPdsEvol = 10;;
int aNbIterSupl = 3;
int aMaxIterProgr = 6;
const std::vector<cLink2Img*> & aVLImC = mImCAmel->VLink();
for (int aKIm=0 ; aKIm<int(mIms.size()) ; aKIm++)
{
mIms[aKIm]->HF()->ReinitHom(cElHomographie::Id());
mIms[aKIm]->GainLoc() = 0;
mIms[aKIm]->InitLoc() = false;
}
std::vector<cImagH *> aVIms ;
for (int aKL=0 ; aKL<int(aVLImC.size()) ; aKL++)
{
cLink2Img * aLnK = aVLImC[aKL];
cImagH * anIm = aLnK->Dest();
anIm->GainLoc() = aLnK->PdsEchant() + 1e-7;
anIm->C2CI() = true;
aVIms.push_back(anIm);
anIm->HF()->ReinitHom(aLnK->Hom12().Inverse());
}
mImCAmel->GainLoc() = 1e10;
mImCAmel->InitLoc() = true;
mImCAmel->C2CI() = true;
int aNbIm2Init = aVIms.size();
cCmpPtrImOnGain aCmpPtrIm;
std::sort(aVIms.begin(),aVIms.end(),aCmpPtrIm);
int aNbIterProgr = std::min(aMaxIterProgr,round_up(aVIms.size()/3.0));
int aNbIterTot = aNbIterProgr + aNbIterSupl;
double aErrorIn = ErrorSolLoc();
if (Show(eShowGlob))
std::cout << "ERROR IN " << aErrorIn << "\n";
for (int aNbIter =0 ; aNbIter < aNbIterTot ; aNbIter ++)
{
if (aNbIter < aNbIterProgr)
{
int aK0 = (aNbIter *aNbIm2Init) / aNbIterProgr;
int aK1 = ((aNbIter+1) *aNbIm2Init) / aNbIterProgr;
for (int aKIm=aK0; aKIm<aK1 ; aKIm++)
{
ElPackHomologue aPack;
cImagH * anIm = aVIms[aKIm];
const std::vector<cLink2Img*> & aVL = anIm->VLink();
int aNbInit=0;
for (int aKL=0 ; aKL<int(aVL.size()) ; aKL++)
{
cLink2Img * aLnK = aVL[aKL];
cImagH * anI2 = aLnK->Dest();
if (anI2->InitLoc())
{
const std::vector<Pt3dr> & anEch = aLnK->EchantP1();
cElHomographie aH = anI2->HF()->HomCur() * aLnK->Hom12();
for (int aKP=0 ; aKP<int(anEch.size()) ; aKP++)
{
const Pt3dr & aP3d = anEch[aKP];
Pt2dr aP1 (aP3d.x,aP3d.y);
double aPds = aP3d.z;
Pt2dr aP2 = aH.Direct(aP1);
aPack.Cple_Add(ElCplePtsHomologues(aP1,aP2,aPds));
}
aNbInit++;
}
}
cElHomographie aNewH(aPack,true);
anIm->HF()->ReinitHom(aNewH);
if (Show(eShowDetail)) std::cout << anIm->Name() << " : " << aNbInit << "\n";
}
for (int aKIm=aK0; aKIm<aK1 ; aKIm++)
{
aVIms[aKIm]->InitLoc() = true;
}
if (Show(eShowDetail)) std::cout << "==============================\n";
}
if (mDoCompensLoc)
{
mSetEq.SetPhaseEquation();
double aSomEr=0;
double aSomP=0;
for (int aKIm1=0 ; aKIm1<int(mIms.size()) ; aKIm1++)
{
cImagH * anI1 = mIms[aKIm1];
anI1->AddViscositty((anI1== mImCAmel) ? aPdsFreezC : aPdsLVMStd);
cElHomographie aCurH1 = anI1->HF()->HomCur();
if (anI1->InitLoc())
{
double aPdsE = aPdsEvol / anI1->PdsEchant();
const std::vector<cLink2Img*> & aVL = anI1->VLink();
for (int aKL=0 ; aKL<int(aVL.size()) ; aKL++)
{
cLink2Img * aLnk = aVL[aKL];
cImagH* anI2 = aLnk->Dest();
cElHomographie aCurH2 = anI2->HF()->HomCur();
cElHomographie aCurH2Inv = aCurH2.Inverse();
if (anI2->InitLoc())
{
double aSomRes = 0;
double aSomCtrl = 0;
cElHomographie aH12 = aLnk->Hom12();
const std::vector<Pt3dr> & anEch = aLnk->EchantP1();
cEqHomogFormelle * anEq = aLnk->EqHF();
int aNbPts = anEch.size();
for (int aKEch = 0 ; aKEch<int(aNbPts) ; aKEch++)
{
Pt3dr aP3d = anEch[aKEch];
Pt2dr aP1(aP3d.x,aP3d.y);
Pt2dr aP2 = aH12.Direct(aP1);
double aPds = aP3d.z * aPdsE;
Pt2dr aRes = anEq->StdAddLiaisonP1P2(aP1,aP2,aPds,false);
Pt2dr aCtrl = aCurH2Inv.Direct(aCurH1.Direct(aP1)) - aP2;
aSomRes += euclid(aRes);
aSomCtrl += euclid(aCtrl);
double anEr = square_euclid(aRes);
aSomEr+= anEr * aPds;
aSomP+= aPds;
}
/*
std::cout << anEq
<< " N12=" << anI1->Name() << " " << anI2->Name()
<< " ; RES = " << aSomRes/aNbPts << " Ctrl=" << aSomCtrl/aNbPts << "\n";
*/
}
}
}
// getchar();
// anI->HF()->SetModeCtrl(cNameSpaceEqF::eHomFigee);
}
if (Show(eShowDetail)) std::cout << "ERR = " << sqrt(aSomEr/aSomP) << "\n";
mSetEq.SolveResetUpdate();
}
}
for (int aKIm1=0 ; aKIm1<int(mIms.size()) ; aKIm1++)
{
cImagH * anI1 = mIms[aKIm1];
anI1->H2ImC() = anI1->HF()->HomCur();
}
double aErrorOut = ErrorSolLoc();
if (Show(eShowGlob))
std::cout << "ERROR OUT " << aErrorOut << " DELTA=" << aErrorOut - aErrorIn << "\n";
}
int HomFilterMasq_main(int argc,char ** argv)
{
// MemoArg(argc,argv);
MMD_InitArgcArgv(argc,argv);
std::string aDir,aPat,aFullDir;
bool ExpTxt=false;
std::string PostPlan="_Masq";
std::string KeyCalcMasq;
std::string KeyEquivNoMasq;
std::string MasqGlob;
double aResol=10;
bool AcceptNoMask;
std::string aPostIn= "";
std::string aPostOut= "MasqFiltered";
ElInitArgMain
(
argc,argv,
LArgMain() << EAMC(aFullDir,"Full name (Dir+Pat)", eSAM_IsPatFile),
LArgMain()
<< EAM(PostPlan,"PostPlan",true,"Post to plan, Def : toto ->toto_Masq.tif like with SaisieMasq")
<< EAM(MasqGlob,"GlobalMasq",true,"Global Masq to add to all image")
<< EAM(KeyCalcMasq,"KeyCalculMasq",true,"For tuning masq per image")
<< EAM(KeyEquivNoMasq,"KeyEquivNoMasq",true,"When given if KENM(i1)==KENM(i2), don't masq")
<< EAM(aResol,"Resol",true,"Sub Resolution for masq storing, Def=10")
<< EAM(AcceptNoMask,"ANM",true,"Accept no mask, def = true if MasqGlob and false else")
<< EAM(ExpTxt,"ExpTxt",true,"Ascii format for in and out, def=false")
<< EAM(aPostIn,"PostIn",true,"Post for Input dir Hom, Def=")
<< EAM(aPostOut,"PostOut",true,"Post for Output dir Hom, Def=MasqFiltered")
);
#if (ELISE_windows)
replace( aFullDir.begin(), aFullDir.end(), '\\', '/' );
#endif
SplitDirAndFile(aDir,aPat,aFullDir);
if (EAMIsInit(&PostPlan))
{
CorrecNameMasq(aDir,aPat,PostPlan);
}
if (!EAMIsInit(&AcceptNoMask))
AcceptNoMask = EAMIsInit(&MasqGlob);
cInterfChantierNameManipulateur * anICNM = cInterfChantierNameManipulateur::BasicAlloc(aDir);
Im2D_Bits<1> aImMasqGlob(1,1);
if (EAMIsInit(&MasqGlob))
aImMasqGlob = GetMasqSubResol(aDir+MasqGlob,aResol);
const std::vector<std::string> * aVN = anICNM->Get(aPat);
std::vector<Im2D_Bits<1> > aVMasq;
for (int aKN = 0 ; aKN<int(aVN->size()) ; aKN++)
{
std::string aNameIm = (*aVN)[aKN];
Tiff_Im aTF = Tiff_Im::StdConvGen(aNameIm,1,false);
Pt2di aSzG = aTF.sz();
Pt2di aSzR (round_ni(Pt2dr(aSzG)/aResol));
Im2D_Bits<1> aImMasq(aSzR.x,aSzR.y,1);
std::string aNameMasq = StdPrefix(aNameIm)+PostPlan + ".tif";
if (EAMIsInit(&KeyCalcMasq))
{
aNameMasq = anICNM->Assoc1To1(KeyCalcMasq,aNameIm,true);
}
if (ELISE_fp::exist_file(aNameMasq))
{
Im2D_Bits<1> aImMasqLoc = GetMasqSubResol(aDir+aNameMasq,aResol);
ELISE_COPY(aImMasq.all_pts(),aImMasq.in() && aImMasqLoc.in(0),aImMasq.out());
}
else
{
if (!AcceptNoMask)
{
std::cout << "For Im " << aNameIm << " file " << aNameMasq << " does not exist\n";
ELISE_ASSERT(false,"Masq not found");
}
}
if (EAMIsInit(&MasqGlob))
{
ELISE_COPY(aImMasq.all_pts(),aImMasq.in() && aImMasqGlob.in(0),aImMasq.out());
}
aVMasq.push_back(aImMasq);
// Tiff_Im::CreateFromIm(aImMasq,"SousRes"+aNameMasq);
}
std::string anExt = ExpTxt ? "txt" : "dat";
std::string aKHIn = std::string("NKS-Assoc-CplIm2Hom@")
+ std::string(aPostIn)
+ std::string("@")
+ std::string(anExt);
std::string aKHOut = std::string("NKS-Assoc-CplIm2Hom@")
+ std::string(aPostOut)
+ std::string("@")
+ std::string(anExt);
for (int aKN1 = 0 ; aKN1<int(aVN->size()) ; aKN1++)
{
for (int aKN2 = 0 ; aKN2<int(aVN->size()) ; aKN2++)
{
std::string aNameIm1 = (*aVN)[aKN1];
std::string aNameIm2 = (*aVN)[aKN2];
std::string aNameIn = aDir + anICNM->Assoc1To2(aKHIn,aNameIm1,aNameIm2,true);
if (ELISE_fp::exist_file(aNameIn))
{
bool UseMasq = true;
if (EAMIsInit(&KeyEquivNoMasq))
{
UseMasq = (anICNM->Assoc1To1(KeyEquivNoMasq,aNameIm1,true) != anICNM->Assoc1To1(KeyEquivNoMasq,aNameIm2,true) );
}
TIm2DBits<1> aMasq1 ( aVMasq[aKN1]);
TIm2DBits<1> aMasq2 ( aVMasq[aKN2]);
ElPackHomologue aPackIn = ElPackHomologue::FromFile(aNameIn);
ElPackHomologue aPackOut;
for (ElPackHomologue::const_iterator itP=aPackIn.begin(); itP!=aPackIn.end() ; itP++)
{
Pt2dr aP1 = itP->P1();
Pt2dr aP2 = itP->P2();
Pt2di aQ1 = round_ni(aP1/aResol);
Pt2di aQ2 = round_ni(aP2/aResol);
if ((aMasq1.get(aQ1,0) && aMasq2.get(aQ2,0)) || (! UseMasq))
{
ElCplePtsHomologues aCple(aP1,aP2);
aPackOut.Cple_Add(aCple);
}
}
std::string aNameOut = aDir + anICNM->Assoc1To2(aKHOut,aNameIm1,aNameIm2,true);
aPackOut.StdPutInFile(aNameOut);
std::cout << "IN " << aNameIn << " " << aNameOut << " UseM " << UseMasq << "\n";
}
}
}
// std::vector<cImFMasq *> mVIm;
return 0;
}
void cApply_CreateEpip_main::DoEpipGen()
{
mLengthMin = 500.0;
mStepReech = 10.0;
mNbZ = 1;
mNbXY = 100;
ElPackHomologue aPack;
Pt2dr aDir2 = DirEpipIm2(mGenI1,mGenI2,aPack,true);
Pt2dr aDir1 = DirEpipIm2(mGenI2,mGenI1,aPack,false);
std::cout << "Compute Epip\n";
CpleEpipolaireCoord * aCple = CpleEpipolaireCoord::PolynomialFromHomologue(false,aPack,mDegre,aDir1,aDir2);
EpipolaireCoordinate & e1 = aCple->EPI1();
EpipolaireCoordinate & e2 = aCple->EPI2();
Pt2dr aInf1(1e20,1e20),aSup1(-1e20,-1e20);
Pt2dr aInf2(1e20,1e20),aSup2(-1e20,-1e20);
double aErrMax = 0.0;
double aErrMoy = 0.0;
int mNbP = 0;
double aX2mX1 = 0.0;
CpleEpipolaireCoord * aCple3 = 0;
CpleEpipolaireCoord * aCple7 = 0;
if (0)
{
aCple3 = CpleEpipolaireCoord::PolynomialFromHomologue(false,aPack,3,aDir1,aDir2);
aCple7 = CpleEpipolaireCoord::PolynomialFromHomologue(false,aPack,7,aDir1,aDir2);
}
double aErrMaxDir1 = 0.0;
double aErrMaxInv1 = 0.0;
for (ElPackHomologue::const_iterator itC=aPack.begin() ; itC!= aPack.end() ; itC++)
{
Pt2dr aP1 = e1.Direct(itC->P1());
Pt2dr aP2 = e2.Direct(itC->P2());
aInf1 = Inf(aInf1,aP1);
aSup1 = Sup(aSup1,aP1);
aInf2 = Inf(aInf2,aP2);
aSup2 = Sup(aSup2,aP2);
aX2mX1 += aP2.x - aP1.x;
double anErr = ElAbs(aP1.y-aP2.y);
mNbP++;
aErrMax = ElMax(anErr,aErrMax);
aErrMoy += anErr;
if (aCple3)
{
Pt2dr aQ1D3 = aCple3->EPI1().Direct(itC->P1());
Pt2dr aQ1D7 = aCple7->EPI1().Direct(itC->P1());
double aDQ1 = euclid(aQ1D3-aQ1D7);
aErrMaxDir1 = ElMax(aDQ1,aErrMaxDir1);
Pt2dr aR1D3 = aCple3->EPI1().Inverse(aQ1D3);
Pt2dr aR1D7 = aCple7->EPI1().Inverse(aQ1D7);
double aDR1 = euclid(aR1D3-aR1D7);
aErrMaxInv1 = ElMax(aDR1,aErrMaxInv1);
}
}
if (aCple3)
{
std::cout << "MAX ER " << aErrMaxDir1 << " " << aErrMaxInv1 << "\n";
}
aX2mX1 /= mNbP;
double aInfY = ElMax(aInf1.y,aInf2.y);
double aSupY = ElMax(aSup1.y,aSup2.y);
aInf1.y = aInf2.y = aInfY;
aSup1.y = aSup2.y = aSupY;
std::cout << aInf1 << " " << aSup1 << "\n";
std::cout << aInf2 << " " << aSup2 << "\n";
std::cout << "DIR " << aDir1 << " " << aDir2 << " X2-X1 " << aX2mX1<< "\n";
std::cout << "Epip Rect Accuracy, Moy " << aErrMoy/mNbP << " Max " << aErrMax << "\n";
cTmpReechEpip aReech1(true,mName1,Box2dr(Pt2dr(0,0),Pt2dr(mGenI1->SzBasicCapt3D())),&e1,Box2dr(aInf1,aSup1),mStepReech,"ImEpi1"+mPostIm+".tif",mPostMasq,mNumKer,mDebug);
std::cout << "DONE IM1 \n";
cTmpReechEpip aReech2(true,mName2,Box2dr(Pt2dr(0,0),Pt2dr(mGenI2->SzBasicCapt3D())),&e2,Box2dr(aInf2,aSup2),mStepReech,"ImEpi2"+mPostIm+".tif",mPostMasq,mNumKer,mDebug);
std::cout << "DONE IM2 \n";
std::cout << "DONNE REECH TMP \n";
// ::cTmpReechEpip(cBasicGeomCap3D * aGeom,EpipolaireCoordinate * anEpi,Box2dr aBox,double aStep) :
}
