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

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);

  lambda=lambdaDep;

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

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

void vpTemplateTrackerMIInverseCompositional::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);

  int Nbpoint=0;

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

  vpColVector p_avant_estimation;p_avant_estimation=p;
  MI_preEstimation=-getCost(I,p);
  NMI_preEstimation=-getNormalizedCost(I,p);

  //    std::cout << "MI avant: " << MI_preEstimation << std::endl;
  //    std::cout << "NMI avant: " << NMI_preEstimation << std::endl;

  initPosEvalRMS(p);

  vpColVector dpinv(nbParam);
  double alpha=2.;

  unsigned int iteration=0;

  //unsigned int bspline_ = (unsigned int) bspline;
  //unsigned int totParam = (bspline_ * bspline_)*(1+nbParam+nbParam*nbParam);

  vpMatrix Hnorm(nbParam,nbParam);

  do
  {
    Nbpoint=0;
    MIprec=MI;
    MI=0;

    zeroProbabilities();

    Warp->computeCoeff(p);

    {
      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) )
            {
              vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(Prt, dPrt, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam, bspline);
            }
            else if (ptTemplateSelect[point] || !useTemplateSelect)
            {
              if(bspline==3){
                vpTemplateTrackerMIBSpline::PutTotPVBspline3(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam);
              }
              else{
                vpTemplateTrackerMIBSpline::PutTotPVBspline4(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam);
              }
            }
            else{
              vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(Prt, cr, er, ct, et, Ncb,nbParam, bspline);
            }
          }

        }
      }
    }

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

/*!
  Get the view of the virtual camera. Be carefull, the image I is modified. The projected image is not added as an overlay!
  
  To take into account the projection of several images, a matrix \f$ zBuffer \f$ is given as argument. This matrix contains the z coordinates of all the pixel of the image \f$ I \f$ in the camera frame. During the projection, the pixels are updated if there is no other plan between the camera and the projected image. The matrix \f$ zBuffer \f$ is updated in this case.
  
  \param I : The image used to store the result.
  \param cam : The parameters of the virtual camera.
  \param zBuffer : A matrix containing the z coordinates of the pixels of the image \f$ I \f$
*/
void
vpImageSimulator::getImage(vpImage<vpRGBa> &I, const vpCameraParameters &cam, 
			   vpMatrix &zBuffer)
{
  if (I.getWidth() != (unsigned int)zBuffer.getCols() || I.getHeight() != (unsigned int)zBuffer.getRows())
    throw (vpMatrixException(vpMatrixException::incorrectMatrixSizeError, " zBuffer must have the same size as the image I ! "));
  
  if (cleanPrevImage)
  {
    for (unsigned int i = 0; i < I.getHeight(); i++)
    {
      for (unsigned int j = 0; j < I.getWidth(); j++)
      {
        I[i][j] = bgColor;
      }
    }
  }
  if(visible)
  {
    if(!needClipping)
      getRoi(I.getWidth(),I.getHeight(),cam,pt,rect);
    else
      getRoi(I.getWidth(),I.getHeight(),cam,ptClipped,rect);
    
    double top = rect.getTop();
    double bottom = rect.getBottom();
    double left = rect.getLeft();
    double right= rect.getRight();
    
    vpRGBa *bitmap = I.bitmap;
    unsigned int width = I.getWidth();
    vpImagePoint ip;
    int nb_point_dessine = 0;

    for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++)
    {
      for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++)
      {
        double x=0,y=0;
        ip.set_ij(i,j);
        vpPixelMeterConversion::convertPoint(cam,ip, x,y);
        ip.set_ij(y,x);
        if (colorI == GRAY_SCALED)
        {
          unsigned char Ipixelplan;
          if(getPixel(ip,Ipixelplan))
          {
            if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0)
            {
              vpRGBa pixelcolor;
              pixelcolor.R = Ipixelplan;
              pixelcolor.G = Ipixelplan;
              pixelcolor.B = Ipixelplan;
              *(bitmap+i*width+j) = pixelcolor;
              nb_point_dessine++;
              zBuffer[i][j] = Xinter_optim[2];
            }
          }
        }
        else if (colorI == COLORED)
        {
          vpRGBa Ipixelplan;
          if(getPixel(ip,Ipixelplan))
          {
            if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0)
            {
              *(bitmap+i*width+j) = Ipixelplan;
              nb_point_dessine++;
              zBuffer[i][j] = Xinter_optim[2];
            }
          }
        }
      }
    }
  }
}

//.........这里部分代码省略.........
    X[3][2] = 1;

    vpImageSimulator sim2;
    sim2.init(Iimage, X);

    sim.setCameraPosition(vpHomogeneousMatrix(0,0,5,vpMath::rad(0),vpMath::rad(30),0));
    sim2.setCameraPosition(vpHomogeneousMatrix(0,0,5,vpMath::rad(0),vpMath::rad(-30),0));

    std::list<vpImageSimulator> listSim;
    listSim.addRight(sim);
    listSim.addRight(sim2);

    sim.setCameraPosition(vpHomogeneousMatrix(0,0,5,vpMath::rad(60),vpMath::rad(0),0));

    vpCameraParameters cam(868.0, 869.0, 320, 240);

    vpImageSimulator::getImage(Icamera,listSim,cam);

    return 0;
  }
  \endcode

  \param I : The image used to store the result
  \param list : List of vpImageSimulator to project
  \param cam : The parameters of the virtual camera
*/
void
vpImageSimulator::getImage(vpImage<vpRGBa> &I,
                           std::list<vpImageSimulator> &list,
                           const vpCameraParameters &cam)
{

  unsigned int width = I.getWidth();
  unsigned int height = I.getHeight();

  unsigned int nbsimList = (unsigned int)list.size();

  if (nbsimList < 1)
    return;

  vpImageSimulator** simList = new vpImageSimulator* [nbsimList];

  double topFinal = height+1;;
  double bottomFinal = -1;
  double leftFinal = width+1;
  double rightFinal = -1;

  unsigned int unvisible = 0;
  unsigned int indexSimu = 0;
  for(std::list<vpImageSimulator>::iterator it=list.begin(); it!=list.end(); ++it, ++indexSimu){
    vpImageSimulator* sim = &(*it);
    if (sim->visible)
      simList[indexSimu] = sim;
    else
      unvisible++;
  }

  nbsimList = nbsimList - unvisible;

   if (nbsimList < 1)
   {
     delete[] simList;
     return;
   }

  for (unsigned int i = 0; i < nbsimList; i++)

/*!
  Perform the tracking of a dot by connex components.

  \param mean_value : Threshold to use for the next call to track()
  and corresponding to the mean value of the dot intensity.

  \warning The content of the image is modified thanks to
  setGrayLevelOut() called before. This method choose a gray level
  (default is 0) used to modify the "in" dot level in "out" dot
  level. This gray level is here needed to stop the recursivity . The
  pixels of the dot are set to this new gray level "\out\".

  \return vpDot::out if an error occurs, vpDot::in otherwise.

  \sa setGrayLevelOut()
*/
int
vpDot::connexe(vpImage<unsigned char>& I, int u, int v,
	       unsigned int gray_level_min, unsigned int gray_level_max,
	       double &mean_value, double &u_cog, double &v_cog, double &n)
{

  int width = I.getWidth();
  int height= I.getHeight();

  // Test if we are in the image
  if ( (u < 0) || (v < 0) || (u >= width) || (v >= height) ) {
    return  vpDot::out ;
  }
  if (I[v][u] >= gray_level_min && I[v][u] <= gray_level_max)
  {
    vpImagePoint ip;
    ip.set_u(u);
    ip.set_v(v);

    if (graphics==true)
    {
      //      printf("u %d v %d\n", u, v);
      vpDisplay::displayPoint(I, ip, vpColor::green) ;
      //vpDisplay::flush(I);
    }

    ip_edges_list += ip;

    u_cog += u ;
    v_cog += v ;
    n+=1 ;

    if (n > nbMaxPoint) {
      vpERROR_TRACE("Too many point %lf (%lf%% of image size). "
		    "This threshold can be modified using the setMaxDotSize() "
		    "method.",
		    n, n / (I.getWidth() * I.getHeight()),
		    nbMaxPoint, maxDotSizePercentage) ;

      throw(vpTrackingException(vpTrackingException::featureLostError,
				"Dot to big")) ;
    }

    // Bounding box update
    if (u < this->u_min) this->u_min = u;
    if (u > this->u_max) this->u_max = u;
    if (v < this->v_min) this->v_min = v;
    if (v > this->v_max) this->v_max = v;
//     if (graphics==true)
//     {
//       vpRect r(this->u_min, this->v_min,
// 	       this->u_max - this->u_min + 1,
// 	       this->v_max - this->v_min + 1);
//       vpDisplay::displayRectangle(I, r, vpColor::white) ;
//     }
    // Mean value of the dot intensities
    mean_value = (mean_value *(n-1) + I[v][u]) / n;
    if (compute_moment==true)
    {
      m00++ ;
      m10 += u ;
      m01 += v ;
      m11 += (u*v) ;
      m20 += u*u ;
      m02 += v*v ;
    }
    I[v][u] = this->gray_level_out ;
  }
  else
  {
    return vpDot::out ;
  }
  if ( u-1 >= 0)
  {
    if (I[v][u-1] >= gray_level_min && I[v][u-1] <= gray_level_max)
      connexe(I,u-1,v, gray_level_min, gray_level_max, mean_value,
	      u_cog,v_cog, n) ;
  }

  if (u+1 < width)
  {
    if (I[v][u+1] >= gray_level_min && I[v][u+1] <= gray_level_max)
      connexe(I,u+1,v,gray_level_min, gray_level_max, mean_value,
	      u_cog, v_cog, n) ;
//.........这里部分代码省略.........

/*!

  Compute the center of gravity (COG) of the dot using connex
  components.  We assume the origin pixel (u, v) is in the dot. If
  not, the dot is seach arround this origin using a spiral search.

  \param I : Image to process.
  \param u : Starting pixel coordinate along the columns from where the
  dot is searched .

  \param v : Starting pixel coordinate along the rows from where the
  dot is searched .

  \warning The content of the image is modified.

  \exception vpTrackingException::featureLostError : If the tracking fails.

  \sa connexe()
*/
void
vpDot::COG(vpImage<unsigned char> &I, double& u, double& v)
{
  // Set the maximal number of points considering the maximal dot size
  // image percentage
  nbMaxPoint = (I.getWidth() * I.getHeight()) *  maxDotSizePercentage;

  // segmentation de l'image apres seuillage
  // (etiquetage des composante connexe)
  if (compute_moment)
    m00 = m11 = m02 = m20 = m10 = m01 = mu11 = mu20 = mu02 = 0;

  double u_cog = 0 ;
  double v_cog = 0 ;
  double npoint = 0 ;
  this->mean_gray_level = 0 ;

  ip_edges_list.kill() ;

  // Initialise the boundig box
  this->u_min = I.getWidth();
  this->u_max = 0;
  this->v_min = I.getHeight();
  this->v_max = 0;

#if 0
  // Original version
  if (  connexe(I, (unsigned int)u, (unsigned int)v,
		gray_level_min, gray_level_max,
		mean_gray_level, u_cog, v_cog, npoint) == vpDot::out)
  {
    bool sol = false ;
    unsigned int pas  ;
    for (pas = 2 ; pas <= 25 ; pas ++ )if (sol==false)
    {
      for (int k=-1 ; k <=1 ; k++) if (sol==false)
	for (int l=-1 ; l <=1 ; l++) if (sol==false)
	{
	  u_cog = 0 ;
	  v_cog = 0 ;
	  ip_edges_list.kill() ;
	 
	  this->mean_gray_level = 0 ;
	  if (connexe(I, (unsigned int)(u+k*pas),(unsigned int)(v+l*pas),
		      gray_level_min, gray_level_max,
		      mean_gray_level, u_cog, v_cog, npoint) != vpDot::out)
	  {
	    sol = true ; u += k*pas ; v += l*pas ;
	  }
	}
    }
    if (sol == false)
    {
      vpERROR_TRACE("Dot has been lost") ;
      throw(vpTrackingException(vpTrackingException::featureLostError,
				"Dot has been lost")) ;
    }
  }
#else
  // If the dot is not found, search around using a spiral
  if (  connexe(I,(unsigned int)u,(unsigned int)v,
		gray_level_min, gray_level_max,
		mean_gray_level, u_cog, v_cog, npoint) == vpDot::out)
  {

    bool sol = false ;

    unsigned int right = 1;
    unsigned int botom = 1;
    unsigned int left = 2;
    unsigned int up = 2;
    double u_ = u, v_ = v;
    unsigned int k;

    // Spiral search from the center to find the nearest dot
    while( (right < SPIRAL_SEARCH_SIZE) && (sol == false) ) {
      for (k=1; k <= right; k++) if(sol==false) {
	u_cog = 0 ;
	v_cog = 0 ;
	ip_edges_list.kill() ;
	
//.........这里部分代码省略.........

void MSRCR(vpImage<vpRGBa> &I, const int _scale, const int scaleDiv,
    const int level, const double dynamic, const int _kernelSize) {
  //Calculate the scales of filtering according to the number of filter and their distribution.
  std::vector<double> retinexScales = retinexScalesDistribution(scaleDiv, level, _scale);

  //Filtering according to the various scales.
  //Summarize the results of the various filters according to a specific weight(here equivalent for all).
  double weight = 1.0 / (double) scaleDiv;

  std::vector<vpImage<double> > doubleRGB(3);
  std::vector<vpImage<double> > doubleResRGB(3);
  unsigned int size = I.getSize();

  int kernelSize = _kernelSize;
  if(kernelSize == -1) {
    //Compute the kernel size from the input image size
    kernelSize = (int) (std::min(I.getWidth(), I.getHeight()) / 2.0);
    kernelSize = (kernelSize - kernelSize%2) + 1;
  }

  for(int channel = 0; channel < 3; channel++) {
    doubleRGB[(size_t) channel] = vpImage<double>(I.getHeight(), I.getWidth());
    doubleResRGB[(size_t) channel] = vpImage<double>(I.getHeight(), I.getWidth());

    for(unsigned int cpt = 0; cpt < size; cpt++) {
      //Shift the pixel values by 1 to avoid problem with log(0)
      switch(channel) {
      case 0:
        doubleRGB[(size_t) channel].bitmap[cpt] = I.bitmap[cpt].R + 1.0;
        break;

      case 1:
        doubleRGB[(size_t) channel].bitmap[cpt] = I.bitmap[cpt].G + 1.0;
        break;

      case 2:
        doubleRGB[(size_t) channel].bitmap[cpt] = I.bitmap[cpt].B + 1.0;
        break;

      default:
        break;
      }
    }

    for (int sc = 0; sc < scaleDiv; sc++) {
      vpImage<double> blurImage;
      double sigma = retinexScales[(size_t) sc];
      vpImageFilter::gaussianBlur(doubleRGB[(size_t) channel], blurImage, (unsigned int) kernelSize, sigma);

      for(unsigned int cpt = 0; cpt < size; cpt++) {
        //Summarize the filtered values.
        //In fact one calculates a ratio between the original values and the filtered values.
        doubleResRGB[(size_t) channel].bitmap[cpt] += weight * (std::log(doubleRGB[(size_t) channel].bitmap[cpt])
          - std::log(blurImage.bitmap[cpt]));
      }
    }
  }

  std::vector<double> dest(size*3);
  const double gain = 1.0, alpha = 128.0, offset = 0.0;

  for(unsigned int cpt = 0; cpt < size; cpt++) {
    double logl = std::log( (double) (I.bitmap[cpt].R + I.bitmap[cpt].G + I.bitmap[cpt].B + 3.0) );

    dest[cpt*3] = gain * (std::log(alpha * doubleRGB[0].bitmap[cpt]) - logl) * doubleResRGB[0].bitmap[cpt] + offset;
    dest[cpt*3 + 1] = gain * (std::log(alpha * doubleRGB[1].bitmap[cpt]) - logl) * doubleResRGB[1].bitmap[cpt] + offset;
    dest[cpt*3 + 2] = gain * (std::log(alpha * doubleRGB[2].bitmap[cpt]) - logl) * doubleResRGB[2].bitmap[cpt] + offset;
  }

  double sum = std::accumulate(dest.begin(), dest.end(), 0.0);
  double mean = sum / dest.size();

  std::vector<double> diff(dest.size());
  std::transform(dest.begin(), dest.end(), diff.begin(), std::bind2nd(std::minus<double>(), mean));
  double sq_sum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0);
  double stdev = std::sqrt(sq_sum / dest.size());

  double mini = mean - dynamic*stdev;
  double maxi = mean + dynamic*stdev;
  double range = maxi - mini;

  if(vpMath::nul(range)) {
    range = 1.0;
  }

  for(unsigned int cpt = 0; cpt < size; cpt++) {
    I.bitmap[cpt].R = vpMath::saturate<unsigned char>((255.0 * (dest[cpt*3 + 0] - mini) / range));
    I.bitmap[cpt].G = vpMath::saturate<unsigned char>((255.0 * (dest[cpt*3 + 1] - mini) / range));
    I.bitmap[cpt].B = vpMath::saturate<unsigned char>((255.0 * (dest[cpt*3 + 2] - mini) / range));
  }
}

/*!

\brief Constructor : initialize a display to visualize a RGBa image
(32 bits).

\param I : Image to be displayed (note that image has to be initialized).
\param winx, winy : The window is set at position x,y (column index, row index).
\param title : Window's title.
\param scaleType : If this parameter is set to:
  - vpDisplay::SCALE_AUTO, the display size is adapted to ensure the image
    is fully displayed in the screen;
  - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the same than the image size.
  - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines and the columns.
  - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines and the columns.
  - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines and the columns.
  - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines and the columns.

*/
vpDisplayD3D::vpDisplayD3D(vpImage<vpRGBa> &I, vpScaleType scaleType)
  : vpDisplayWin32(new vpD3DRenderer())
{
  setScale(scaleType, I.getWidth(), I.getHeight());
  init(I);
}

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;
//.........这里部分代码省略.........

/*!
  Check if the polygon is visible in the image and if the angle between the normal
  to the face and the line vector going from the optical center to the cog of the face is below
  the given threshold.
  To do that, the polygon is projected into the image thanks to the camera pose.

  \param cMo : The pose of the camera.
  \param alpha : Maximum angle to detect if the face is visible (in rad).
  \param modulo : Indicates if the test should also consider faces that are not oriented
  counter clockwise. If true, the orientation of the face is without importance.
  \param cam : Camera parameters (intrinsics parameters)
  \param I : Image used to consider level of detail.

  \return Return true if the polygon is visible.
*/
bool
vpMbtPolygon::isVisible(const vpHomogeneousMatrix &cMo, const double alpha, const bool &modulo,
      const vpCameraParameters &cam, const vpImage<unsigned char> &I)
{
  //   std::cout << "Computing angle from MBT Face (cMo, alpha)" << std::endl;

  changeFrame(cMo);

  if(nbpt <= 2) {
    //Level of detail (LOD)
    if(useLod) {
      vpCameraParameters c = cam;
      if(clippingFlag > 3) { // Contains at least one FOV constraint
        c.computeFov(I.getWidth(), I.getHeight());
      }
      computePolygonClipped(c);
      std::vector<vpImagePoint> roiImagePoints;
      getRoiClipped(c, roiImagePoints);

      if (roiImagePoints.size() == 2) {
        double x1 = roiImagePoints[0].get_u();
        double y1 = roiImagePoints[0].get_v();
        double x2 = roiImagePoints[1].get_u();
        double y2 = roiImagePoints[1].get_v();
        double length = std::sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
//          std::cout << "Index=" << index << " ; Line length=" << length << " ; clippingFlag=" << clippingFlag << std::endl;
//        vpTRACE("index=%d lenght=%f minLineLengthThresh=%f", index, length, minLineLengthThresh);

        if (length < minLineLengthThresh) {
          isvisible = false;
          isappearing = false;
          return false;
        }
      }
    }

    /* a line is always visible when LOD is not used */
    isvisible = true;
    isappearing = false;
    return  true ;
  }

  // If the polygon has no orientation, the angle check visibility is always valid.
  // Feature mainly used for cylinders.
  if(!hasOrientation)
  {
    isvisible = true;
    isappearing = false;
    return true;
  }

  //Check visibility from normal
  //Newell's Method for calculating the normal of an arbitrary 3D polygon
  //https://www.opengl.org/wiki/Calculating_a_Surface_Normal
  vpColVector faceNormal(3);
  vpColVector currentVertex, nextVertex;
  for(unsigned int  i = 0; i<nbpt; i++) {
    currentVertex = p[i].cP;
    nextVertex = p[(i+1) % nbpt].cP;

    faceNormal[0] += (currentVertex[1] - nextVertex[1]) * (currentVertex[2] + nextVertex[2]);
    faceNormal[1] += (currentVertex[2] - nextVertex[2]) * (currentVertex[0] + nextVertex[0]);
    faceNormal[2] += (currentVertex[0] - nextVertex[0]) * (currentVertex[1] + nextVertex[1]);
  }
  faceNormal.normalize();

  vpColVector e4(3) ;
  vpPoint pt;
  for (unsigned int i = 0; i < nbpt; i += 1){
    pt.set_X(pt.get_X() + p[i].get_X());
    pt.set_Y(pt.get_Y() + p[i].get_Y());
    pt.set_Z(pt.get_Z() + p[i].get_Z());
  }
  e4[0] = -pt.get_X() / (double)nbpt;
  e4[1] = -pt.get_Y() / (double)nbpt;
  e4[2] = -pt.get_Z() / (double)nbpt;
  e4.normalize();

  double angle = acos(vpColVector::dotProd(e4, faceNormal));

//  vpCTRACE << angle << "/" << vpMath::deg(angle) << "/" << vpMath::deg(alpha) << std::endl;

  if( angle < alpha || (modulo && (M_PI - angle) < alpha)) {
    isvisible = true;
    isappearing = false;
//.........这里部分代码省略.........

bool vpBlobsTargetTracker::track(const cv::Mat &cvI, const vpImage<unsigned char> &I )
{

  if (m_state == detection || m_force_detection) {
    //std::cout << "STATE: DETECTION "<< std::endl;

    bool obj_found = false;
    if (!m_manual_blob_init && !m_full_manual)
      obj_found = m_colBlob.detect(cvI);
    // Delete previuos list of blobs
    m_blob_list.clear();
    m_blob_list.resize(4);
    m_initPose = true;
    m_target_found = false;

    if (obj_found || m_manual_blob_init || m_full_manual) {

      try{


        if (m_full_manual)
        {
          std::cout << "Full manual" << std::endl;
          vpDisplay::displayText(I, vpImagePoint(I.getHeight() - 10, 10), "Click on the 4 blobs", vpColor::red);

          vpDisplay::flush(I);
          for(std::list<vpDot2>::iterator it=m_blob_list.begin(); it != m_blob_list.end(); ++it)
          {
            (*it).setGraphics(true);
            (*it).setGraphicsThickness(1);
            (*it).initTracking(I);
            (*it).track(I);
            vpDisplay::flush(I);

          }
          m_state = tracking;
          m_force_detection = false;
        }


        else {

          // std::cout << "TARGET FOUND" << std::endl;

          vpDot2 blob;
          blob.setGraphics(true);
          blob.setGraphicsThickness(1);
          blob.setEllipsoidShapePrecision(0.9);
          if (m_manual_blob_init)
          {
            vpDisplay::displayText(I, vpImagePoint(I.getHeight() - 10, 10), "Click on the colored blob", vpColor::red);
            vpDisplay::flush(I);
            blob.initTracking(I);
            m_manual_blob_init = false;
          }
          else
          {
            vpImagePoint cog = m_colBlob.getCog(0);
            vpDisplay::displayCross(I,cog,10, vpColor::red,2 );
            blob.initTracking(I,cog);
          }
          blob.track(I);

//          printf("Dot characteristics: \n");
//          printf("  width : %lf\n", blob.getWidth());
//          printf("  height: %lf\n", blob.getHeight());
//          printf("  area: %lf\n", blob.getArea());
//          printf("  gray level min: %d\n", blob.getGrayLevelMin());
//          printf("  gray level max: %d\n", blob.getGrayLevelMax());
//          printf("  grayLevelPrecision: %lf\n", blob.getGrayLevelPrecision());
//          printf("  sizePrecision: %lf\n", blob.getSizePrecision());
//          printf("  ellipsoidShapePrecision: %lf\n", blob.getEllipsoidShapePrecision());

          vpDot2 black_blob = blob;
          black_blob.setGrayLevelMax(m_grayLevelMaxBlob);
          black_blob.setGrayLevelMin(m_grayLevelMinBlob);

          int i,j,aj,ai;

          if(m_left_hand_target)
          {
            i = blob.getCog().get_i()-blob.getHeight()*2.3;
            j = blob.getCog().get_j()-blob.getWidth()*3.3;
            ai = blob.getHeight()*5;
            aj = blob.getWidth()*5;
          }
          else
          {
            i = blob.getCog().get_i()-blob.getHeight();
            j = blob.getCog().get_j()-blob.getWidth()*2.0;
            ai = blob.getHeight()*4.5;
            aj = blob.getWidth()*4;

          }

          //search similar blobs in the image and store them in blob_list
          //black_blob.searchDotsInArea(I, 0, 0, I.getWidth(), I.getHeight(), m_blob_list);
          //vpDisplay::displayRectangle(I, i, j, ai, aj, vpColor::red, false, 1);
          black_blob.searchDotsInArea(I, j, i, ai, aj, m_blob_list);

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

/*!

  Computes the SURF points in the current image I and try to matched
  them with the points in the reference list. Only the matched points
  are stored.

  \param I : The gray scaled image where the points are computed.

  \return the number of point which have been matched.
*/
unsigned int vpKeyPointSurf::matchPoint(const vpImage<unsigned char> &I)
{
  IplImage* currentImage = NULL;

  if((I.getWidth() % 8) == 0){
    int height = (int)I.getHeight();
    int width  = (int)I.getWidth();
    CvSize size = cvSize(width, height);
    currentImage = cvCreateImageHeader(size, IPL_DEPTH_8U, 1);
    currentImage->imageData = (char*)I.bitmap;
  }else{
    vpImageConvert::convert(I,currentImage);
  }
  
  /* we release the memory storage for the current points (it has to be kept 
      allocated for the get descriptor points, ...) */
  if(storage_cur != NULL){
    cvReleaseMemStorage(&storage_cur);
    storage_cur = NULL;
  }
  storage_cur = cvCreateMemStorage(0);

  cvExtractSURF( currentImage, 0, &image_keypoints, &image_descriptors,
     storage_cur, params );

  CvSeqReader reader, kreader;
  cvStartReadSeq( ref_keypoints, &kreader );
  cvStartReadSeq( ref_descriptors, &reader );


  std::list<int> indexImagePair;
  std::list<int> indexReferencePair;


  unsigned int nbrPair = 0;

  for(int i = 0; i < ref_descriptors->total; i++ )
  {
    const CvSURFPoint* kp = (const CvSURFPoint*)kreader.ptr;
    const float* descriptor = (const float*)reader.ptr;
    CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
    CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
    int nearest_neighbor = naiveNearestNeighbor( descriptor,
						 kp->laplacian,
						 image_keypoints,
						 image_descriptors );
    if( nearest_neighbor >= 0 )
    {
      indexReferencePair.push_back(i);
      indexImagePair.push_back(nearest_neighbor);
      nbrPair++;
    }
  }

  std::list<int>::const_iterator indexImagePairIter = indexImagePair.begin();
  std::list<int>::const_iterator indexReferencePairIter = indexReferencePair.begin();

  matchedReferencePoints.resize(0);

  if (nbrPair == 0)
    return (0);

  currentImagePointsList.resize(nbrPair);
  matchedReferencePoints.resize(nbrPair);

  for (unsigned int i = 0; i < nbrPair; i++)
  {
      int index = *indexImagePairIter;

      CvSURFPoint* r1 = (CvSURFPoint*)cvGetSeqElem(image_keypoints, index);

      currentImagePointsList[i].set_i(r1->pt.y);
      currentImagePointsList[i].set_j(r1->pt.x);

      matchedReferencePoints[i] = (unsigned int)*indexReferencePairIter;


      ++indexImagePairIter;
      ++indexReferencePairIter;
  }

  if((I.getWidth() % 8) == 0){
    currentImage->imageData = NULL;
    cvReleaseImageHeader(&currentImage);
  }else{
    cvReleaseImage(&currentImage);
  }

  return nbrPair;
}