Пример #1
0
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));
}