C++ vpImage::getValue方法代码示例

bool
vpImageSimulator::getPixel(vpImage<unsigned char> &Isrc, 
			   const vpImagePoint &iP, unsigned char &Ipixelplan)
{
  //test si pixel dans zone projetee
  bool inside = false;
  for(unsigned int i = 0 ; i < listTriangle.size() ; i++)
      if(listTriangle[i].inTriangle(iP)){
          inside = true;
          break;
      }
  if(!inside) return false;

//  if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
//    return false;

  //methoed algebrique
  double z;

  //calcul de la profondeur de l'intersection
  z = distance/(normal_Cam_optim[0]*iP.get_u()+normal_Cam_optim[1]*iP.get_v()+normal_Cam_optim[2]);
  //calcul coordonnees 3D intersection
  Xinter_optim[0]=iP.get_u()*z;
  Xinter_optim[1]=iP.get_v()*z;
  Xinter_optim[2]=z;

  //recuperation des coordonnes de l'intersection dans le plan objet
  //repere plan object : 
  //	centre = X0_2_optim[i] (premier point definissant le plan)
  //	base =  u:(X[1]-X[0]) et v:(X[3]-X[0])
  //ici j'ai considere que le plan est un rectangle => coordonnees sont simplement obtenu par un produit scalaire
  double u = 0, v = 0;
  for(unsigned int i = 0; i < 3; i++)
  {
    double diff = (Xinter_optim[i]-X0_2_optim[i]);
    u += diff*vbase_u_optim[i];
    v += diff*vbase_v_optim[i];
  }
  u = u/(euclideanNorm_u*euclideanNorm_u);
  v = v/(euclideanNorm_v*euclideanNorm_v);

  if( u > 0 && v > 0 && u < 1. && v < 1.)
  {
    double i2,j2;
    i2=v*(Isrc.getHeight()-1);
    j2=u*(Isrc.getWidth()-1);
    if (interp == BILINEAR_INTERPOLATION)
      Ipixelplan = Isrc.getValue(i2,j2);
    else if (interp == SIMPLE)
      Ipixelplan = Isrc[(unsigned int)i2][(unsigned int)j2];
    return true;
  }
  else
    return false;
}

void vpTemplateTrackerSSDESM::trackNoPyr(const vpImage<unsigned char> &I)
{
  double erreur=0;
  unsigned int Nbpoint=0;

  if(blur)
    vpImageFilter::filter(I, BI,fgG,taillef);
  vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
  vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);

  double IW,dIWx,dIWy;
  double Tij;
  unsigned int iteration=0;
  int i,j;
  double i2,j2;
  double alpha=2.;
  do
  {
    Nbpoint=0;
    erreur=0;
    dp=0;
    HDir=0;
    GDir=0;
    GInv=0;
    Warp->computeCoeff(p);
    for(unsigned int point=0;point<templateSize;point++)
    {
      i=ptTemplate[point].y;
      j=ptTemplate[point].x;
      X1[0]=j;X1[1]=i;

      Warp->computeDenom(X1,p);
      Warp->warpX(X1,X2,p);

      j2=X2[0];i2=X2[1];
      if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
      {
        //INVERSE
        Tij=ptTemplate[point].val;
        if(!blur)
          IW=I.getValue(i2,j2);
        else
          IW=BI.getValue(i2,j2);
        Nbpoint++;
        double er=(Tij-IW);
        for(unsigned int it=0;it<nbParam;it++)
          GInv[it]+=er*ptTemplate[point].dW[it];

        erreur+=er*er;

        //DIRECT
        //dIWx=dIx.getValue(i2,j2);
        //dIWy=dIy.getValue(i2,j2);

        dIWx=dIx.getValue(i2,j2)+ptTemplate[point].dx;
        dIWy=dIy.getValue(i2,j2)+ptTemplate[point].dy;

        //Calcul du Hessien
        //Warp->dWarp(X1,X2,p,dW);
        Warp->dWarpCompo(X1,X2,p,ptTemplateCompo[point].dW,dW);

        double *tempt=new double[nbParam];
        for(unsigned int it=0;it<nbParam;it++)
          tempt[it]=dW[0][it]*dIWx+dW[1][it]*dIWy;

        for(unsigned int it=0;it<nbParam;it++)
          for(unsigned int jt=0;jt<nbParam;jt++)
            HDir[it][jt]+=tempt[it]*tempt[jt];

        for(unsigned int it=0;it<nbParam;it++)
          GDir[it]+=er*tempt[it];
        delete[] tempt;
      }


    }
    if(Nbpoint==0) {
      //std::cout<<"plus de point dans template suivi"<<std::endl;
      throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
    }

    vpMatrix::computeHLM(HDir,lambdaDep,HLMDir);

    try
    {
      //dp=(HLMInv+HLMDir).inverseByLU()*(GInv+GDir);
      //dp=HLMInv.inverseByLU()*GInv+HLMDir.inverseByLU()*GDir;
      //dp=HLMInv.inverseByLU()*GInv;
      dp=(HLMDir).inverseByLU()*(GDir);
    }
    catch(vpException &e)
    {
      //std::cout<<"probleme inversion"<<std::endl;
      throw(e);
    }

    dp=gain*dp;
    if(useBrent)
    {
      alpha=2.;
//.........这里部分代码省略.........

void vpTemplateTrackerZNCCInverseCompositional::initHessienDesired(const vpImage<unsigned char> &I)
{
  initCompInverse(I);

  if(blur)
    vpImageFilter::filter(I, BI,fgG,taillef);
  vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
  vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);

  vpImage<double> dIxx,dIxy,dIyx,dIyy;
  vpImageFilter::getGradX(dIx, dIxx, fgdG,taillef);
  vpImageFilter::getGradY(dIx, dIxy, fgdG,taillef);

  vpImageFilter::getGradX(dIy, dIyx, fgdG,taillef);
  vpImageFilter::getGradY(dIy, dIyy, fgdG,taillef);

  Warp->computeCoeff(p);
  double Ic,dIcx=0.,dIcy=0.;
  double Iref;
  int i,j;
  double i2,j2;
  int Nbpoint=0;

  double moyIref=0;
  double moyIc=0;
  double denom=0;
  moydIrefdp.resize(nbParam);	moydIrefdp=0;
  vpMatrix moyd2Iref(nbParam,nbParam);moyd2Iref=0;

  for(unsigned int point=0;point<templateSize;point++)
  {
    i=ptTemplate[point].y;
    j=ptTemplate[point].x;
    X1[0]=j;X1[1]=i;
    X2[0]=j;X2[1]=i;

    Warp->computeDenom(X1,p);

    j2=X2[0];i2=X2[1];

    if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
    {
      Iref=ptTemplate[point].val;

      if(!blur)
        Ic=I.getValue(i2,j2);
      else
        Ic=BI.getValue(i2,j2);

      Nbpoint++;
      moyIref+=Iref;
      moyIc+=Ic;

      for(unsigned int it=0;it<nbParam;it++)
        moydIrefdp[it]+=ptTemplate[point].dW[it];


      Warp->dWarp(X1,X2,p,dW);
      double *tempt=new double[nbParam];
      for(unsigned int it=0;it<nbParam;it++)
        tempt[it]=dW[0][it]*dIcx+dW[1][it]*dIcy;
      double d_Ixx=dIxx.getValue(i2,j2);
      double d_Iyy=dIyy.getValue(i2,j2);
      double d_Ixy=dIxy.getValue(i2,j2);

      for(unsigned int it=0;it<nbParam;it++)
        for(unsigned int jt=0;jt<nbParam;jt++)
        {
          moyd2Iref[it][jt] +=(dW[0][it]*(dW[0][jt]*d_Ixx+dW[1][jt]*d_Ixy)
              +dW[1][it]*(dW[0][jt]*d_Ixy+dW[1][jt]*d_Iyy));
        }

      delete[] tempt;


    }
  }

  moyIref=moyIref/Nbpoint;
  moydIrefdp=moydIrefdp/Nbpoint;
  moyd2Iref=moyd2Iref/Nbpoint;
  moyIc=moyIc/Nbpoint;
  Hdesire=0;
  double covarIref=0,covarIc=0;
  double sIcIref=0;
  vpColVector sIcdIref(nbParam);sIcdIref=0;
  vpMatrix sIcd2Iref(nbParam,nbParam);sIcd2Iref=0;
  vpMatrix sdIrefdIref(nbParam,nbParam);sdIrefdIref=0;
  for(unsigned int point=0;point<templateSize;point++)
  {
    i=ptTemplate[point].y;
    j=ptTemplate[point].x;
    X1[0]=j;X1[1]=i;
    X2[0]=j;X2[1]=i;

    Warp->computeDenom(X1,p);

    j2=X2[0];i2=X2[1];

    if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
//.........这里部分代码省略.........

void vpTemplateTrackerZNCCInverseCompositional::trackNoPyr(const vpImage<unsigned char> &I)
{
  if(blur)
    vpImageFilter::filter(I, BI,fgG,taillef);

  //double erreur=0;
  vpColVector dpinv(nbParam);
  double Ic;
  double Iref;
  unsigned int iteration=0;
  int i,j;
  double i2,j2;
  initPosEvalRMS(p);
  do
  {
    unsigned int Nbpoint=0;
    //erreur=0;
    G=0;
    Warp->computeCoeff(p);
    double moyIref=0;
    double moyIc=0;
    for(unsigned int point=0;point<templateSize;point++)
    {
      i=ptTemplate[point].y;
      j=ptTemplate[point].x;
      X1[0]=j;X1[1]=i;

      Warp->computeDenom(X1,p);
      Warp->warpX(X1,X2,p);

      j2=X2[0];i2=X2[1];
      if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
      {
        Iref=ptTemplate[point].val;

        if(!blur)
          Ic=I.getValue(i2,j2);
        else
          Ic=BI.getValue(i2,j2);

        Nbpoint++;
        moyIref+=Iref;
        moyIc+=Ic;
      }


    }
    if(Nbpoint > 0)
    {
      moyIref=moyIref/Nbpoint;
      moyIc=moyIc/Nbpoint;
      double sIcIref=0;
      double covarIref=0,covarIc=0;
      vpColVector sIcdIref(nbParam);sIcdIref=0;
      vpColVector sIrefdIref(nbParam);sIrefdIref=0;


      for(unsigned int point=0;point<templateSize;point++)
      {
        i=ptTemplate[point].y;
        j=ptTemplate[point].x;
        X1[0]=j;X1[1]=i;

        Warp->computeDenom(X1,p);
        Warp->warpX(X1,X2,p);

        j2=X2[0];i2=X2[1];
        if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
        {
          Iref=ptTemplate[point].val;

          if(!blur)
            Ic=I.getValue(i2,j2);
          else
            Ic=BI.getValue(i2,j2);


          double prod=(Ic-moyIc);
          for(unsigned int it=0;it<nbParam;it++)
            sIcdIref[it]+=prod*(ptTemplate[point].dW[it]-moydIrefdp[it]);
          for(unsigned int it=0;it<nbParam;it++)
            sIrefdIref[it]+=(Iref-moyIref)*(ptTemplate[point].dW[it]-moydIrefdp[it]);

          //double er=(Iref-Ic);
          //erreur+=(er*er);
          //denom+=(Iref-moyIref)*(Iref-moyIref)*(Ic-moyIc)*(Ic-moyIc);
          covarIref+=(Iref-moyIref)*(Iref-moyIref);
          covarIc+=(Ic-moyIc)*(Ic-moyIc);
          sIcIref+=(Iref-moyIref)*(Ic-moyIc);
        }


      }
      covarIref=sqrt(covarIref);
      covarIc=sqrt(covarIc);
      double denom=covarIref*covarIc;

      //if(denom==0.0)
      if (std::fabs(denom) <= std::numeric_limits<double>::epsilon())
      {
//.........这里部分代码省略.........

void vpTemplateTrackerSSDInverseCompositional::trackNoPyr(const vpImage<unsigned char> &I)
{
  double erreur=0;
  unsigned int Nbpoint=0;
  if(blur)
    vpImageFilter::filter(I, BI,fgG,taillef);

  vpColVector dpinv(nbParam);
  double IW;
  double Tij;
  unsigned int iteration=0;
  int i,j;
  double i2,j2;
  double alpha=2.;
  //vpTemplateTrackerPointtest *pt;
  initPosEvalRMS(p);

  vpTemplateTrackerPoint *pt;
  do
  {
    Nbpoint=0;
    erreur=0;
    dp=0;
    Warp->computeCoeff(p);
    for(unsigned int point=0;point<templateSize;point++)
    {
      if((!useTemplateSelect)||(ptTemplateSelect[point]))
      {
        //pt=&ptTemplatetest[point];
        pt=&ptTemplate[point];
        i=pt->y;
        j=pt->x;
        X1[0]=j;X1[1]=i;
        Warp->computeDenom(X1,p);
        Warp->warpX(X1,X2,p);
        j2=X2[0];i2=X2[1];

        if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
        {
          Tij=pt->val;
          if(!blur)
            IW=I.getValue(i2,j2);
          else
            IW=BI.getValue(i2,j2);
          Nbpoint++;
          double er=(Tij-IW);
          for(unsigned int it=0;it<nbParam;it++)
            dp[it]+=er*pt->HiG[it];

          erreur+=er*er;
        }
      }
    }
    //std::cout << "npoint: " << Nbpoint << std::endl;
    if(Nbpoint==0) {
      //std::cout<<"plus de point dans template suivi"<<std::endl;
      deletePosEvalRMS();
      throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
    }
    dp=gain*dp;
    //std::cout<<erreur/Nbpoint<<","<<GetCost(I,p)<<std::endl;
    if(useBrent)
    {
      alpha=2.;
      computeOptimalBrentGain(I,p,erreur/Nbpoint,dp,alpha);
      dp=alpha*dp;
    }
    Warp->getParamInverse(dp,dpinv);
    Warp->pRondp(p,dpinv,p);
    iteration++;

    computeEvalRMS(p);
    //std::cout << "iteration: " << iteration << " max: " << iterationMax << std::endl;
    //std::cout << "evolRMS: " <<  evolRMS << " threshold: " << threshold_RMS << std::endl;
  }
  while(/*( erreur_prec-erreur<50) &&*/ (iteration < iterationMax)&&(evolRMS>threshold_RMS));

  nbIteration=iteration;
  deletePosEvalRMS();
}

//.........这里部分代码省略.........
            d2Prt[indd2]=0.0;
            indd2++;
          }
        }
      }

#pragma omp barrier
#pragma omp for reduction(+:Nbpoint)
#endif
      for(int point=0;point<(int)templateSize;point++)
      {
        vpColVector x1(2),x2(2);
        double i2,j2;
        double IW;
        int cr,ct;
        double er,et;

        x1[0]=(double)ptTemplate[point].x;
        x1[1]=(double)ptTemplate[point].y;

        Warp->computeDenom(x1,p); // A modif pour parallelisation mais ne pose pas de pb avec warp utilises dans DECSA
        Warp->warpX(x1,x2,p);

        j2=x2[0];
        i2=x2[1];

        if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
        {

          //if(m_ptCurrentMask == NULL ||(m_ptCurrentMask->getWidth() == I.getWidth() && m_ptCurrentMask->getHeight() == I.getHeight() && (*m_ptCurrentMask)[(unsigned int)i2][(unsigned int)j2] > 128))
          {
            Nbpoint++;
            if(!blur)
              IW=(double)I.getValue(i2,j2);
            else
              IW=BI.getValue(i2,j2);

            ct=ptTemplateSupp[point].ct;
            et=ptTemplateSupp[point].et;
            double tmp = IW*(((double)Nc)-1.f)/255.f;
            cr=(int)tmp;
            er=tmp-(double)cr;

            if( (ApproxHessian==HESSIAN_NONSECOND||hessianComputation==vpTemplateTrackerMI::USE_HESSIEN_DESIRE) && (ptTemplateSelect[point] || !useTemplateSelect) )
            {
#if defined(USE_OPENMP_MI_INVCOMP) && defined(VISP_HAVE_OPENMP)
              vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(Prtbis, dPrtbis, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam, bspline);
#else
              vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(Prt, dPrt, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam, bspline);
#endif
            }
            else if (ptTemplateSelect[point] || !useTemplateSelect)
            {
              if(bspline==3){
#if defined(USE_OPENMP_MI_INVCOMP) && defined(VISP_HAVE_OPENMP)
                vpTemplateTrackerMIBSpline::PutTotPVBspline3(Prtbis, dPrtbis, d2Prtbis, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam);
#else
                vpTemplateTrackerMIBSpline::PutTotPVBspline3(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam);
#endif
                //                        {
                //                            // ################### AY : Optim
                //                            if(et>0.5){ct++;}
                //                            if(er>0.5){cr++;}
                //                            int index = (cr*Nc+ct)*totParam;
                //                            double *ptb = &PrtTout[index];
                //                            #endif

void vpTemplateTrackerMIInverseCompositional::initHessienDesired(const vpImage<unsigned char> &I)
{
  initCompInverse(I);

  double erreur=0;
  int Nbpoint=0;

  double i2,j2;
  double Tij;
  double IW;
  int cr,ct;
  double er,et;

  int i,j;

  Nbpoint=0;
  erreur=0;

  if(blur)
    vpImageFilter::filter(I, BI,fgG,taillef);

  zeroProbabilities();
  Warp->computeCoeff(p);

  // AY : Optim
  //    unsigned int totParam = (bspline * bspline)*(1+nbParam+nbParam*nbParam);
  //    unsigned int size = (1 + nbParam + nbParam*nbParam)*bspline;
  //    double *ptb;

  for(unsigned int point=0;point<templateSize;point++)
  {
    i=ptTemplate[point].y;
    j=ptTemplate[point].x;
    X1[0]=j;X1[1]=i;

    Warp->computeDenom(X1,p);
    Warp->warpX(X1,X2,p);

    j2=X2[0];i2=X2[1];

    if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
    {
      Nbpoint++;
      Tij=ptTemplate[point].val;

      if(blur)
        IW=BI.getValue(i2,j2);
      else
        IW=I.getValue(i2,j2);

      ct=ptTemplateSupp[point].ct;
      et=ptTemplateSupp[point].et;
      cr=(int)((IW*(Nc-1))/255.);
      er=((double)IW*(Nc-1))/255.-cr;

      //calcul de l'erreur
      erreur+=(Tij-IW)*(Tij-IW);

      if( ApproxHessian==HESSIAN_NONSECOND && (ptTemplateSelect[point] || !useTemplateSelect) )
      {
        vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam, bspline);
      }
      else if ((ApproxHessian==HESSIAN_0||ApproxHessian==HESSIAN_NEW) && (ptTemplateSelect[point] || !useTemplateSelect))
      {
        if(bspline==3){
          vpTemplateTrackerMIBSpline::PutTotPVBspline3(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam);
          //                    {
          //                        if(et>0.5){ct++;}
          //                        if(er>0.5){cr++;}
          //                        int index = (cr*Nc+ct)*totParam;
          //                        double *ptb = &PrtTout[index];
          //                        vpTemplateTrackerMIBSpline::PutTotPVBspline3(ptb, er, ptTemplateSupp[point].BtInit, size);
          //                    }
        }
        else{
          vpTemplateTrackerMIBSpline::PutTotPVBspline4(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam);

          //                    {
          //                        // ################### AY : Optim
          //                        unsigned int index = (cr*Nc+ct)*totParam;
          //                        ptb = &PrtTout[index];
          //                        vpTemplateTrackerMIBSpline::PutTotPVBspline4(ptb, er, ptTemplateSupp[point].BtInit, size);
          //                        // ###################
          //                    }
        }
      }
      else if (ptTemplateSelect[point] || !useTemplateSelect)
        vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(PrtTout, cr, er, ct, et, Nc,nbParam, bspline);
    }
  }

  double MI;
  computeProba(Nbpoint);
  computeMI(MI);
  computeHessien(Hdesire);

  double lambda=lambdaDep;

  vpMatrix::computeHLM(Hdesire,lambda,HLMdesire);

//.........这里部分代码省略.........

void vpTemplateTrackerMIESM::initHessienDesired(const vpImage<unsigned char> &I)
{
  initCompInverse();
  std::cout<<"Initialise Hessian at Desired position..."<<std::endl;

  dW=0;

  double erreur=0;
  int Nbpoint=0;

  double i2,j2;
  double Tij;
  double IW,dx,dy;
  //int cr,ct;
  //double er,et;
  int i,j;

  Nbpoint=0;
  erreur=0;

  if(blur)
    vpImageFilter::filter(I, BI,fgG,taillef);

  zeroProbabilities();

  Warp->computeCoeff(p);
  for(unsigned int point=0;point<templateSize;point++)
  {
    i=ptTemplate[point].y;
    j=ptTemplate[point].x;
    X1[0]=j;X1[1]=i;

    Warp->computeDenom(X1,p);
    Warp->warpX(X1,X2,p);

    j2=X2[0];i2=X2[1];

    if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
    {
      Nbpoint++;
      Tij=ptTemplate[point].val;
      //Tij=Iterateurvecteur->val;
      if(blur)
        IW=BI.getValue(i2,j2);
      else
        IW=I.getValue(i2,j2);

      //ct=ptTemplateSupp[point].ct;
      //et=ptTemplateSupp[point].et;
      //cr=(int)((IW*(Nc-1))/255.);
      //er=((double)IW*(Nc-1))/255.-cr;

      //            if(ApproxHessian==vpTemplateTrackerMI::HESSIAN_NONSECOND) // cas à tester AY
      //                vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout,cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam, bspline);
      //            else
      //                vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout,cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam, bspline);

      //            vpTemplateTrackerMIBSpline::computeProbabilities(PrtTout,cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam,bspline,ApproxHessian,false);
    }
  }


  double MI;
  computeProba(Nbpoint);
  computeMI(MI);
  computeHessien(HdesireInverse);
  //std::cout<<"HdesireInverse"<<std::endl<<HdesireInverse<<std::endl;

  /////////////////////////////////////////////////////////////////////////
  // DIRECT COMPO

  vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
  vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
  if(ApproxHessian!=HESSIAN_NONSECOND && ApproxHessian!=HESSIAN_0 && ApproxHessian!=HESSIAN_NEW && ApproxHessian!=HESSIAN_YOUCEF)
  {
    vpImageFilter::getGradX(dIx, d2Ix,fgdG,taillef);
    vpImageFilter::getGradY(dIx, d2Ixy,fgdG,taillef);
    vpImageFilter::getGradY(dIy, d2Iy,fgdG,taillef);
  }

  Nbpoint=0;
  erreur=0;
  zeroProbabilities();

  Warp->computeCoeff(p);
  for(unsigned int point=0;point<templateSize;point++)
  {
    i=ptTemplate[point].y;
    j=ptTemplate[point].x;
    X1[0]=j;X1[1]=i;

    Warp->computeDenom(X1,p);
    Warp->warpX(X1,X2,p);

    j2=X2[0];i2=X2[1];

    if((i2>=0)&&(j2>=0)&&(i2<I.getHeight())&&(j2<I.getWidth()))
    {
      Nbpoint++;
      Tij=ptTemplate[point].val;
//.........这里部分代码省略.........

void vpTemplateTrackerMIESM::trackNoPyr(const vpImage<unsigned char> &I)
{
  if(!CompoInitialised)
    std::cout<<"Compositionnal tracking no initialised\nUse initCompInverse(vpImage<unsigned char> &I) function"<<std::endl;
  dW=0;

  if(blur)
    vpImageFilter::filter(I, BI,fgG,taillef);
  vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
  vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
  /*	if(ApproxHessian!=HESSIAN_NONSECOND && ApproxHessian!=HESSIAN_0 && ApproxHessian!=HESSIAN_NEW && ApproxHessian!=HESSIAN_YOUCEF)
  {
    getGradX(dIx, d2Ix,fgdG,taillef);
    getGradY(dIx, d2Ixy,fgdG,taillef);
    getGradY(dIy, d2Iy,fgdG,taillef);
  }*/

  double erreur=0;
  int Nbpoint=0;
  int point;

  MI_preEstimation=-getCost(I,p);

  double lambda=lambdaDep;
  double MI=0;
  //double MIprec=-1000;

  double i2,j2;
  double Tij;
  double IW;
  //int cr,ct;
  //double er,et;

  vpColVector dpinv(nbParam);

  double dx,dy;
  double alpha=2.;

  int i,j;
  unsigned int iteration=0;
  do
  {
    Nbpoint=0;
    //MIprec=MI;
    MI=0;
    erreur=0;

    zeroProbabilities();

    /////////////////////////////////////////////////////////////////////////
    // Inverse
    Warp->computeCoeff(p);
    for(point=0;point<(int)templateSize;point++)
    {
      i=ptTemplate[point].y;
      j=ptTemplate[point].x;
      X1[0]=j;X1[1]=i;

      Warp->computeDenom(X1,p);
      Warp->warpX(X1,X2,p);

      j2=X2[0];i2=X2[1];

      if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
      {
        Nbpoint++;
        Tij=ptTemplate[point].val;
        if(!blur)
          IW=I.getValue(i2,j2);
        else
          IW=BI.getValue(i2,j2);

        //ct=ptTemplateSupp[point].ct;
        //et=ptTemplateSupp[point].et;
        //cr=(int)((IW*(Nc-1))/255.);
        //er=((double)IW*(Nc-1))/255.-cr;

        //                if(ApproxHessian==vpTemplateTrackerMI::HESSIAN_NONSECOND||hessianComputation==vpTemplateTrackerMI::USE_HESSIEN_DESIRE) // cas à tester AY
        //                    vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam, bspline);
        //                else
        //                    vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam, bspline);
      }
    }

    if(Nbpoint==0)
    {
      //std::cout<<"plus de point dans template suivi"<<std::endl;
      diverge=true;
      MI=0;
      throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
    }
    else
    {
      computeProba(Nbpoint);
      computeMI(MI);
      if(hessianComputation!= vpTemplateTrackerMI::USE_HESSIEN_DESIRE)
        computeHessien(HInverse);
      computeGradient();
      GInverse=G;

//.........这里部分代码省略.........

double vpTemplateTrackerZNCC::getCost(const vpImage<unsigned char> &I, const vpColVector &tp)
{
  double IW,Tij;
  int i,j;
  double i2,j2;
  int Nbpoint=0;

  Warp->computeCoeff(tp);

  double moyTij=0;
  double moyIW=0;
  for(unsigned int point=0;point<templateSize;point++)
  {
    i=ptTemplate[point].y;
    j=ptTemplate[point].x;
    X1[0]=j;X1[1]=i;
    Warp->computeDenom(X1,tp);
    Warp->warpX(X1,X2,tp);

    j2=X2[0];i2=X2[1];
    if((j2<I.getWidth()-1)&&(i2<I.getHeight()-1)&&(i2>0)&&(j2>0))
    {
      Tij=ptTemplate[point].val;
      if(!blur)
        IW=I.getValue(i2,j2);
      else
        IW=BI.getValue(i2,j2);
      //IW=getSubPixBspline4(I,i2,j2);
      moyTij+=Tij;
      moyIW+=IW;
      Nbpoint++;
    }
  }
  ratioPixelIn=(double)Nbpoint/(double)templateSize;
  if(! Nbpoint) {
    throw(vpException(vpException::divideByZeroError,
          "Cannot get cost: size = 0")) ;
  }

  moyTij=moyTij/Nbpoint;
  moyIW=moyIW/Nbpoint;

  double nom=0;//,denom=0;
  double var1=0,var2=0;
  for(unsigned int point=0;point<templateSize;point++)
  {
    i=ptTemplate[point].y;
    j=ptTemplate[point].x;
    X1[0]=j;X1[1]=i;
    Warp->computeDenom(X1,tp);
    Warp->warpX(X1,X2,tp);

    j2=X2[0];i2=X2[1];
    if((j2<I.getWidth()-1)&&(i2<I.getHeight()-1)&&(i2>0)&&(j2>0))
    {
      Tij=ptTemplate[point].val;
      if(!blur)
        IW=I.getValue(i2,j2);
      else
        IW=BI.getValue(i2,j2);
      //IW=getSubPixBspline4(I,i2,j2);
      nom+=(Tij-moyTij)*(IW-moyIW);
      //denom+=(Tij-moyTij)*(Tij-moyTij)*(IW-moyIW)*(IW-moyIW);
      var1+=(IW-moyIW)*(IW-moyIW);
      var2+=(Tij-moyTij)*(Tij-moyTij);

      Nbpoint++;
    }
  }
  // if(Nbpoint==0)return 10e10; // cannot occur
  //return -nom/sqrt(denom);
  return -nom/sqrt(var1*var2);
}

void vpTemplateTrackerMIForwardAdditional::initHessienDesired(const vpImage<unsigned char> &I)
{
  //std::cout<<"Initialise Hessian at Desired position..."<<std::endl;

  dW=0;

  int Nbpoint=0;

  if(blur)
    vpImageFilter::filter(I, BI,fgG,taillef);
  vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
  vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);

  double i2,j2;
  double Tij;
  double IW,dx,dy;
  int cr,ct;
  double er,et;

  int i,j;

  Nbpoint=0;

  zeroProbabilities();
  Warp->computeCoeff(p);
  for(unsigned int point=0;point<templateSize;point++)
  {
    i=ptTemplate[point].y;
    j=ptTemplate[point].x;
    X1[0]=j;X1[1]=i;
    X2[0]=j;X2[1]=i;

    Warp->computeDenom(X1,p);
    Warp->warpX(X1,X2,p);

    j2=X2[0];i2=X2[1];

    if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
    {
      Nbpoint++;
      Tij=ptTemplate[point].val;
      if(!blur)
        IW=I.getValue(i2,j2);
      else
        IW=BI.getValue(i2,j2);

      dx=1.*dIx.getValue(i2,j2)*(Nc-1)/255.;
      dy=1.*dIy.getValue(i2,j2)*(Nc-1)/255.;

      ct=(int)((IW*(Nc-1))/255.);
      cr=(int)((Tij*(Nc-1))/255.);
      et=(IW*(Nc-1))/255.-ct;
      er=((double)Tij*(Nc-1))/255.-cr;
      //std::cout<<"test"<<std::endl;
      Warp->dWarp(X1,X2,p,dW);

      double *tptemp=new double[nbParam];
      for(unsigned int it=0;it<nbParam;it++)
        tptemp[it] =dW[0][it]*dx+dW[1][it]*dy;

      if(ApproxHessian==HESSIAN_NONSECOND)
        vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
      else if(ApproxHessian==HESSIAN_0 || ApproxHessian==HESSIAN_NEW)
        vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);

      delete[] tptemp;
    }
  }

  if(Nbpoint>0)
  {
    double MI;
    computeProba(Nbpoint);
    computeMI(MI);
    computeHessien(Hdesire);


    //	double conditionnement=GetConditionnement(Hdesire);
    //	std::cout<<"conditionnement : "<<conditionnement<<std::endl;
    vpMatrix::computeHLM(Hdesire,lambda,HLMdesire);
    try
    {
      HLMdesireInverse=HLMdesire.inverseByLU();
    }
    catch(vpException &e)
    {
      //std::cerr<<"probleme inversion"<<std::endl;
      throw(e);
    }
    //std::cout<<"Hdesire = "<<Hdesire<<std::endl;
    //std::cout<<"\tEnd initialisation..."<<std::endl;
  }

}

void vpTemplateTrackerMIForwardAdditional::trackNoPyr(const vpImage<unsigned char> &I)
{
  dW=0;

  double erreur=0;
  int Nbpoint=0;
  if(blur)
    vpImageFilter::filter(I, BI,fgG,taillef);
  vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
  vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);

  double MI=0,MIprec=-1000;

  MI_preEstimation=-getCost(I,p);

  double i2,j2;
  double Tij;
  double IW,dx,dy;
  //unsigned
  int cr,ct;
  double er,et;
  double alpha=2.;

  int i,j;
  unsigned int iteration=0;

  initPosEvalRMS(p);
  do
  {
    if(iteration%5==0)
      initHessienDesired(I);
    Nbpoint=0;
    MIprec=MI;
    MI=0;
    erreur=0;

    zeroProbabilities();

    Warp->computeCoeff(p);
#ifdef VISP_HAVE_OPENMP
    int nthreads = omp_get_num_procs() ;
    //std::cout << "file: " __FILE__ << " line: " << __LINE__ << " function: " << __FUNCTION__ << " nthread: " << nthreads << std::endl;
    omp_set_num_threads(nthreads);
#pragma omp parallel for private(Tij,IW,i,j,i2,j2,cr,ct,er,et,dx,dy) default(shared)
#endif
    for(int point=0;point<(int)templateSize;point++)
    {
      i=ptTemplate[point].y;
      j=ptTemplate[point].x;
      X1[0]=j;X1[1]=i;

      Warp->computeDenom(X1,p);
      Warp->warpX(X1,X2,p);

      j2=X2[0];i2=X2[1];

      if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
      {
        Nbpoint++;
        Tij=ptTemplate[point].val;
        //Tij=Iterateurvecteur->val;
        if(!blur)
          IW=I.getValue(i2,j2);
        else
          IW=BI.getValue(i2,j2);

        dx=1.*dIx.getValue(i2,j2)*(Nc-1)/255.;
        dy=1.*dIy.getValue(i2,j2)*(Nc-1)/255.;

        ct=(int)((IW*(Nc-1))/255.);
        cr=(int)((Tij*(Nc-1))/255.);
        et=(IW*(Nc-1))/255.-ct;
        er=((double)Tij*(Nc-1))/255.-cr;

        //calcul de l'erreur
        erreur+=(Tij-IW)*(Tij-IW);

        //Calcul de l'histogramme joint par interpolation bilinÈaire (Bspline ordre 1)
        Warp->dWarp(X1,X2,p,dW);

        //double *tptemp=temp;
        double *tptemp=new double[nbParam];;
        for(unsigned int it=0;it<nbParam;it++)
          tptemp[it] =(dW[0][it]*dx+dW[1][it]*dy);
        //*tptemp++ =dW[0][it]*dIWx+dW[1][it]*dIWy;
        //std::cout<<cr<<"   "<<ct<<"  ; ";
        if(ApproxHessian==HESSIAN_NONSECOND||hessianComputation==vpTemplateTrackerMI::USE_HESSIEN_DESIRE)
          vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
        else if(ApproxHessian==HESSIAN_0 || ApproxHessian==HESSIAN_NEW)
          vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);

        delete[] tptemp;
      }
    }

    if(Nbpoint==0)
    {
      //std::cout<<"plus de point dans template suivi"<<std::endl;
      diverge=true;
      MI=0;
//.........这里部分代码省略.........