void Verif(Pt2df aPf)
{
Pt2dr aPd = ToPt2dr(aPf);
if (std_isnan(aPd.x) || std_isnan(aPd.y))
{
std::cout << "PB PTS " << aPf << " => " << aPd << "\n";
ELISE_ASSERT(false,"PB PTS in Verif");
}
}
double cPackObsLiaison::AddObs
(
const cPonderationPackMesure & aPond,
const cPonderationPackMesure * aPondSurf,
cStatObs & aSO,
const cRapOnZ * aRAZ
)
{
double aS1=0;
double aSEr=0;
// SPECIAL DEBUG CHOLESKY
cElRegex aRegDebug("XXhjkjYtiuoiu062",10);
if (mIsMult)
{
for (int aK= 0 ; aK< 2; aK++)
{
for
(
std::map<std::string,cObsLiaisonMultiple *>::iterator itOML=mDicoMul.begin();
itOML!=mDicoMul.end();
itOML++
)
{
// std::cout << "OOLLM "<< itOML->first << "\n";
cObsLiaisonMultiple * anOLM = itOML->second;
cGenPoseCam * aPC = anOLM->Pose1() ;
bool IsDebug = aRegDebug.Match(aPC->Name());
bool aDoIt = (aK==0) ? IsDebug : (!IsDebug);
if (aDoIt)
{
if (aSO.AddEq() || aPondSurf || aPond.IdFilter3D().IsInit())
{
aS1++;
aSEr += anOLM->AddObsLM(aPond,aPondSurf,0,(cArgVerifAero*)0,aSO,aRAZ);
}
else
{
aSEr += anOLM->BasicAddObsLM (aPond,aSO,aRAZ);
}
}
}
}
}
else
{
cCpleCmpEcMax aCmp;
std::sort(mLObs.begin(),mLObs.end(),aCmp);
for
(
std::vector<cObservLiaison_1Cple *>::iterator it1C=mLObs.begin();
it1C != mLObs.end();
it1C++
)
{
if (((*it1C)->Pose1()->RotIsInit()) && ((*it1C)->Pose2()->RotIsInit()))
{
aS1++;
aSEr += (*it1C)->AddObs(aPond,aPondSurf);
}
}
}
aSEr /= aS1;
if (aS1 && (int(aPond.Show().Val()) >= int(eNSM_Iter)))
{
double aSqrtEr = sqrt(aSEr);
mAppli.CurXmlE().AverageResidual() = aSqrtEr;
mAppli.COUT() << "| | " << " Residual = "
<< aSqrtEr ;
if (aSO.PdsEvol())
{
mAppli.CurXmlE().EvolMax().SetVal(aSO.MaxEvol());
mAppli.CurXmlE().EvolMoy().SetVal(aSO.MoyEvol());
mAppli.COUT() << " ;; Evol, Moy=" << aSO.MoyEvol() << " ,Max=" << aSO.MaxEvol() ;
}
mAppli.COUT() << "\n";
if (mAppli.mPoseWorstRes)
{
mAppli.COUT() << "| | Worst, Res " << mAppli.mWorstRes << " for " << mAppli.mPoseWorstRes->Name();
mAppli.COUT() << ", Perc " << mAppli.mWorstPerc << " for " << mAppli.mPoseWorstPerc->Name();
mAppli.COUT() << "\n";
}
if (aNbCond)
{
mAppli.COUT() << "| | Cond , Aver " << aSomCond/aNbCond
<< " Max " << aGlobMaxCond
<< " Prop>100 " << aNb100/aNbCond << "\n";
}
if (TheExitOnNan)
{
if (std_isnan(aSqrtEr))
{
ElEXIT(1,"Nan value in residual");
}
}
}
return aSEr;
}
bool cElComposHomographie::HasNan() const
{
return std_isnan(mX) || std_isnan(mY) || std_isnan(m1);
}
