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));
}
void cASAMG::ComputeIncidGradProf()
{
Im2DGen * aImProf = mStdN->ImProf();
L2SysSurResol aSys (3);
Pt2di aP0;
Im2D_Bits<1> aMasqTmp = ImMarqueurCC(mSz);
TIm2DBits<1> aTMasqTmp(aMasqTmp);
for (aP0.x=0 ; aP0.x<mSz.x ; aP0.x++)
{
for (aP0.y=0 ; aP0.y<mSz.y ; aP0.y++)
{
double Angle = 1.5;
if (mTMasqN.get(aP0))
{
cCalcPlanImage aCalcPl(CCDist(),aSys,aImProf);
int aNb = OneZC(aP0,CCV4(),aTMasqTmp,1,0,mTMasqN,1,aCalcPl);
ResetMarqueur(aTMasqTmp,aCalcPl.mVPts);
if (aNb >= SeuimNbPtsCCDist())
{
Im1D_REAL8 aSol = aSys.Solve((bool *)0);
double * aDS = aSol.data();
Pt2dr aGrad (aDS[0],aDS[1]);
Angle = euclid(aGrad);
}
/*
*/
}
mTIncid.oset(aP0,ElMin(255,ElMax(0,round_ni(Angle*DynAng()))));
}
}
}