double cAppliOptimTriplet::ResiduTriplet(const ElRotation3D & aR1,const ElRotation3D & aR2,const ElRotation3D & aR3)
{
std::vector<double> aVRes;
for (int aK=0 ; aK<int(mIm1->VFullPtOf3().size()) ; aK++)
{
std::vector<Pt3dr> aW1;
std::vector<Pt3dr> aW2;
AddSegOfRot(aW1,aW2,aR1,mIm1->VFullPtOf3()[aK]);
AddSegOfRot(aW1,aW2,aR2,mIm2->VFullPtOf3()[aK]);
AddSegOfRot(aW1,aW2,aR3,mIm3->VFullPtOf3()[aK]);
bool OkI;
Pt3dr aI = InterSeg(aW1,aW2,OkI);
if (OkI)
{
double aRes1 = Residu(mIm1->Im(),aR1,aI,mIm1->VFullPtOf3()[aK]);
double aRes2 = Residu(mIm2->Im(),aR2,aI,mIm2->VFullPtOf3()[aK]);
double aRes3 = Residu(mIm3->Im(),aR3,aI,mIm3->VFullPtOf3()[aK]);
/*
double aRes2 = Residu(mIm2->Im(),R2(),aI,mVP2[aK]);
*/
aVRes.push_back((aRes1+aRes2+aRes3)/3.0);
}
}
return MedianeSup(aVRes);
}
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
}
}
double cPairOfTriplet::ResiduMoy(const ElRotation3D & aR1,const ElRotation3D & aR2)
{
std::vector<double> aVRes;
for (int aK=0 ; aK<int(mFullVP1.size()) ; aK++)
{
std::vector<Pt3dr> aW1;
std::vector<Pt3dr> aW2;
AddSegOfRot(aW1,aW2,aR1,mFullVP1[aK]);
AddSegOfRot(aW1,aW2,aR2,mFullVP2[aK]);
bool OkI;
Pt3dr aI = InterSeg(aW1,aW2,OkI);
if (OkI)
{
double aRes1 = Residu(mIm1->Im(),aR1,aI,mFullVP1[aK]);
double aRes2 = Residu(mIm2->Im(),aR2,aI,mFullVP2[aK]);
aVRes.push_back((aRes1+aRes2)/2.0);
}
}
return MedianeSup(aVRes);
}
cElHomographie cElHomographie::RobustInit(double & aDMIn,double * aQuality,const ElPackHomologue & aPack,bool & Ok ,int aNbTestEstim, double aPerc,int aNbMaxPts)
{
cElHomographie aRes = cElHomographie::Id();
Ok = false;
Pt2dr aCdg(0,0);
for (ElPackHomologue::tCstIter itH=aPack.begin() ; itH!=aPack.end() ; itH++)
{
aCdg = aCdg + itH->P1();
}
aCdg = aCdg / double(aPack.size());
std::vector<std::pair<Pt2dr,Pt2dr> > aV00;
std::vector<std::pair<Pt2dr,Pt2dr> > aV01;
std::vector<std::pair<Pt2dr,Pt2dr> > aV10;
std::vector<std::pair<Pt2dr,Pt2dr> > aV11;
std::vector<std::pair<Pt2dr,Pt2dr> > aVAll;
int aNbPtsTot = aPack.size();
int aCpt = 0;
for (ElPackHomologue::tCstIter itH=aPack.begin() ; itH!=aPack.end() ; itH++)
{
Pt2dr aP1 = itH->P1();
Pt2dr aP2 = itH->P2();
std::pair<Pt2dr,Pt2dr> aPair(aP1,aP2);
if ( (((aCpt-1)*aNbMaxPts)/aNbPtsTot) != ((aCpt*aNbMaxPts)/aNbPtsTot))
{
aVAll.push_back(aPair);
}
if (aP1.x < aCdg.x)
{
if (aP1.y < aCdg.y) aV00.push_back(aPair);
else aV01.push_back(aPair);
}
else
{
if (aP1.y < aCdg.y) aV10.push_back(aPair);
else aV11.push_back(aPair);
}
aCpt++;
}
if (aV00.empty() || aV01.empty() || aV10.empty() || aV11.empty() )
return aRes;
aDMIn = 1e30;
int aNbPts = aVAll.size();
int aNbKth = ElMax(1,ElMin(aNbPts-1,round_ni((aPerc/100.0) * aNbPts)));
std::vector<double> aVDist;
if (aNbMaxPts<aNbPtsTot)
aNbTestEstim = (aNbTestEstim*aNbPtsTot) / aNbMaxPts;
// int aKMIN = -1;
std::vector<double> aVD; // For tuning and show in if(0) ...
while (aNbTestEstim)
{
int aK00 = NRrandom3(aV00.size());
int aK01 = NRrandom3(aV01.size());
int aK10 = NRrandom3(aV10.size());
int aK11 = NRrandom3(aV11.size());
ElPackHomologue aP4;
AddPair(aP4,aV00[aK00]);
AddPair(aP4,aV01[aK01]);
AddPair(aP4,aV10[aK10]);
AddPair(aP4,aV11[aK11]);
cElHomographie aSol = cElHomographie(aP4,true);
aVDist.clear();
for (int aK=0 ; aK< aNbPts ; aK++)
{
Pt2dr aP1 = aVAll[aK].first;
Pt2dr aP2 = aVAll[aK].second;
/*
Pt2dr aDif = aP2 -aSol.Direct(aP1);
double aDx = ElAbs(aDif.x);
double aDy = ElAbs(aDif.y);
double aDist = (aDx+aDy + ElMax(aDx,aDy))/ 2.0;
*/
double aDist = QuickDist(aP2 -aSol.Direct(aP1));
aVDist.push_back(aDist);
}
double aSom = MoyKPPVal(aVDist,aNbKth);
aVD.push_back(aSom);
//std::cout << "Robust:Hom:SOM = " << aDMIn << " " << aSom << "\n";
if (aSom <aDMIn)
{
aRes = aSol;
aDMIn = aSom;
}
aNbTestEstim--;
}
// double aDMinInit = aDMIn;
ElPackHomologue aPckPds;
for (int anIterL2 = 0 ; anIterL2 < 4 ; anIterL2++)
{
aPckPds = ElPackHomologue();
aVDist.clear();
int aCpt = 0;
for (ElPackHomologue::tCstIter itH=aPack.begin() ; itH!=aPack.end() ; itH++)
{
Pt2dr aP1 = itH->P1();
Pt2dr aP2 = itH->P2();
double aDist = QuickDist(aP2 -aRes.Direct(aP1));
aVDist.push_back(aDist);
double aPds = 1/ (1+ 4*ElSquare(aDist/aDMIn));
aPckPds.Cple_Add(ElCplePtsHomologues(aP1,aP2,aPds));
aCpt++;
}
ELISE_ASSERT(aNbPtsTot==aPack.size() ,"KKKKK ????");
int aKTh = round_ni(aNbPtsTot * (aPerc/100.0));
ELISE_ASSERT(int(aVDist.size())==aNbPtsTot,"Compat MoyKPPVal/SplitArrounKthValue");
aDMIn = MoyKPPVal(aVDist,aKTh);
aRes = cElHomographie(aPckPds,true);
}
if (aQuality)
{
std::vector<double> aVEstim;
int aNbTestValid = 71;
for (int aKTest = 0 ; aKTest <aNbTestValid ; aKTest++)
{
ElPackHomologue aPckPdsA;
ElPackHomologue aPckPdsB;
cRandNParmiQ aSelec(aNbPtsTot/2,aNbPtsTot);
for (ElPackHomologue::tCstIter itH=aPack.begin() ; itH!=aPack.end() ; itH++)
{
Pt2dr aP1 = itH->P1();
Pt2dr aP2 = itH->P2();
double aDist = QuickDist(aP2 -aRes.Direct(aP1));
aVDist.push_back(aDist);
double aPds = 1/ sqrt(1+ ElSquare(aDist/aDMIn));
// if (NRrandom3() > 0.5)
if (aSelec.GetNext())
aPckPdsA.Cple_Add(ElCplePtsHomologues(aP1,aP2,aPds));
else
aPckPdsB.Cple_Add(ElCplePtsHomologues(aP1,aP2,aPds));
}
cElHomographie aResA = cElHomographie(aPckPdsA,true);
cElHomographie aResB = cElHomographie(aPckPdsB,true);
double aSomDist = 0;
for (ElPackHomologue::tCstIter itH=aPack.begin() ; itH!=aPack.end() ; itH++)
{
Pt2dr aP1 = itH->P1();
Pt2dr aQ = aRes.Direct(aP1);
Pt2dr aQA = aResA.Direct(aP1);
Pt2dr aQB = aResB.Direct(aP1);
double aDist = (QuickDist(aQ-aQA) + QuickDist(aQ-aQB) + QuickDist(aQB-aQA)) / 3.0;
aSomDist += aDist;
}
aSomDist /= aNbPtsTot;
aVEstim.push_back(aSomDist);
}
*aQuality = MedianeSup(aVEstim);
}
Ok= true;
return aRes;
}
void cAppliTiepRed::DoReduceBox()
{
DoLoadTiePoints();
DoFilterCamAnLinks();
// == OK
// merge topological tie point
// Create an empty merging struct
mMergeStruct = new tMergeStr(mVecCam.size(),true);
// for each link do the mergin
for (std::list<cLnk2ImTiepRed *>::const_iterator itL=mLnk2Im.begin() ; itL!=mLnk2Im.end() ; itL++)
{
(*itL)->Add2Merge(mMergeStruct);
}
mMergeStruct->DoExport(); // "Compile" to make the point usable
mLMerge = & mMergeStruct->ListMerged(); // Get the merged multiple points
std::vector<int> aVHist(mVecCam.size()+1,0);
// Compute the average
double aSzTileAver = sqrt(mBoxLoc.surf()/mLMerge->size());
// Quod tree for spatial indexation
mQT = new tTiePRed_QT ( mPMul2Gr, mBoxLoc, 5 /* 5 obj max box */, 2*aSzTileAver);
// Heap for priority management
// OK
// give ground coord to multiple point and put them in quod-tree and heap
{
std::vector<double> aVPrec;
mVPM.reserve(mLMerge->size());
for (std::list<tMerge *>::const_iterator itM=mLMerge->begin() ; itM!=mLMerge->end() ; itM++)
{
cPMulTiepRed * aPM = new cPMulTiepRed(*itM,*this);
if (mBoxLoc.inside(aPM->Pt()))
{
mVPM.push_back(aPM);
aVPrec.push_back(aPM->Prec());
ELISE_ASSERT((*itM)->NbSom()<int(aVHist.size()),"JJJJJJJJJJJJJJJJJJJJ");
aVHist[(*itM)->NbSom()] ++;
mQT->insert(aPM);
}
else
{
delete aPM;
}
}
if (aVPrec.size() ==0)
{
return;
}
mStdPrec = MedianeSup(aVPrec);
mHeap = new tTiePRed_Heap(mPMulCmp);
// The gain can be computed once we know the standard precision
for (int aKP=0 ; aKP<int(mVPM.size()) ; aKP++)
{
mVPM[aKP]->InitGain(*this);
mHeap->push(mVPM[aKP]);
}
}
int aNbInit = mHeap->nb();
// PAs OK
tPMulTiepRedPtr aPMPtr;
while (mHeap->pop(aPMPtr))
{
mListSel.push_back(aPMPtr);
aPMPtr->Remove();
aPMPtr->SetSelected();
std::set<tPMulTiepRedPtr> aSetNeigh; // = *(new std::set<tPMulTiepRedPtr>);
double aDist= mDistPMul * mResol;
mQT->RVoisins(aSetNeigh,aPMPtr->Pt(),aDist);
for (std::set<tPMulTiepRedPtr>::const_iterator itS=aSetNeigh.begin() ; itS!=aSetNeigh.end() ; itS++)
{
// tPMulTiepRedPtr aNeigh = aSetNeigh[aK];
tPMulTiepRedPtr aNeigh = *itS;
if (! aNeigh->Removed())
{
aNeigh->UpdateNewSel(aPMPtr,*this);
if (aNeigh->Removable())
{
aNeigh->Remove();
mQT->remove(aNeigh);
mHeap->Sortir(aNeigh);
}
}
}
/* std::cout << " GAIN " << aPMPtr->Gain() << " "
<< aPMPtr->Merge()->NbArc() << " " << aPMPtr->Merge()->NbSom()
<< " " << aSetNeigh.size() << " " << mHeap->nb() << "\n";
*/
}
VonGruber();
// PAOK
std::cout << "NBPTS " << aNbInit << " => " << mListSel.size() << "\n";
DoExport();
}
