cElHomographie cElHomographie::RansacInitH(const ElPackHomologue & aPack,int aNbRansac,int aNbMaxPts)
{
   if ((aPack.size()<10)  || (aNbMaxPts<4))
      return cElHomographie(aPack,false);
   

   cRandNParmiQ aRand(aNbMaxPts,aPack.size());
   std::vector<ElCplePtsHomologues> aVCH;

   for (ElPackHomologue::tCstIter itH=aPack.begin() ; itH!=aPack.end() ; itH++)
   {
      if (aRand.GetNext())
      {
          aVCH.push_back(itH->ToCple());
      }
   }

   double anEcMin = 1e30;
   cElHomographie aHMin = cElHomographie::Id();

   while (aNbRansac)
   {
       int aK1 = NRrandom3(aVCH.size());
       int aK2 = NRrandom3(aVCH.size());
       int aK3 = NRrandom3(aVCH.size());
       int aK4 = NRrandom3(aVCH.size());
       if ( 
               (aK1!=aK2) && (aK1!=aK3) &&  (aK1!=aK4)
            && (aK2!=aK3) && (aK2!=aK4)
            && (aK3!=aK4)
          )
        {
            aNbRansac--;
            ElPackHomologue aP4;
            aP4.Cple_Add(aVCH[aK1]);
            aP4.Cple_Add(aVCH[aK2]);
            aP4.Cple_Add(aVCH[aK3]);
            aP4.Cple_Add(aVCH[aK4]);

            cElHomographie aSol = cElHomographie(aP4,true);
            double anEcart = 0;
            for (int aKP=0 ; aKP<int(aVCH.size()) ;  aKP++)
            {
                anEcart += euclid(aSol.Direct(aVCH[aKP].P1()),aVCH[aKP].P2());
            }
            anEcart /= aVCH.size();
            if (anEcart<anEcMin)
            {
                anEcMin = anEcart;
                aHMin = aSol;
            }
        }
   }
   std::cout << "ECART H " << anEcMin << "\n";
   return aHMin;
 
}
cElHomographie cElHomographie::MapingChScale(REAL aChSacle) const
{
	return cElHomographie
               (
                   mHX.MulCste(aChSacle),
                   mHY.MulCste(aChSacle),
                   mHZ.MulXY(1.0/aChSacle)
               );
}
cElHomographie cElHomographie::Homotie(Pt2dr aP,REAL aSc)
{
   return cElHomographie
          (
                 cElComposHomographie( aSc,    0,   aP.x),
                 cElComposHomographie(   0,  aSc,   aP.y),
                 cElComposHomographie(   0,    0,      1)
          );
}
cElHomographie cElHomographie::Id()
{
   return cElHomographie
          (
                 cElComposHomographie(1,0,0),
                 cElComposHomographie(0,1,0),
                 cElComposHomographie(0,0,1)
          );
}
cElHomographie cElHomographie::FromMatrix(const ElMatrix<REAL> & M)
{
     
   return cElHomographie
          (
                 cElComposHomographie( M(0,0),M(1,0),M(2,0)),
                 cElComposHomographie( M(0,1),M(1,1),M(2,1)),
                 cElComposHomographie( M(0,2),M(1,2),M(2,2))
          );
}
cElHomographie cElHomographie::read(ELISE_fp & aFile)
{
    cElComposHomographie aHX(0,0,0);
    cElComposHomographie aHY(0,0,0);
    cElComposHomographie aHZ(0,0,0);
    aHX = cElComposHomographie::read(aFile);
    aHY = cElComposHomographie::read(aFile);
    aHZ = cElComposHomographie::read(aFile);

    return cElHomographie(aHX,aHY,aHZ);
}
cElHomographie cElHomographie::SomPondHom(const std::vector<cElHomographie> & aVH,const std::vector<double> & aVP)
{
    ELISE_ASSERT(aVH.size()==aVP.size(),"SomPondHom");
    double aSomHXx = 0;
    double aSomHXy = 0;
    double aSomHX1 = 0;
    double aSomHYx = 0;
    double aSomHYy = 0;
    double aSomHY1 = 0;
    double aSomHZx = 0;
    double aSomHZy = 0;
    double aSomHZ1 = 0;

    double aSomPds = 0;

    for (int aK=0 ; aK<int(aVH.size()) ; aK++)
    {
        double aPds = aVP[aK];

        const cElComposHomographie &  aHX = aVH[aK].HX();
        const cElComposHomographie &  aHY = aVH[aK].HY();
        const cElComposHomographie &  aHZ = aVH[aK].HZ();

        double aMul = aPds / aHZ.Coeff1();


        aSomHXx += aHX.CoeffX() * aMul;
        aSomHXy += aHX.CoeffY() * aMul;
        aSomHX1 += aHX.Coeff1() * aMul;

        aSomHYx += aHY.CoeffX() * aMul;
        aSomHYy += aHY.CoeffY() * aMul;
        aSomHY1 += aHY.Coeff1() * aMul;

        aSomHZx += aHZ.CoeffX() * aMul;
        aSomHZy += aHZ.CoeffY() * aMul;
        aSomHZ1 += aHZ.Coeff1() * aPds;

        aSomPds += aPds;
    }


    return cElHomographie
           (
               cElComposHomographie(aSomHXx/aSomPds,aSomHXy/aSomPds,aSomHX1/aSomPds),
               cElComposHomographie(aSomHYx/aSomPds,aSomHYy/aSomPds,aSomHY1/aSomPds),
               cElComposHomographie(aSomHZx/aSomPds,aSomHZy/aSomPds,aSomHZ1/aSomPds)
           );
}
cElHomographie cElHomographie::Inverse() const
{
/*
      ElPackHomologue aPack;

      AddForInverse(aPack,Pt2dr( 1, 1));
      AddForInverse(aPack,Pt2dr(-1, 1));
      AddForInverse(aPack,Pt2dr( 1,-1));
      AddForInverse(aPack,Pt2dr(-1,-1));

      return cElHomographie(aPack,true);
*/
      REAL a = mHX.mX, b = mHX.mY, c = mHX.m1;
      REAL d = mHY.mX, e = mHY.mY, f = mHY.m1;
      REAL ALPHA = mHZ.mX, BETA = mHZ.mY;

     cElComposHomographie  IX (    e-BETA * f    ,   -b+ BETA*c     ,   b*f-e*c  );
     cElComposHomographie  IY (    -d+ALPHA*f    ,    a-ALPHA*c     ,   d*c-a*f  );
     cElComposHomographie  IZ (  BETA*d-ALPHA*e  ,  ALPHA*b-BETA*a  ,   a*e-b*d  );

     return cElHomographie(IX,IY,IZ);
}
void cModeleAnalytiqueComp::SolveHomographie(const ElPackHomologue &  aPackHom)
{
   mHomogr = cElHomographie(aPackHom,mModele.HomographieL2().Val());

   mBoxPoly = Box2dr(Pt2dr(0,0),Pt2dr(0,0));
   if (mDegrPolAdd >0)
   {
      ElPackHomologue aPck2 = aPackHom;
      Pt2dr aPMin(1e5,1e5);
      Pt2dr aPMax(-1e5,-1e5);
      for 
      (
         ElPackHomologue::iterator iT = aPck2.begin();
         iT != aPck2.end();
         iT++
      )
      {
           iT->P1() = mHomogr.Direct(iT->P1());
           aPMin.SetInf(iT->P1());
           aPMax.SetSup(iT->P1());
      }
      double anAmpl = ElMax(dist8(aPMin),dist8(aPMax));
      mBoxPoly = Box2dr(aPMin,aPMax);
      bool aPL2 = mModele.PolynomeL2().Val();
      mPolX = aPck2.FitPolynome(aPL2,mDegrPolAdd,anAmpl,true);
      mPolY = aPck2.FitPolynome(aPL2,mDegrPolAdd,anAmpl,false);

   }
// Polynome2dReal  ElPackHomologue::FitPolynome

   {
      cElXMLFileIn aFileXML(mNameXML);
      aFileXML.PutElHomographie(mHomogr,"Homographie");

      if (mDegrPolAdd >0)
      {
         cElXMLFileIn::cTag aTag(aFileXML,"PolynomeCompl"); aTag.NoOp();
         
         aFileXML.PutPoly(mPolX,"XPoly");
         aFileXML.PutPoly(mPolY,"YPoly");
         
         aFileXML.PutPt2dr(mBoxPoly._p0,"PMinBox");
         aFileXML.PutPt2dr(mBoxPoly._p1,"PMaxBox");
      }
   } 
   MakeInverseModele();

   // Verification de la correction du calcul  de l'inverse
    if (0)
   {
      int aNb=10;
      double aEps = 0.05;
      for (int aKx=0 ; aKx<= aNb ; aKx++)
      {
          double aPdsX = ElMax(aEps,ElMin(1-aEps,aKx/double(aNb)));
          for (int aKy=0 ; aKy<= aNb ; aKy++)
          {
              
              double aPdsY = ElMax(aEps,ElMin(1-aEps,aKy/double(aNb)));
              Pt2dr aPRas = Pt2dr (mSzGl.x*aPdsX, mSzGl.x*aPdsY);
              Pt2dr aPTer = mGeoTer.RDiscToR2(aPRas);
              Pt2dr aP1 = Direct(Pt2dr(aPTer));

              Pt2dr aQ2 =  Inverse(aP1);
              double anEr = euclid(aPTer,aQ2);
              if (anEr>0.05)
              {
                 std::cout << "Erreur = " << anEr << "\n";
                 std::cout <<  aPRas <<  aPTer << "\n";
                 
                 Pt2dr aP0 = aPTer;
                 Pt2dr aP1 = mGeom2.CorrigeDist1(aP0);
                 Pt2dr aP2 = CorrecDirecte(aP1);
                 Pt2dr aP3 = mGeom2.InvCorrDist2(aP2);

                 Pt2dr aQ0 = mGeom2.InvCorrDist1(aP1);
                 std::cout << "pq0= " << euclid(aP0,aQ0) << aP0 << aQ0 << "\n";

                 Pt2dr aQ1 = CorrecInverse(aP2);
                 std::cout << "pq1= " << euclid(aP1,aQ1) << aP1 << aQ1 << "\n";

                 Pt2dr aQ2 =  mGeom2.CorrigeDist2(aP3);
                 std::cout << "pq2= " << euclid(aP2,aQ2) << aP2 << aQ2 << "\n";
                 std::cout << "P3 " << aP3 << "\n";

                  ELISE_ASSERT(false,"MakeInverseModele Pb!!");
              }
         }
     }
   }

   if (mModele.ExportImage().Val() || mModele.ReuseResiduelle().Val() )
   {
      Im2D_REAL4 anImX(mSzGl.x,mSzGl.y);
      TIm2D<REAL4,REAL8> aTImX(anImX);
      Im2D_REAL4 anImY(mSzGl.x,mSzGl.y);
      TIm2D<REAL4,REAL8> aTImY(anImY);

      Pt2di aPRas;
      double aPX0[2] = {0.0,0.0};
      for (aPRas.x=0 ; aPRas.x<mSzGl.x ; aPRas.x++)
      {
          for (aPRas.y=0 ; aPRas.y<mSzGl.y ; aPRas.y++)
          {
              Pt2dr aPTer = mGeoTer.DiscToR2(aPRas);
              Pt2dr aP1 = Direct(Pt2dr(aPTer));


              Pt2dr aP2 ;
              if (mModele.ReuseResiduelle().Val())
                 aP2 = mGeom2.Objet2ImageInit_Euclid(Pt2dr(aPTer),aPX0);
              else
                 aP2 = mGeom2.InvCorrDist2(mGeom2.CorrigeDist1(aPTer));
              double aPx[2];
              aPx[0] = aP1.x- aP2.x;
              aPx[1] = aP1.y- aP2.y;
              mGeoTer.PxReel2PxDisc(aPx,aPx);
              
               
              aTImX.oset(aPRas,aPx[0]);
              aTImY.oset(aPRas,aPx[1]);
          }
      }
      if (mModele.ExportImage().Val())
      {
         Tiff_Im::Create8BFromFonc(mNameImX,mSzGl,ToUC(anImX.in()));
         Tiff_Im::Create8BFromFonc(mNameImY,mSzGl,ToUC(anImY.in()));
      }
      if (mModele.ReuseResiduelle().Val())
      {
         Tiff_Im::Create8BFromFonc(mNameResX,mSzGl,ToUC(anImX.in()-ImPx(0)));
         Tiff_Im::Create8BFromFonc(mNameResY,mSzGl,ToUC(anImY.in()-ImPx(1)));
      }
  } 
  if (mExpModeleGlobal)
  {

      const cGeomDiscFPx &  aG = mAppli.GeomDFPxInit() ;
      Box2dr aBox = aG.BoxEngl();
      double aME =mModele.MailleExport().Val();


      cDbleGrid  aGrid
                 (
                     true, // P0P1 Direct par defaut maintien du comp actuel
                     true,
                     aBox._p0,aBox._p1,
                     Pt2dr(aME,aME),
                     *this,
                     "toto"
                 );


     std::string aNameXML =  mAppli.ICNM()->Assoc1To2
                             (
			        mModele.FCND_ExportModeleGlobal().Val(),
                                mAppli.PDV1()->Name(),
                                mAppli.PDV2()->Name(),
				true
			     );
     aGrid.SaveXML(mAppli.FullDirResult() + aNameXML);
  }

}
cElHomographie  cElHomographie::RobustInit(double & aDMIn,double * aQuality,const ElPackHomologue & aPack,bool & Ok ,int aNbTestEstim, double aPerc,int aNbMaxPts)
{
   cElHomographie aRes = cElHomographie::Id();
   Ok = false;
   Pt2dr aCdg(0,0);
   for (ElPackHomologue::tCstIter itH=aPack.begin() ; itH!=aPack.end() ; itH++)
   {
       aCdg = aCdg + itH->P1();
   }

   aCdg = aCdg / double(aPack.size());
   
   std::vector<std::pair<Pt2dr,Pt2dr> > aV00;
   std::vector<std::pair<Pt2dr,Pt2dr> > aV01;
   std::vector<std::pair<Pt2dr,Pt2dr> > aV10;
   std::vector<std::pair<Pt2dr,Pt2dr> > aV11;
   std::vector<std::pair<Pt2dr,Pt2dr> > aVAll;

   int aNbPtsTot = aPack.size();


   int aCpt = 0;
   for (ElPackHomologue::tCstIter itH=aPack.begin() ; itH!=aPack.end() ; itH++)
   {
        Pt2dr aP1 = itH->P1();
        Pt2dr aP2 = itH->P2();
        std::pair<Pt2dr,Pt2dr> aPair(aP1,aP2);

        if (  (((aCpt-1)*aNbMaxPts)/aNbPtsTot)  !=  ((aCpt*aNbMaxPts)/aNbPtsTot))
        {
            aVAll.push_back(aPair);
        }

        if (aP1.x < aCdg.x)
        {
            if (aP1.y < aCdg.y) aV00.push_back(aPair);
            else                aV01.push_back(aPair);
        }
        else
        {
            if (aP1.y < aCdg.y) aV10.push_back(aPair);
            else                aV11.push_back(aPair);
        }
        aCpt++;
   }


   if (aV00.empty()  || aV01.empty()  || aV10.empty()  || aV11.empty()  )
      return aRes;


   aDMIn = 1e30;
   int aNbPts = aVAll.size();
   int aNbKth = ElMax(1,ElMin(aNbPts-1,round_ni((aPerc/100.0) * aNbPts)));
   std::vector<double> aVDist;

   if (aNbMaxPts<aNbPtsTot)
      aNbTestEstim = (aNbTestEstim*aNbPtsTot) / aNbMaxPts;

   // int aKMIN = -1;
   std::vector<double> aVD; // For tuning and show in if(0) ...
   while (aNbTestEstim)
   {
       int aK00 = NRrandom3(aV00.size());
       int aK01 = NRrandom3(aV01.size());
       int aK10 = NRrandom3(aV10.size());
       int aK11 = NRrandom3(aV11.size());

       ElPackHomologue aP4;
       AddPair(aP4,aV00[aK00]);
       AddPair(aP4,aV01[aK01]);
       AddPair(aP4,aV10[aK10]);
       AddPair(aP4,aV11[aK11]);

       cElHomographie aSol = cElHomographie(aP4,true);

       aVDist.clear();
       for (int aK=0 ; aK< aNbPts ; aK++)
       {
          Pt2dr aP1 = aVAll[aK].first;
          Pt2dr aP2 = aVAll[aK].second;

/*
          Pt2dr aDif = aP2 -aSol.Direct(aP1);
          double aDx = ElAbs(aDif.x);
          double aDy = ElAbs(aDif.y);
          double aDist =  (aDx+aDy + ElMax(aDx,aDy))/ 2.0;
*/
          double aDist = QuickDist(aP2 -aSol.Direct(aP1));
          aVDist.push_back(aDist);
       }
       
       double aSom = MoyKPPVal(aVDist,aNbKth);
       
       aVD.push_back(aSom);

       //std::cout << "Robust:Hom:SOM = " << aDMIn << " " << aSom << "\n";

       if (aSom <aDMIn)
       {
          aRes = aSol;
          aDMIn = aSom;
       }
       aNbTestEstim--;
   }


   // double aDMinInit = aDMIn;
   ElPackHomologue aPckPds;
   for (int anIterL2 = 0 ; anIterL2 < 4 ; anIterL2++)
   {
       aPckPds = ElPackHomologue();
       aVDist.clear();
       int aCpt = 0;
       for (ElPackHomologue::tCstIter itH=aPack.begin() ; itH!=aPack.end() ; itH++)
       {
           Pt2dr aP1 = itH->P1();
           Pt2dr aP2 = itH->P2();
           double aDist = QuickDist(aP2 -aRes.Direct(aP1));
           aVDist.push_back(aDist);

           double aPds = 1/ (1+ 4*ElSquare(aDist/aDMIn));
           aPckPds.Cple_Add(ElCplePtsHomologues(aP1,aP2,aPds));
           aCpt++;
       }
       ELISE_ASSERT(aNbPtsTot==aPack.size() ,"KKKKK ????");
       int aKTh = round_ni(aNbPtsTot * (aPerc/100.0));

       ELISE_ASSERT(int(aVDist.size())==aNbPtsTot,"Compat MoyKPPVal/SplitArrounKthValue");
       aDMIn = MoyKPPVal(aVDist,aKTh);

       aRes = cElHomographie(aPckPds,true);
   }

   if (aQuality)
   {
      std::vector<double> aVEstim;
      int aNbTestValid = 71;
      for (int aKTest = 0 ; aKTest <aNbTestValid ; aKTest++)
      {
          ElPackHomologue aPckPdsA;
          ElPackHomologue aPckPdsB;
          cRandNParmiQ  aSelec(aNbPtsTot/2,aNbPtsTot);

          for (ElPackHomologue::tCstIter itH=aPack.begin() ; itH!=aPack.end() ; itH++)
          {
              Pt2dr aP1 = itH->P1();
              Pt2dr aP2 = itH->P2();
              double aDist = QuickDist(aP2 -aRes.Direct(aP1));
              aVDist.push_back(aDist);

              double aPds = 1/ sqrt(1+ ElSquare(aDist/aDMIn));
              // if (NRrandom3() > 0.5) 
              if (aSelec.GetNext())
                  aPckPdsA.Cple_Add(ElCplePtsHomologues(aP1,aP2,aPds));
              else
                  aPckPdsB.Cple_Add(ElCplePtsHomologues(aP1,aP2,aPds));

           
          }
          cElHomographie aResA = cElHomographie(aPckPdsA,true);
          cElHomographie aResB = cElHomographie(aPckPdsB,true);

          double aSomDist = 0; 
          for (ElPackHomologue::tCstIter itH=aPack.begin() ; itH!=aPack.end() ; itH++)
          {
              Pt2dr aP1 = itH->P1();

              Pt2dr  aQ   = aRes.Direct(aP1);
              Pt2dr  aQA  = aResA.Direct(aP1);
              Pt2dr  aQB  = aResB.Direct(aP1);
              double aDist = (QuickDist(aQ-aQA) + QuickDist(aQ-aQB) + QuickDist(aQB-aQA)) / 3.0;
              aSomDist += aDist;
          }
          aSomDist /= aNbPtsTot;
          aVEstim.push_back(aSomDist);
      }
      *aQuality  = MedianeSup(aVEstim);
   }


   Ok= true;
   return aRes;
}