double cLiaisORPO::Residu()
{
    SetOrCam();
    if (! mProfCalc)
    {
       mProfCalc = true;
       SetProf();
    }

    // ElRotation3D aC1toM = mCam1.Orientation().inv();
    // ElRotation3D aMtoC2 = mCam2.Orientation();

    ElRotation3D  aC1toC2 =  mCam2.Orient() *  mCam1.Orient().inv();

    Pt3dr aBaseInC2 = aC1toC2.ImAff(Pt3dr(0,0,0));

    ElMatrix<double> aMat =  aC1toC2.Mat() ;

    Pt3dr aC0,aC1,aC2;
    aMat.GetCol(0,aC0);
    aMat.GetCol(1,aC1);
    aMat.GetCol(2,aC2);


    double sDist=0.0;
    int aK = 0;

    for (ElPackHomologue::const_iterator itP =mPack.begin() ; itP!=mPack.end(); itP++)
    {
       double aPds = itP->Pds();
       double aLambda =  mProfs.at(aK);

        Pt2dr aPIm1 = itP->P1();
        Pt3dr aRay1 = aC0 * aPIm1.x + aC1 * aPIm1.y + aC2;


        Pt3dr aPTerA = aBaseInC2 + aRay1 * (aLambda *0.99);
        Pt3dr aPTerB = aBaseInC2 + aRay1 * (aLambda *1.01);

        Pt2dr aProjA(aPTerA.x/aPTerA.z,aPTerA.y/aPTerA.z);
        Pt2dr aProjB(aPTerB.x/aPTerB.z,aPTerB.y/aPTerB.z);

         Pt2dr aVAB  (aProjB.y-aProjA.y,aProjA.x-aProjB.x);

         double aD2 =  ElAbs(scal(aVAB,aProjA-itP->P2()) / euclid(aVAB));
         sDist += aPds*aD2;

         aK++;
    }

    if (DEBUG_POWEL)
    {
        std::cout << sDist/mPack.size() << "\n"; // getchar();
    }

    return sDist * mPPL->Pds().Val();
}
bool  cModeleAnalytiqueComp::FiltragePointHomologues
      (
          const ElPackHomologue & aPackInit,
	  ElPackHomologue & aNewPack, 
	  double aTol,
	  double aFiltre
      )
{
       bool GotOut = false;
       Box2dr aBox1 = mAppli.PDV1()->BoxIm().AddTol(aTol);
       Box2dr aBox2 = mAppli.PDV2()->BoxIm().AddTol(aTol);

       Box2dr aBoxF1 = mAppli.PDV1()->BoxIm().AddTol(aFiltre);
       Box2dr aBoxF2 = mAppli.PDV2()->BoxIm().AddTol(aFiltre);

       for 
       (
          ElPackHomologue::const_iterator iT = aPackInit.begin();
          iT != aPackInit.end();
          iT++
       )
       {
          if (    ( aBoxF1.inside(iT->P1()))
	       && ( aBoxF2.inside(iT->P2()))
             )
          {

              bool thisOut = (! aBox1.inside(iT->P1()))
                          || (! aBox2.inside(iT->P2()));
              if (thisOut)
              {
                  std::cout << aBox1._p1 << aBox2._p1 << "\n";
                  std::cout << "OUT " << iT->P1() << " " << iT->P2() << " " << iT->Pds()<< "\n";
              }
              GotOut =    GotOut || thisOut;

	     aNewPack.Cple_Add(iT->ToCple());
          }
       }
       return GotOut;
}
CpleEpipolaireCoord  * CpleEpipolaireCoord::PolynomialFromHomologue
                     (
                          CpleEpipolaireCoord  *  aSolApprox,
                          REAL                    aResiduMin,
                          const ElPackHomologue & aPackH,
                          INT   aDegre,
                          Pt2dr aDir1,
                          Pt2dr aDir2
                     )
{
   StatElPackH  aStat(aPackH);


   Polynome2dReal aPol1(aDegre,aStat.RMax1());
   Polynome2dReal aPol2(aDegre,aStat.RMax2());

   INT aNbInc =0;
   for (INT k=0; k<aPol1.NbMonome() ; k++)
   {
      const Monome2dReal & aMon = aPol1.KthMonome(k);
      if (aMon.DegreX() != 0)
         aNbInc++;
      aNbInc++;
   }


   SystLinSurResolu aSys (aNbInc,aStat.NbPts()) ;
/*
   cGenSysSurResol * aSys =   (aSolApprox == (CpleEpipolaireCoord *)0 ) ?
                              new SystLinSurResolu(aNbInc,aStat.NbPts()) :
                              new L2SysSurResol(aNbInc);
*/
   Im1D_REAL8 aVecPds(aNbInc);
   REAL8 * aDVP = aVecPds.data();

   for
   (
        ElPackHomologue::const_iterator itC = aPackH.begin();
        itC != aPackH.end();
        itC++
   )
   {
      REAL aPdsResidu = 1;
      if (aSolApprox)
      {
          Pt2dr aQ1 = aSolApprox->EPI1().Direct(itC->P1());
          Pt2dr aQ2 = aSolApprox->EPI2().Direct(itC->P2());

          REAL aResidu = ElAbs(aQ1.y-aQ2.y);
          aPdsResidu =  1/sqrt(ElSquare(aResidu)+ElSquare(aResiduMin));
      }
      Pt2dr aP1 = ( itC->P1() -aStat.Cdg1()) / aDir1;
      Pt2dr aP2 = ( itC->P2() -aStat.Cdg2()) / aDir2;

      aNbInc=0;
      for (INT k=0; k<aPol1.NbMonome() ; k++)
      {
         const Monome2dReal & aMon1 = aPol1.KthMonome(k);
         if (aMon1.DegreX() != 0)
         {
            aDVP[aNbInc++] = -aMon1(aP1);
         }
         const Monome2dReal & aMon2 = aPol2.KthMonome(k);
         aDVP[aNbInc++] = aMon2(aP2);
      }
      // aSys->GSSR_AddNewEquation(itC->Pds()*aPdsResidu,aDVP,aP1.y);
      aSys.PushEquation(aVecPds,aP1.y,itC->Pds()*aPdsResidu);
   }
   
   bool aOk;
   // Im1D_REAL8  aSol =  aSys->GSSR_Solve(&aOk);
   Im1D_REAL8  aSol = (aSolApprox ? aSys.L2Solve(&aOk) : aSys.L1Solve());

   aNbInc=0;
   {
   for (INT k=0; k<aPol1.NbMonome() ; k++)
   {
        const Monome2dReal & aMon1 = aPol1.KthMonome(k);
        if (aMon1.DegreX() != 0)
        {
            aPol1.SetCoeff(k,aSol.data()[aNbInc++]);
        }
        else
            aPol1.SetCoeff(k,(aMon1.DegreY() == 1)*aStat.RMax1());

        aPol2.SetCoeff(k,aSol.data()[aNbInc++]);
   }
   }

   PolynomialEpipolaireCoordinate * anEpi1 = new PolynomialEpipolaireCoordinate
                                                 (
                                                      aStat.Cdg1(),
                                                      aDir1,
                                                      aPol1,
                                                      aStat.RMax1()
                                                 );

   PolynomialEpipolaireCoordinate * anEpi2 = new PolynomialEpipolaireCoordinate
                                                 (
                                                      aStat.Cdg2(),
                                                      aDir2,
                                                      aPol2,
                                                      aStat.RMax2()
                                                 );

   // delete aSys;

   return  new CpleEpipolaireCoord(anEpi1,anEpi2);
}