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 cCalcPlanImage::OnNewPt(const Pt2di & aP)
{
cCCMaxNbStep::OnNewPt(aP);
// A x + By + C = Z
static double aCoeff[3];
aCoeff[0] = aP.x;
aCoeff[1] = aP.y;
aCoeff[2] = 1.0;
mSys->AddEquation(1.0,aCoeff,mImProf->GetR(aP));
}