Esempio n. 1
0
/*!
  Test the quality of the tracking.
  The tracking is supposed to fail if less than 10 points are tracked.

  \todo Find a efficient way to test the quality.

  \throw vpTrackingException::fatalError  if the test fails.
*/
void
vpMbKltTracker::testTracking()
{
  unsigned int nbTotalPoints = 0;
  vpMbtDistanceKltPoints *kltpoly;
//  for (unsigned int i = 0; i < faces.size(); i += 1){
  for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
    kltpoly = *it;
    if(kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2 && kltpoly->hasEnoughPoints()){
      nbTotalPoints += kltpoly->getCurrentNumberPoints();
    }
  }

  vpMbtDistanceKltCylinder *kltPolyCylinder;
  for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
    kltPolyCylinder = *it;
    if(kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
      nbTotalPoints += kltPolyCylinder->getCurrentNumberPoints();
  }

  if(nbTotalPoints < 10){
    std::cerr << "test tracking failed (too few points to realize a good tracking)." << std::endl;
    throw vpTrackingException(vpTrackingException::fatalError,
          "test tracking failed (too few points to realize a good tracking).");
  }
}
void vpTemplateTrackerWarpHomography::warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM)
{
  //if((ParamM[2]*vX[0]+ParamM[5]*vX[1]+1)>0)//si dans le plan image reel
  if((denom)>0)// FS optimisation
  {
    vXres[0]=((1+ParamM[0])*vX[0]+ParamM[3]*vX[1]+ParamM[6])*denom;
    vXres[1]=(ParamM[1]*vX[0]+(1+ParamM[4])*vX[1]+ParamM[7])*denom;
  }
  else
    throw(vpTrackingException(vpTrackingException::fatalError,"Division by zero in vpTemplateTrackerWarpHomography::warpX()"));
}
void vpTemplateTrackerWarpHomography::warpXInv(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM)
{

  if((ParamM[2]*vX[0]+ParamM[5]*vX[1]+1)<0)//si dans le plan image reel
  {
    vXres[0]=((1+ParamM[0])*vX[0]+ParamM[3]*vX[1]+ParamM[6])/(ParamM[2]*vX[0]+ParamM[5]*vX[1]+1);
    vXres[1]=(ParamM[1]*vX[0]+(1+ParamM[4])*vX[1]+ParamM[7])/(ParamM[2]*vX[0]+ParamM[5]*vX[1]+1);
  }
  else
    throw(vpTrackingException(vpTrackingException::fatalError,"Division by zero in vpTemplateTrackerWarpHomography::warpXSpecialInv()")) ;
} 
Esempio n. 4
0
void
vpMbKltTracker::computeVVSInteractionMatrixAndResidu(unsigned int shift, vpColVector &R, vpMatrix &L, vpHomography &H,
    std::list<vpMbtDistanceKltPoints*> &kltPolygons_, std::list<vpMbtDistanceKltCylinder*> &kltCylinders_,
    const vpHomogeneousMatrix &ctTc0_) {
  vpMbtDistanceKltPoints *kltpoly;
//  for (unsigned int i = 0; i < faces.size(); i += 1){
  for(std::list<vpMbtDistanceKltPoints*>::const_iterator it = kltPolygons_.begin(); it != kltPolygons_.end(); ++it){
    kltpoly = *it;
    if(kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2 &&
       kltpoly->hasEnoughPoints()){
      vpSubColVector subR(R, shift, 2*kltpoly->getCurrentNumberPoints());
      vpSubMatrix subL(L, shift, 0, 2*kltpoly->getCurrentNumberPoints(), 6);
      try{
        kltpoly->computeHomography(ctTc0_, H);
        kltpoly->computeInteractionMatrixAndResidu(subR, subL);
      }catch(...){
        throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
      }

      shift += 2*kltpoly->getCurrentNumberPoints();
    }
  }

  vpMbtDistanceKltCylinder *kltPolyCylinder;
  for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it = kltCylinders_.begin(); it != kltCylinders_.end(); ++it){
    kltPolyCylinder = *it;

    if(kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
    {
      vpSubColVector subR(R, shift, 2*kltPolyCylinder->getCurrentNumberPoints());
      vpSubMatrix subL(L, shift, 0, 2*kltPolyCylinder->getCurrentNumberPoints(), 6);
      try{
        kltPolyCylinder->computeInteractionMatrixAndResidu(ctTc0_,subR, subL);
      }catch(...){
        throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
      }

      shift += 2*kltPolyCylinder->getCurrentNumberPoints();
    }
  }
}
Esempio n. 5
0
/*!

  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\" when connexe() is called.

  \exception vpTrackingException::initializationError : No valid gray
  level "out" can be found. This means that the min gray level is set to 0
  and the max gray level to 255. This should not occur
*/
void 
vpDot::setGrayLevelOut()
{
  if (gray_level_min == 0) {
    if (gray_level_max == 255) {
      // gray_level_min = 0 and gray_level_max = 255: this should not occur
      vpERROR_TRACE("Unable to choose a good \"out\" level") ;
      throw(vpTrackingException(vpTrackingException::initializationError,
				"Unable to choose a good \"out\" level")) ;
    }
    gray_level_out = static_cast<unsigned char>(gray_level_max + 1u);
  }
}
Esempio n. 6
0
/*!
	
  Resample the ellipse if the number of sample is less than 90% of the
  expected value.
	
  \note The expected value is computed thanks to the difference between the smallest and the biggest \f$ \alpha \f$ angles
  and the parameter which indicates the number of degrees between
  two points (vpMe::sample_step).

  \param I : Image in which the ellipse appears.

  \exception vpTrackingException::initializationError : Moving edges not initialized.

*/
void
vpMeEllipse::reSample(const vpImage<unsigned char>  &I)
{
  if (!me) {
    vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
    throw(vpTrackingException(vpTrackingException::initializationError,
      "Moving edges not initialized")) ;
  }

  unsigned int n = numberOfSignal() ;
  expecteddensity = (alpha2-alpha1) / vpMath::rad((double)me->getSampleStep());
  if ((double)n<0.9*expecteddensity){
    sample(I) ;
  }
}
Esempio n. 7
0
void
vpMbKltTracker::computeVVSCheckLevenbergMarquardtKlt(const unsigned int iter, const unsigned int nbInfos,
    const vpHomogeneousMatrix &cMoPrev, const vpColVector &error_prev, const vpHomogeneousMatrix &ctTc0_Prev,
    double &mu, bool &reStartFromLastIncrement) {
  if(iter != 0 && m_optimizationMethod == vpMbTracker::LEVENBERG_MARQUARDT_OPT){
    if(m_error.sumSquare()/(double)(2*nbInfos) > error_prev.sumSquare()/(double)(2*nbInfos)){
      mu *= 10.0;

      if(mu > 1.0)
        throw vpTrackingException(vpTrackingException::fatalError, "Optimization diverged");

      cMo = cMoPrev;
      m_error = error_prev;
      ctTc0 = ctTc0_Prev;
      reStartFromLastIncrement = true;
    }
  }
}
Esempio n. 8
0
/*!
   Track KLT keypoints using the iterative Lucas-Kanade method with pyramids.

   \param I : Input image.
 */
void vpKltOpencv::track(const cv::Mat &I)
{
  if(m_points[1].size() == 0)
    throw vpTrackingException(vpTrackingException::fatalError, "Not enough key points to track.");

  std::vector<float> err;
  int flags = 0;

  cv::swap(m_prevGray, m_gray);

  if (m_initial_guess) {
    flags |= cv::OPTFLOW_USE_INITIAL_FLOW;
    m_initial_guess = false;
  }
  else {
    std::swap(m_points[1], m_points[0]);
  }

  //cvtColor(I, m_gray, cv::COLOR_BGR2GRAY);
  I.copyTo(m_gray);

  if(m_prevGray.empty()){
    m_gray.copyTo(m_prevGray);
  }

  std::vector<uchar> status;

  cv::calcOpticalFlowPyrLK(m_prevGray, m_gray, m_points[0], m_points[1], status, err, cv::Size(m_winSize, m_winSize),
      m_pyrMaxLevel, m_termcrit, flags, m_minEigThreshold);

  // Remove points that are lost
  for (int i=(int)status.size()-1; i>=0; i--) {
    if (status[(size_t)i] == 0) { // point is lost
      m_points[0].erase(m_points[0].begin()+i);
      m_points[1].erase(m_points[1].begin()+i);
      m_points_id.erase(m_points_id.begin()+i);
    }
  }
}
Esempio n. 9
0
/*!
	
  Resample the line if the number of sample is less than 80% of the
  expected value.
	
  \note The expected value is computed thanks to the length of the
  line and the parameter which indicates the number of pixel between
  two points (vpMe::sample_step).

  \param I : Image in which the line appears.
*/
void
vpMeLine::reSample(const vpImage<unsigned char> &I)
{
  double i1,j1,i2,j2 ;

  if (!me) {
    vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
    throw(vpTrackingException(vpTrackingException::initializationError,
      "Moving edges not initialized")) ;
  }

  project(a,b,c,PExt[0].ifloat,PExt[0].jfloat,i1,j1) ;
  project(a,b,c,PExt[1].ifloat,PExt[1].jfloat,i2,j2) ;

  // Points extremites
  PExt[0].ifloat = i1 ;
  PExt[0].jfloat = j1 ;
  PExt[1].ifloat = i2 ;
  PExt[1].jfloat = j2 ;

  double d = sqrt(vpMath::sqr(i1-i2)+vpMath::sqr(j1-j2)) ;

  unsigned int n = numberOfSignal() ;
  double expecteddensity = d / (double)me->getSampleStep();

  if ((double)n<0.9*expecteddensity)
  {
    double delta_new = delta;
    delta = delta_1;
    sample(I) ;
    delta = delta_new;
    //  2. On appelle ce qui n'est pas specifique
    {
      vpMeTracker::initTracking(I) ;
    }
  }
}
Esempio n. 10
0
/*!
  Realize the tracking of the object in the image

  \throw vpException : if the tracking is supposed to have failed

  \param I : the input image
*/
void
vpMbKltTracker::track(const vpImage<unsigned char>& I)
{   
  unsigned int nbInfos = 0;
  unsigned int nbFaceUsed = 0;

  try{
    preTracking(I, nbInfos, nbFaceUsed);
  }
  catch(vpException &e){
    throw e;
  }
  
  if(nbInfos < 4 || nbFaceUsed == 0){
    vpERROR_TRACE("\n\t\t Error-> not enough data") ;
    throw vpTrackingException(vpTrackingException::notEnoughPointError, "\n\t\t Error-> not enough data");
  }

  //vpColVector w;
  computeVVS(nbInfos, m_w);

  if(postTracking(I, m_w))
    reinit(I);
}
Esempio n. 11
0
/*!
  Construct a list of vpMeSite moving edges at a particular sampling
  step between the two extremities. The two extremities are defined by
  the points with the smallest and the biggest \f$ alpha \f$ angle.

  \param I : Image in which the ellipse appears.

  \exception vpTrackingException::initializationError : Moving edges not initialized.

*/
void
vpMeEllipse::sample(const vpImage<unsigned char> & I)
{
  vpCDEBUG(1) <<"begin vpMeEllipse::sample() : "<<std::endl ;

  if (!me) {
    vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
    throw(vpTrackingException(vpTrackingException::initializationError,
      "Moving edges not initialized")) ;
  }

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

  double n_sample;

  //if (me->getSampleStep()==0)
  if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon())
  {
    std::cout << "In vpMeEllipse::sample: " ;
    std::cout << "function called with sample step = 0" ;
    //return fatalError ;
  }

  double j, i;//, j11, i11;
  vpImagePoint iP11;
  j = i = 0.0 ;

  double incr = vpMath::rad(me->getSampleStep()) ; // angle increment en degree
  vpColor col = vpColor::red ;
  getParameters() ;

  // Delete old list
  list.clear();

  angle.clear();

  // sample positions
  double k = alpha1 ;
  while (k<alpha2)
  {
//     j = a *cos(k) ; // equation of an ellipse
//     i = b *sin(k) ; // equation of an ellipse

    j = a *sin(k) ; // equation of an ellipse
    i = b *cos(k) ; // equation of an ellipse

    // (i,j) are the coordinates on the origin centered ellipse ;
    // a rotation by "e" and a translation by (xci,jc) are done
    // to get the coordinates of the point on the shifted ellipse
//     iP11.set_j( iPc.get_j() + ce *j - se *i );
//     iP11.set_i( iPc.get_i() -( se *j + ce *i) );

    iP11.set_j( iPc.get_j() + ce *j + se *i );
    iP11.set_i( iPc.get_i() - se *j + ce *i );

    vpDisplay::displayCross(I, iP11,  5, col) ;

    double theta ;
    computeTheta(theta, K, iP11)  ;

    // If point is in the image, add to the sample list
    if(!outOfImage(vpMath::round(iP11.get_i()), vpMath::round(iP11.get_j()), 0, height, width))
    {
      vpMeSite pix ;
      pix.init((int)iP11.get_i(), (int)iP11.get_j(), theta) ;
      pix.setDisplay(selectDisplay) ;
      pix.setState(vpMeSite::NO_SUPPRESSION);

      if(vpDEBUG_ENABLE(3))
      {
        vpDisplay::displayCross(I,iP11, 5, vpColor::blue);
      }
      list.push_back(pix);
      angle.push_back(k);
    }
    k += incr ;

  }
  vpMeTracker::initTracking(I) ;

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

  vpCDEBUG(1) << "end vpMeEllipse::sample() : " ;
  vpCDEBUG(1) << n_sample << " point inserted in the list " << std::endl  ;
}
Esempio n. 12
0
/*!
	
  Least squares method used to make the tracking more robust. It
  ensures that the points taken into account to compute the right
  equation belong to the line.
*/
void
vpMeLine::leastSquare()
{
  vpMatrix A(numberOfSignal(),2) ;
  vpColVector x(2), x_1(2) ;
  x_1 = 0;

  unsigned int i ;

  vpRobust r(numberOfSignal()) ;
  r.setThreshold(2);
  r.setIteration(0) ;
  vpMatrix D(numberOfSignal(),numberOfSignal()) ;
  D.eye() ;
  vpMatrix DA, DAmemory ;
  vpColVector DAx ;
  vpColVector w(numberOfSignal()) ;
  vpColVector B(numberOfSignal()) ;
  w =1 ;
  vpMeSite p_me ;
  unsigned int iter =0 ;
  unsigned int nos_1 = 0 ;
  double distance = 100;

  if (list.size() <= 2 || numberOfSignal() <= 2)
  {
    //vpERROR_TRACE("Not enough point") ;
    vpCDEBUG(1) << "Not enough point";
    throw(vpTrackingException(vpTrackingException::notEnoughPointError,
                              "not enough point")) ;
  }

  if ((fabs(b) >=0.9)) // Construction du systeme Ax=B
    // a i + j + c = 0
    // A = (i 1)   B = (-j)
  {
    nos_1 = numberOfSignal() ;
    unsigned int k =0 ;
    for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
      p_me = *it;
      if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
      {
        A[k][0] = p_me.ifloat ;
        A[k][1] = 1 ;
        B[k] = -p_me.jfloat ;
        k++ ;
      }
    }

    while (iter < 4 && distance > 0.05)
    {
      DA = D*A ;
      x = DA.pseudoInverse(1e-26) *D*B ;

      vpColVector residu(nos_1);
      residu = B - A*x;
      r.setIteration(iter) ;
      r.MEstimator(vpRobust::TUKEY,residu,w) ;

      k = 0;
      for (i=0 ; i < nos_1 ; i++)
      {
        D[k][k] =w[k]  ;
        k++;
      }
      iter++ ;
      distance = fabs(x[0]-x_1[0])+fabs(x[1]-x_1[1]);
      x_1 = x;
    }

    k =0 ;
    for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
      p_me = *it;
      if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
      {
        if (w[k] < 0.2)
        {
          p_me.setState(vpMeSite::M_ESTIMATOR);
          
          *it = p_me;
        }
        k++ ;
      }
    }

    // mise a jour de l'equation de la droite
    a = x[0] ;
    b = 1 ;
    c = x[1] ;

    double s =sqrt( vpMath::sqr(a)+vpMath::sqr(b)) ;
    a /= s ;
    b /= s ;
    c /= s ;
  }


  else		// Construction du systeme Ax=B
    // i + bj + c = 0
    // A = (j 1)   B = (-i)
  {
    nos_1 = numberOfSignal() ;
    unsigned int k =0 ;
    for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
      p_me = *it;
      if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
      {
        A[k][0] = p_me.jfloat ;
        A[k][1] = 1 ;
        B[k] = -p_me.ifloat ;
        k++ ;
      }
    }

    while (iter < 4 && distance > 0.05)
    {
      DA = D*A ;
      x = DA.pseudoInverse(1e-26) *D*B ;

      vpColVector residu(nos_1);
      residu = B - A*x;
      r.setIteration(iter) ;
      r.MEstimator(vpRobust::TUKEY,residu,w) ;

      k = 0;
      for (i=0 ; i < nos_1 ; i++)
      {
        D[k][k] =w[k]  ;
        k++;
      }
      iter++ ;
      distance = fabs(x[0]-x_1[0])+fabs(x[1]-x_1[1]);
      x_1 = x;
    }

    k =0 ;
    for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
      p_me = *it;
      if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
      {
        if (w[k] < 0.2)
        {
          p_me.setState(vpMeSite::M_ESTIMATOR);
          
          *it = p_me;
        }
        k++ ;
      }
    }
    a = 1 ;
    b = x[0] ;
    c = x[1] ;

    double s = sqrt(vpMath::sqr(a)+vpMath::sqr(b)) ;
    a /= s ;
    b /= s ;
    c /= s ;
  }

  // mise a jour du delta
  delta = atan2(a,b) ;

  normalizeAngle(delta) ;
}
Esempio n. 13
0
/*!  

  Seek along the line defined by its equation, the two extremities of
  the line. This function is useful in case of translation of the
  line.

  \param I : Image in which the line appears.

  \exception vpTrackingException::initializationError : Moving edges not initialized.
*/
void
vpMeLine::seekExtremities(const vpImage<unsigned char> &I)
{
  vpCDEBUG(1) <<"begin vpMeLine::sample() : "<<std::endl ;

  if (!me) {
    vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
    throw(vpTrackingException(vpTrackingException::initializationError,
      "Moving edges not initialized")) ;
  }

  int rows = (int)I.getHeight() ;
  int cols = (int)I.getWidth() ;
  double n_sample;

  //if (me->getSampleStep()==0)
  if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon())
  {

    vpERROR_TRACE("function called with sample step = 0") ;
    throw(vpTrackingException(vpTrackingException::fatalError,
                              "sample step = 0")) ;
  }

  // i, j portions of the line_p
  double diffsi = PExt[0].ifloat-PExt[1].ifloat;
  double diffsj = PExt[0].jfloat-PExt[1].jfloat;

  double s = vpMath::sqr(diffsi)+vpMath::sqr(diffsj) ;

  double di = diffsi/sqrt(s) ; // pas de risque de /0 car d(P1,P2) >0
  double dj = diffsj/sqrt(s) ;

  double length_p = sqrt((vpMath::sqr(diffsi)+vpMath::sqr(diffsj)));

  // number of samples along line_p
  n_sample = length_p/(double)me->getSampleStep();
  double sample_step = (double)me->getSampleStep();

  vpMeSite P ;
  P.init((int) PExt[0].ifloat, (int)PExt[0].jfloat, delta_1, 0, sign) ;
  P.setDisplay(selectDisplay) ;

  unsigned int  memory_range = me->getRange() ;
  me->setRange(1);

  vpImagePoint ip;

  for (int i=0 ; i < 3 ; i++)
  {
    P.ifloat = P.ifloat + di*sample_step ; P.i = (int)P.ifloat ;
    P.jfloat = P.jfloat + dj*sample_step ; P.j = (int)P.jfloat ;

    if(!outOfImage(P.i, P.j, 5, rows, cols))
    {
      P.track(I,me,false) ;

      if (P.getState() == vpMeSite::NO_SUPPRESSION)
      {
        list.push_back(P);
        if (vpDEBUG_ENABLE(3)) {
          ip.set_i( P.i );
          ip.set_j( P.j );

          vpDisplay::displayCross(I, ip, 5, vpColor::green) ;
        }
      }
      else {
        if (vpDEBUG_ENABLE(3)) {
          ip.set_i( P.i );
          ip.set_j( P.j );
          vpDisplay::displayCross(I, ip, 10, vpColor::blue) ;
        }
      }
    }
  }

  P.init((int) PExt[1].ifloat, (int)PExt[1].jfloat, delta_1, 0, sign) ;
  P.setDisplay(selectDisplay) ;
  for (int i=0 ; i < 3 ; i++)
  {
    P.ifloat = P.ifloat - di*sample_step ; P.i = (int)P.ifloat ;
    P.jfloat = P.jfloat - dj*sample_step ; P.j = (int)P.jfloat ;

    if(!outOfImage(P.i, P.j, 5, rows, cols))
    {
      P.track(I,me,false) ;

      if (P.getState() == vpMeSite::NO_SUPPRESSION)
      {
        list.push_back(P);
        if (vpDEBUG_ENABLE(3)) {
          ip.set_i( P.i );
          ip.set_j( P.j );
          vpDisplay::displayCross(I, ip, 5, vpColor::green) ;
        }
      }
      else {
        if (vpDEBUG_ENABLE(3)) {
          ip.set_i( P.i );
          ip.set_j( P.j );
          vpDisplay::displayCross(I, ip, 10, vpColor::blue) ;
        }
      }
    }
  }

  me->setRange(memory_range);

  vpCDEBUG(1) <<"end vpMeLine::sample() : " ;
  vpCDEBUG(1) << n_sample << " point inserted in the list " << std::endl  ;
}
Esempio n. 14
0
/*!
  Least squares method used to make the tracking more robust. It
  ensures that the points taken into account to compute the right
  equation belong to the ellipse.
*/
void
vpMeEllipse::leastSquare()
{
  // Construction du systeme Ax=b
  //! i^2 + K0 j^2 + 2 K1 i j + 2 K2 i + 2 K3 j + K4
  // A = (j^2 2ij 2i 2j 1)   x = (K0 K1 K2 K3 K4)^T  b = (-i^2 )
  unsigned int i ;

  vpMeSite p_me ;

  unsigned int iter =0 ;
  vpColVector b_(numberOfSignal()) ;
  vpRobust r(numberOfSignal()) ;
  r.setThreshold(2);
  r.setIteration(0) ;
  vpMatrix D(numberOfSignal(),numberOfSignal()) ;
  D.setIdentity() ;
  vpMatrix DA, DAmemory ;
  vpColVector DAx ;
  vpColVector w(numberOfSignal()) ;
  w =1 ;
  unsigned int nos_1 = numberOfSignal() ;

  if (list.size() < 3)
  {
    vpERROR_TRACE("Not enough point") ;
    throw(vpTrackingException(vpTrackingException::notEnoughPointError,
			      "not enough point")) ;
  }

  if (circle ==false)
  {
    vpMatrix A(numberOfSignal(),5) ;
    vpColVector x(5);

    unsigned int k =0 ;
    for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
      p_me = *it;
      if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
      {
        A[k][0] = vpMath::sqr(p_me.jfloat) ;
        A[k][1] = 2 * p_me.ifloat * p_me.jfloat ;
        A[k][2] = 2 * p_me.ifloat ;
        A[k][3] = 2 * p_me.jfloat ;
        A[k][4] = 1 ;

        b_[k] = - vpMath::sqr(p_me.ifloat) ;
        k++ ;
      }
    }

    while (iter < 4 )
    {
      DA = D*A ;
      vpMatrix DAp ;

      x = DA.pseudoInverse(1e-26) *D*b_ ;

      vpColVector residu(nos_1);
      residu = b_ - A*x;
      r.setIteration(iter) ;
      r.MEstimator(vpRobust::TUKEY,residu,w) ;

      k = 0;
      for (i=0 ; i < nos_1 ; i++)
      {
        D[k][k] =w[k]  ;
        k++;
      }
      iter++;
    }

    k =0 ;
    for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
      p_me = *it;
      if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
      {
        if (w[k] < thresholdWeight)
        {
          p_me.setState(vpMeSite::M_ESTIMATOR);
          
          *it = p_me;
        }
        k++ ;
      }
    }
    for(i = 0; i < 5; i ++)
      K[i] = x[i];
  }
  
  else
  {
    vpMatrix A(numberOfSignal(),3) ;
    vpColVector x(3);

    unsigned int k =0 ;
    for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
      p_me = *it;
      if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
      {
        A[k][0] = 2* p_me.ifloat ;
        A[k][1] = 2 * p_me.jfloat ;
        A[k][2] = 1 ;

        b_[k] = - vpMath::sqr(p_me.ifloat) - vpMath::sqr(p_me.jfloat) ;
        k++ ;
      }
    }

    while (iter < 4 )
    {
      DA = D*A ;
      vpMatrix DAp ;

      x = DA.pseudoInverse(1e-26) *D*b_ ;

      vpColVector residu(nos_1);
      residu = b_ - A*x;
      r.setIteration(iter) ;
      r.MEstimator(vpRobust::TUKEY,residu,w) ;

      k = 0;
      for (i=0 ; i < nos_1 ; i++)
      {
        D[k][k] =w[k];
        k++;
      }
      iter++;
    }

    k =0 ;
    for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
      p_me = *it;
      if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
      {
        if (w[k] < thresholdWeight)
        {
          p_me.setState(vpMeSite::M_ESTIMATOR);
      
          *it = p_me;
        }
        k++ ;
      }
    }
    for(i = 0; i < 3; i ++)
      K[i+2] = x[i];
  }
  getParameters() ;
}
Esempio n. 15
0
/*!

  Construct a list of vpMeSite moving edges at a particular sampling
  step between the two extremities of the line.

  \param I : Image in which the line appears.

  \exception vpTrackingException::initializationError : Moving edges not initialized.

*/
void
vpMeLine::sample(const vpImage<unsigned char>& I)
{
  if (!me) {
    vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
    throw(vpTrackingException(vpTrackingException::initializationError,
      "Moving edges not initialized")) ;
  }

  int rows = (int)I.getHeight() ;
  int cols = (int)I.getWidth() ;
  double n_sample;

  if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon())
  {
    vpERROR_TRACE("function called with sample step = 0") ;
    throw(vpTrackingException(vpTrackingException::fatalError,
                              "sample step = 0")) ;
  }

  // i, j portions of the line_p
  double diffsi = PExt[0].ifloat-PExt[1].ifloat;
  double diffsj = PExt[0].jfloat-PExt[1].jfloat;

  double length_p = sqrt((vpMath::sqr(diffsi)+vpMath::sqr(diffsj)));
  if(std::fabs(length_p)<=std::numeric_limits<double>::epsilon())
	  throw(vpTrackingException(vpTrackingException::fatalError,"points too close of each other to define a line")) ;
  // number of samples along line_p
  n_sample = length_p/(double)me->getSampleStep();  

  double stepi = diffsi/(double)n_sample;
  double stepj = diffsj/(double)n_sample;

  // Choose starting point
  double is = PExt[1].ifloat;
  double js = PExt[1].jfloat;

  // Delete old list
  list.clear();

  // sample positions at i*me->getSampleStep() interval along the
  // line_p, starting at PSiteExt[0]

  vpImagePoint ip;
  for(int i=0; i<=vpMath::round(n_sample); i++)
  {
    // If point is in the image, add to the sample list
    if(!outOfImage(vpMath::round(is), vpMath::round(js), 0, rows, cols))
    {
      vpMeSite pix ; //= list.value();
      pix.init((int)is, (int)js, delta, 0, sign) ;
      pix.setDisplay(selectDisplay) ;

      if(vpDEBUG_ENABLE(3))
      {
        ip.set_i( is );
        ip.set_j( js );
        vpDisplay::displayCross(I, ip, 2, vpColor::blue);
      }

      list.push_back(pix);
    }
    is += stepi;
    js += stepj;
	  
  }

  vpCDEBUG(1) << "end vpMeLine::sample() : ";
  vpCDEBUG(1) << n_sample << " point inserted in the list " << std::endl  ;
}
Esempio n. 16
0
/*!
  Track moving-edges.

  \param I : Image.

  \exception vpTrackingException::initializationError : Moving edges not initialized.

*/
void
vpMeTracker::track(const vpImage<unsigned char>& I)
{
  if (!me) {
    vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
    throw(vpTrackingException(vpTrackingException::initializationError,
      "Moving edges not initialized")) ;
  }

  if (list.empty())
  {
    vpDERROR_TRACE(2, "Tracking error: too few pixel to track");
    throw(vpTrackingException(vpTrackingException::notEnoughPointError,
      "too few pixel to track")) ;

  }

  vpImagePoint ip1, ip2;
  nGoodElement=0;
  //  int d =0;
  // Loop through list of sites to track
  for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
    vpMeSite s = *it;//current reference pixel

    //    d++ ;
    // If element hasn't been suppressed
    if(s.getState() == vpMeSite::NO_SUPPRESSION)
    {

      try{
        //	vpERROR_TRACE("%d",d ) ;
        //	vpERROR_TRACE("range %d",me->range) ;
        s.track(I,me,true);
      }
      catch(vpTrackingException)
      {
        vpERROR_TRACE("catch exception ") ;
        s.setState(vpMeSite::THRESHOLD);
      }

      if(s.getState() != vpMeSite::THRESHOLD)
      {
        nGoodElement++;

#if (DEBUG_LEVEL2)
        {
          double a,b ;
          a = s.i_1 - s.i ;
          b = s.j_1 - s.j ;
          if(s.getState() == vpMeSite::NO_SUPPRESSION) {
            ip1.set_i( s.i );
            ip1.set_j( s.j );
            ip2.set_i( s.i+a*5 );
            ip2.set_j( s.j+b*5 );
            vpDisplay::displayArrow(I, ip1, ip2, vpColor::green) ;
          }
        }
#endif

      }
      *it = s;
    }
  }
}
Esempio n. 17
0
/*!  
  Seek along the ellipse edge defined by its equation, the two extremities of
  the ellipse (ie the two points with the smallest and the biggest \f$ \alpha \f$ angle.

  \param I : Image in which the ellipse appears.

  \exception vpTrackingException::initializationError : Moving edges not initialized.

*/
void
vpMeEllipse::seekExtremities(const vpImage<unsigned char>  &I)
{
  if (!me) {
    vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
    throw(vpTrackingException(vpTrackingException::initializationError,
      "Moving edges not initialized")) ;
  }

  int rows = (int)I.getHeight() ;
  int cols = (int)I.getWidth() ;

  vpImagePoint ip;

  unsigned int  memory_range = me->getRange() ;
  me->setRange(2);

  double  memory_mu1 = me->getMu1();
  me->setMu1(0.5);

  double  memory_mu2 = me->getMu2();
  me->setMu2(0.5);

  double incr = vpMath::rad(2.0) ;

  if (alpha2-alpha1 < 2*M_PI-vpMath::rad(6.0))
  {
    vpMeSite P;
    double k = alpha1;
    double i1,j1;

    for (unsigned int i=0 ; i < 3 ; i++)
    {
      k -= incr;
      //while ( k < -M_PI ) { k+=2*M_PI; }

      i1 = b *cos(k) ; // equation of an ellipse
      j1 = a *sin(k) ; // equation of an ellipse
      P.ifloat = iPc.get_i() - se *j1 + ce *i1 ; P.i = (int)P.ifloat ;
      P.jfloat = iPc.get_j() + ce *j1 + se *i1 ; P.j = (int)P.jfloat ;

      if(!outOfImage(P.i, P.j, 5, rows, cols))
      {
        P.track(I,me,false) ;

        if (P.getState() == vpMeSite::NO_SUPPRESSION)
        {
          list.push_back(P);
          angle.push_back(k);
          if (vpDEBUG_ENABLE(3)) {
            ip.set_i( P.i );
            ip.set_j( P.j );

            vpDisplay::displayCross(I, ip, 5, vpColor::green) ;
          }
        }
        else {
	  if (vpDEBUG_ENABLE(3)) {
	    ip.set_i( P.i );
	    ip.set_j( P.j );
	    vpDisplay::displayCross(I, ip, 10, vpColor::blue) ;
	  }
        }
      }
    }

    k = alpha2;

    for (unsigned int i=0 ; i < 3 ; i++)
    {
      k += incr;
      //while ( k > M_PI ) { k-=2*M_PI; }

      i1 = b *cos(k) ; // equation of an ellipse
      j1 = a *sin(k) ; // equation of an ellipse
      P.ifloat = iPc.get_i() - se *j1 + ce *i1 ; P.i = (int)P.ifloat ;
      P.jfloat = iPc.get_j() + ce *j1 + se *i1 ; P.j = (int)P.jfloat ;

      if(!outOfImage(P.i, P.j, 5, rows, cols))
      {
        P.track(I,me,false) ;

        if (P.getState() == vpMeSite::NO_SUPPRESSION)
        {
          list.push_back(P);
          angle.push_back(k);
          if (vpDEBUG_ENABLE(3)) {
            ip.set_i( P.i );
            ip.set_j( P.j );

            vpDisplay::displayCross(I, ip, 5, vpColor::green) ;
          }
        }
        else {
          if (vpDEBUG_ENABLE(3)) {
            ip.set_i( P.i );
            ip.set_j( P.j );
            vpDisplay::displayCross(I, ip, 10, vpColor::blue) ;
          }
        }
      }
    }
  }

  suppressPoints() ;

  me->setRange(memory_range);
  me->setMu1(memory_mu1);
  me->setMu2(memory_mu2);
}
Esempio n. 18
0
/*!
	
  Compute the two parameters \f$(\rho, \theta)\f$ of the line.

  \param I : Image in which the line appears.
*/
void
vpMeLine::computeRhoTheta(const vpImage<unsigned char>& I)
{
  //rho = -c ;
  //theta = atan2(a,b) ;
  rho = fabs(c);
  theta = atan2(b,a) ;

  while (theta >= M_PI)    theta -=M_PI ;
  while (theta < 0)    theta +=M_PI ;
  
  if(_useIntensityForRho){

    /*  while(theta < -M_PI)	theta += 2*M_PI ;
    while(theta >= M_PI)	theta -= 2*M_PI ;

    // If theta is between -90 and -180 get the equivalent
    // between 0 and 90
    if(theta <-M_PI/2)
      {
        theta += M_PI ;
        rho *= -1 ;
      }
    // If theta is between 90 and 180 get the equivalent
    // between 0 and -90
    if(theta >M_PI/2)
      {
        theta -= M_PI ;
        rho *= -1 ;
      }
    */
    // convention pour choisir le signe de rho
    int i,j ;
    i = vpMath::round((PExt[0].ifloat + PExt[1].ifloat )/2) ;
    j = vpMath::round((PExt[0].jfloat + PExt[1].jfloat )/2) ;

    int  end = false ;
    int incr = 10 ;


    int i1=0,i2=0,j1=0,j2=0 ;
    unsigned char v1=0,v2=0 ;

    int width_ = (int)I.getWidth();
    int height_ = (int)I.getHeight();
    update_indices(theta,i,j,incr,i1,i2,j1,j2);
    
    if(i1<0 || i1>=height_ || i2<0 || i2>=height_ ||
       j1<0 || j1>=width_ || j2<0 || j2>=width_){
			double rho_lim1 = fabs((double)i/cos(theta));
			double rho_lim2 = fabs((double)j/sin(theta));

      double co_rho_lim1 = fabs(((double)(height_-i))/cos(theta));
      double co_rho_lim2 = fabs(((double)(width_-j))/sin(theta));

			double rho_lim = std::min(rho_lim1,rho_lim2);
			double co_rho_lim = std::min(co_rho_lim1,co_rho_lim2);
			incr = (int)std::floor(std::min(rho_lim,co_rho_lim));
			if(incr<INCR_MIN){
				vpERROR_TRACE("increment is too small") ;
				throw(vpTrackingException(vpTrackingException::fatalError,
                                  "increment is too small")) ;
			}
			update_indices(theta,i,j,incr,i1,i2,j1,j2);
    }

    while (!end)
    {
      end = true;
      unsigned int i1_ = static_cast<unsigned int>(i1);
      unsigned int j1_ = static_cast<unsigned int>(j1);
      unsigned int i2_ = static_cast<unsigned int>(i2);
      unsigned int j2_ = static_cast<unsigned int>(j2);
      v1=I[i1_][j1_];
      v2=I[i2_][j2_];
      if (abs(v1-v2) < 1)
      {

        incr-- ;
        end = false ;
        if (incr==1)
        {
          std::cout << "In CStraightLine::GetParameters() " ;
          std::cout << " Error Tracking " << abs(v1-v2) << std::endl ;
        }
      }
      update_indices(theta,i,j,incr,i1,i2,j1,j2);
    }

    if (theta >=0 && theta <= M_PI/2)
    {
      if (v2 < v1)
      {
        theta += M_PI;
        rho *= -1;
      }
    }

    else
    {
      double jinter;
      jinter = -c/b;
      if (v2 < v1)
      {
        theta += M_PI;
        if (jinter > 0)
        {
          rho *= -1;
        }
      }

      else
      {
        if (jinter < 0)
        {
          rho *= -1;
        }
      }
    }

    if (vpDEBUG_ENABLE(2))
    {
      vpImagePoint ip, ip1, ip2;

      ip.set_i( i );
      ip.set_j( j );
      ip1.set_i( i1 );
      ip1.set_j( j1 );
      ip2.set_i( i2 );
      ip2.set_j( j2 );

      vpDisplay::displayArrow(I, ip, ip1, vpColor::green) ;
      vpDisplay::displayArrow(I, ip, ip2, vpColor::red) ;
    }
  }

}
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();
}
Esempio n. 20
0
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;

      /////////////////////////////////////////////////////////////////////////
      // DIRECT

      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(point, Tij,IW,i,j,i2,j2,/*cr,ct,er,et,*/dx,dy) default(shared)
#endif
      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(i,j,i2,j2,p);
        X2[0]=j2;X2[1]=i2;

        //Warp->computeDenom(X1,p);
        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.);
          //et=((double)IW*(Nc-1))/255.-ct;
          //cr=ptTemplateSupp[point].ct;
          //er=ptTemplateSupp[point].et;

          Warp->dWarpCompo(X1,X2,p,ptTemplateCompo[point].dW,dW);

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


          //calcul de l'erreur
          erreur+=(Tij-IW)*(Tij-IW);
          //                    if(bspline==3)
          //                        vpTemplateTrackerMIBSpline::PutTotPVBspline3NoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam);
          //                    else
          //                        vpTemplateTrackerMIBSpline::PutTotPVBspline4NoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam);
          //					vpTemplateTrackerMIBSpline::computeProbabilities(PrtTout,cr, er, ct, et, Nc, tptemp, nbParam,bspline,ApproxHessian,
          //                               hessianComputation==vpHessienType::USE_HESSIEN_DESIRE);

          delete[] tptemp;

        }
      }

      computeProba(Nbpoint);
      computeMI(MI);
      if(hessianComputation!=vpTemplateTrackerMI::USE_HESSIEN_DESIRE)
        computeHessien(HDirect);
      computeGradient();
      GDirect=G;

      if(hessianComputation!=vpTemplateTrackerMI::USE_HESSIEN_DESIRE)
      {
        H=HDirect+HInverse;
        vpMatrix::computeHLM(H,lambda,HLM);
      }
      G=GDirect-GInverse;
      //G=GDirect;
      //G=-GInverse;

      try
      {
        if(minimizationMethod==vpTemplateTrackerMIESM::USE_GRADIENT)
          dp=-gain*0.3*G;
        else
        {
          switch(hessianComputation)
          {
          case vpTemplateTrackerMI::USE_HESSIEN_DESIRE:
            dp=gain*HLMdesireInverse*G;
            break;
          case vpTemplateTrackerMI::USE_HESSIEN_BEST_COND:
            if(HLM.cond()>HLMdesire.cond())
              dp=gain*HLMdesireInverse*G;
            else
              dp=gain*0.3*HLM.inverseByLU()*G;
            break;
          default:
            dp=gain*0.3*HLM.inverseByLU()*G;
            break;
          }
        }
      }
      catch(vpException &e)
      {
        //std::cerr<<"probleme inversion"<<std::endl;
        throw(e);
      }

      if(ApproxHessian==HESSIAN_NONSECOND)
        dp=-1.*dp;

      if(useBrent)
      {
        alpha=2.;
        computeOptimalBrentGain(I,p,-MI,dp,alpha);
        dp=alpha*dp;
      }
      p+=dp;

      iteration++;
    }
  }
  while( /*(MI!=MIprec) &&*/(iteration< iterationMax));

  MI_postEstimation=-getCost(I,p);
  if(MI_preEstimation>MI_postEstimation)
  {
    MI_postEstimation = -1;
  }

  nbIteration=iteration;
}
Esempio n. 21
0
/*!
  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) ;
  }
  if  (v-1 >= 0)
  {
    if (I[v-1][u] >=gray_level_min && I[v-1][u] <= gray_level_max)
      connexe(I,u, v-1,gray_level_min, gray_level_max, mean_value,
	      u_cog, v_cog, n) ;
  }
  if  (v+1 < height)
  {
    if (I[v+1][u] >=gray_level_min && I[v+1][u] <= gray_level_max)
      connexe(I,u,v+1,gray_level_min, gray_level_max, mean_value,
	      u_cog, v_cog, n) ;
  }

  if (connexity == CONNEXITY_8) {
    if ( (u-1 >= 0) && (v-1 >= 0) )
    {

      if (I[v-1][u-1] >=gray_level_min && I[v-1][u-1] <= gray_level_max)
	connexe(I,u-1,v-1,gray_level_min, gray_level_max, mean_value,
		u_cog, v_cog, n) ;
    }

    if ( (u+1 < width) && (v-1 >= 0 ) )
    {

      if (I[v-1][u+1] >=gray_level_min && I[v-1][u+1] <= gray_level_max)
	connexe(I,u+1,v-1,gray_level_min, gray_level_max, mean_value,
		u_cog, v_cog, n) ;
    }
    if  ( (v+1 < height) && (u-1 >= 0) )
    {

      if (I[v+1][u-1] >=gray_level_min && I[v+1][u-1] <= gray_level_max)
	connexe(I,u-1,v+1,gray_level_min, gray_level_max, mean_value,
		u_cog, v_cog, n) ;
    }
    if  ( (v+1 < height) && (u+1 < width) )
    {

      if (I[v+1][u+1] >=gray_level_min && I[v+1][u+1] <= gray_level_max)
	connexe(I,u+1,v+1,gray_level_min, gray_level_max, mean_value,
		u_cog, v_cog, n) ;
    }
  }
  
  return vpDot::in ;
}
Esempio n. 22
0
/*!
  Virtual function that is called by lower classes vpMeEllipse, vpMeLine
  and vpMeNurbs.

  \exception vpTrackingException::initializationError : Moving edges not initialized.
*/
void
vpMeTracker::initTracking(const vpImage<unsigned char>& I)
{
  if (!me) {
    vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
    throw(vpTrackingException(vpTrackingException::initializationError,
      "Moving edges not initialized")) ;
  }

  // Must set range to 0
  unsigned int range_tmp = me->getRange();
  me->setRange(init_range);

  nGoodElement=0;

  int d = 0;
  vpImagePoint ip1, ip2;

  // Loop through list of sites to track
  for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
    vpMeSite refp = *it;//current reference pixel

    d++ ;
    // If element hasn't been suppressed
    if(refp.getState() == vpMeSite::NO_SUPPRESSION)
    {
      try {
        refp.track(I,me,false);
      }
      catch(...)
      {
        // EM verifier quel signal est de sortie !!!
        vpERROR_TRACE("Error caught") ;
        throw ;
      }
      if(refp.getState() == vpMeSite::NO_SUPPRESSION) nGoodElement++;
    }


#if (DEBUG_LEVEL2)
    {
      double a,b ;
      a = refp.i_1 - refp.i ;
      b = refp.j_1 - refp.j ;
      if(refp.getState() == vpMeSite::NO_SUPPRESSION) {
        ip1.set_i( refp.i );
        ip1.set_j( refp.j );
        ip2.set_i( refp.i+a );
        ip2.set_j( refp.j+b );
        vpDisplay::displayArrow(I, ip1, ip2, vpColor::green) ;
      }
    }
#endif
    *it = refp;
  }

  /*
  if (res != OK)
  {
  std::cout<< "In vpMeTracker::initTracking(): " ;
  switch (res)
  {
  case  ERR_TRACKING:
  std::cout << "vpMeTracker::initTracking:Track return ERR_TRACKING " << std::endl ;
  break ;
  case fatalError:
  std::cout << "vpMeTracker::initTracking:Track return fatalError" << std::endl ;
  break ;
  default:
  std::cout << "vpMeTracker::initTracking:Track return error " << res << std::endl ;
  }
  return res ;
  }
  */

  me->setRange(range_tmp);
}
Esempio n. 23
0
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.;
      computeOptimalBrentGain(I,p,erreur/Nbpoint,dp,alpha);
      dp=alpha*dp;
    }

    //Warp->pRondp(p,dp,p);
    p+=dp;
    iteration++;

  }
  while( (iteration < iterationMax));

  nbIteration=iteration;
}
Esempio n. 24
0
/*!

  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 around 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(const 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_connexities_list.clear() ;
  ip_edges_list.clear();
  
  // 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_connexities_list.clear() ;
	 
	  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, mean_gray_level, u_cog, v_cog, npoint) )
  {
    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_connexities_list.clear() ;
	ip_edges_list.clear();
	
	this->mean_gray_level = 0 ;
	if ( connexe(I, (unsigned int)u_+k, (unsigned int)(v_),mean_gray_level, u_cog, v_cog, npoint) ) {
	  sol = true; u = u_+k; v = v_;
	}
      }
      u_ += k;
      right += 2;

      for (k=1; k <= botom; k++) if (sol==false) {
	u_cog = 0 ;
	v_cog = 0 ;
	ip_connexities_list.clear() ;
	ip_edges_list.clear();
	
	this->mean_gray_level = 0 ;
	
	if ( connexe(I, (unsigned int)(u_), (unsigned int)(v_+k),mean_gray_level, u_cog, v_cog, npoint) ) {
	  sol = true; u = u_; v = v_+k;
	}
      }
      v_ += k;
      botom += 2;

      for (k=1; k <= left; k++) if (sol==false) {
	u_cog = 0 ;
	v_cog = 0 ;
	ip_connexities_list.clear() ;
	ip_edges_list.clear();
	
	this->mean_gray_level = 0 ;

	if ( connexe(I, (unsigned int)(u_-k), (unsigned int)(v_),mean_gray_level,u_cog, v_cog, npoint) ) {
	  sol = true ; u = u_-k; v = v_;
	}
      }
      u_ -= k;
      left += 2;

      for (k=1; k <= up; k++) if(sol==false) {
	u_cog = 0 ;
	v_cog = 0 ;
	ip_connexities_list.clear() ;
	ip_edges_list.clear();
	
	this->mean_gray_level = 0 ;

	if ( connexe(I, (unsigned int)(u_), (unsigned int)(v_-k),mean_gray_level,u_cog, v_cog, npoint) ) {
	  sol = true ; u = u_; v = v_-k;
	}
      }
      v_ -= k;
      up += 2;
    }

    if (sol == false) {
      vpERROR_TRACE("Dot has been lost") ;
      throw(vpTrackingException(vpTrackingException::featureLostError,
				"Dot has been lost")) ;
    }
  }

#endif
/*
  vpImagePoint ip;
  unsigned int i, j;
  std::list<vpImagePoint>::iterator it;
  for (it = ip_connexities_list.begin(); it != ip_connexities_list.end(); it ++) {
    ip = *it;
    i = (unsigned int) ip.get_i();
    j = (unsigned int) ip.get_j();
    I[i][j] = 255 ;
  }*/

  u_cog = u_cog/npoint ;
  v_cog = v_cog/npoint ;

  u = u_cog ;
  v = v_cog ;

  // Initialize the threshold for the next call to track()
  double Ip = pow((double)this->mean_gray_level/255,1/gamma);

  if(Ip - (1 - grayLevelPrecision)<0){
    gray_level_min = 0 ;
  }
  else{
    gray_level_min = (unsigned int) (255*pow(Ip - (1 - grayLevelPrecision),gamma));
    if (gray_level_min > 255)
      gray_level_min = 255;
  }
  gray_level_max = (unsigned int) (255*pow(Ip + (1 - grayLevelPrecision),gamma));
  if (gray_level_max > 255)
    gray_level_max = 255;

  //vpCTRACE << "gray_level_min: " << gray_level_min << std::endl;
  //vpCTRACE << "gray_level_max: " << gray_level_max << std::endl;

  if (npoint < 5)
  {
    vpERROR_TRACE("Dot to small") ;
    throw(vpTrackingException(vpTrackingException::featureLostError,
			      "Dot to small")) ;
  }

  if (npoint > nbMaxPoint)
  {
    vpERROR_TRACE("Too many point %lf (%lf%%). Max allowed is %lf (%lf%%). This threshold can be modified using the setMaxDotSize() method.",
		  npoint, npoint / (I.getWidth() * I.getHeight()),
		  nbMaxPoint, maxDotSizePercentage) ;

   throw(vpTrackingException(vpTrackingException::featureLostError,
			      "Dot to big")) ;
  }  
}
Esempio n. 25
0
bool vpDot::connexe(const vpImage<unsigned char>& I,unsigned int u,unsigned int v,
	       double &mean_value, double &u_cog, double &v_cog, double &n,std::vector<bool> &checkTab)
{

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

  // Test if we are in the image
  if ( (u >= width) || (v >= height) )
  {
    //std::cout << "out of bound" << std::endl;
    return false;
  }
  
  if(checkTab[u + v*I.getWidth()])
    return true;
  
  vpImagePoint ip;
  ip.set_u(u);
  ip.set_v(v);	
  
  if (I[v][u] >= gray_level_min && I[v][u] <= gray_level_max)
  {
    checkTab[v*I.getWidth() + u] = true;

    ip_connexities_list.push_back(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;
    
    // 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 ;
    }
  }
  else
  {
    //std::cout << "not in" << std::endl;
    return false;
  }
  
  bool edge = false;
  
  //if((int)u-1 >= 0)
  if(u >= 1)
    if(!checkTab[u-1 + v*I.getWidth()])
      if(!connexe(I,u-1,v, mean_value,u_cog,v_cog, n, checkTab))
	edge = true;
  
  if(u+1 < I.getWidth())
    if(!checkTab[u+1+v*I.getWidth()])
      if(!connexe(I,u+1,v,mean_value,u_cog, v_cog, n, checkTab))
	edge = true;
      
  if(v >= 1)
    if(!checkTab[u+(v-1)*I.getWidth()])
      if(!connexe(I,u, v-1,mean_value,u_cog, v_cog, n, checkTab))
	edge = true;
      
  if(v+1 < I.getHeight())
    if(!checkTab[u+(v+1)*I.getWidth()])
      if(!connexe(I,u,v+1,mean_value,u_cog, v_cog, n, checkTab))
	edge = true;
  
  if (connexityType == CONNEXITY_8) {
    if(v >= 1 && u >= 1)
      if(!checkTab[u-1+(v-1)*I.getWidth()])
        if(!connexe(I,u-1,v-1,mean_value,u_cog, v_cog, n, checkTab))
          edge = true;
	  
    if(v >= 1 && u+1 < I.getWidth())
      if(!checkTab[u+1+(v-1)*I.getWidth()])
        if(!connexe(I,u+1,v-1,mean_value,u_cog, v_cog, n, checkTab))
          edge = true;
	  
    if(v+1 < I.getHeight() && u >= 1)
      if(!checkTab[u-1+(v+1)*I.getWidth()])
        if(!connexe(I,u-1,v+1,mean_value, u_cog, v_cog, n, checkTab))
          edge = true;
	  
    if(v+1 < I.getHeight() && u+1 < I.getWidth())
      if(!checkTab[u+1+(v+1)*I.getWidth()])
        if(!connexe(I,u+1,v+1,mean_value,u_cog, v_cog, n, checkTab))
          edge = true;
   }
  
  if(edge){
    ip_edges_list.push_back(ip);
    if (graphics==true)
    {
      vpImagePoint ip_(ip);
      for(unsigned int t=0; t<thickness; t++) {
        ip_.set_u(ip.get_u() + t);
        vpDisplay::displayPoint(I, ip_, vpColor::red) ;
      }
      //vpDisplay::flush(I);
    }
  }
  
  return true;
}
Esempio n. 26
0
void vpTemplateTracker::trackPyr(const vpImage<unsigned char> &I)
{
  //vpTRACE("trackPyr");
  vpImage<unsigned char> *pyr_I;
//  pyr_I=new vpImage<unsigned char>[nbLvlPyr+1]; // Why +1 ?
  pyr_I=new vpImage<unsigned char>[nbLvlPyr]; // Why +1 ?
  pyr_I[0]=I;

  try
  {
      vpColVector ptemp(nbParam);
      if(nbLvlPyr>1)
      {
    //    vpColVector *p_sauv=new vpColVector[nbLvlPyr];
    //    for(unsigned int i=0;i<nbLvlPyr;i++)p_sauv[i].resize(nbParam);

    //    p_sauv[0]=p;
        for(unsigned int i=1;i<nbLvlPyr;i++)
        {
          vpImageFilter::getGaussPyramidal(pyr_I[i-1],pyr_I[i]);
          //test getParamPyramidDown
          /*vpColVector vX_test(2);vX_test[0]=15.;vX_test[1]=30.;
          vpColVector vX_test2(2);
          Warp->computeCoeff(p);
          Warp->computeDenom(vX_test,p);
          Warp->warpX(vX_test,vX_test2,p);
          std::cout<<"p = "<<p.t()<<std::endl;*/
          //std::cout<<"get p down"<<std::endl;
          Warp->getParamPyramidDown(p,ptemp);
          p=ptemp;
          zoneTracked=&zoneTrackedPyr[i];

    //      p_sauv[i]=p;
          /*std::cout<<"p_down = "<<p.t()<<std::endl;

          vpColVector vX_testd(2);vX_testd[0]=15./2.;vX_testd[1]=30./2.;
          vpColVector vX_testd2(2);
          Warp->computeCoeff(p);
          Warp->computeDenom(vX_testd,p);
          Warp->warpX(vX_testd,vX_testd2,p);
          std::cout<<2.*vX_testd2[0]<<","<<2.*vX_testd2[1]<<" <=> "<<vX_test2[0]<<","<<vX_test2[1]<<std::endl;*/

        }

        for(int i=(int)nbLvlPyr-1;i>=0;i--)
        {
          if(i>=(int)l0Pyr)
          {
            templateSize=templateSizePyr[i];
            ptTemplate=ptTemplatePyr[i];
            ptTemplateSelect=ptTemplateSelectPyr[i];
            ptTemplateSupp=ptTemplateSuppPyr[i];
            ptTemplateCompo=ptTemplateCompoPyr[i];
            H=HdesirePyr[i];
            HLM=HLMdesirePyr[i];
            HLMdesireInverse=HLMdesireInversePyr[i];
    //        zoneTracked=&zoneTrackedPyr[i];
            trackRobust(pyr_I[i]);
          }
          //std::cout<<"get p up"<<std::endl;
    //      ptemp=p_sauv[i-1];
          if (i > 0) {
            Warp->getParamPyramidUp(p,ptemp);
            p=ptemp;
            zoneTracked=&zoneTrackedPyr[i-1];
          }
        }
    #if 0
        if(l0Pyr==0)
        {
          templateSize=templateSizePyr[0];
          ptTemplate=ptTemplatePyr[0];
          ptTemplateSelect=ptTemplateSelectPyr[0];
          ptTemplateSupp=ptTemplateSuppPyr[0];
          ptTemplateCompo=ptTemplateCompoPyr[0];
          H=HdesirePyr[0];
          HLM=HLMdesirePyr[0];
          HLMdesireInverse=HLMdesireInversePyr[0];
          zoneTracked=&zoneTrackedPyr[0];
          trackRobust(pyr_I[0]);
        }

        if (l0Pyr > 0) {
    //      for (int l=(int)l0Pyr; l >=0; l--) {
    //        Warp->getParamPyramidUp(p,ptemp);
    //        p=ptemp;
    //      }
          zoneTracked=&zoneTrackedPyr[0];
        }
    #endif
        //    delete [] p_sauv;
      }
      else
      {
        //std::cout<<"reviens a tracker de base"<<std::endl;
        trackRobust(I);
      }
      delete[] pyr_I;
  }
  catch(vpException &e){
      delete[] pyr_I;
      throw(vpTrackingException(vpTrackingException::badValue, e.getMessage()));
  }
}
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;
      deletePosEvalRMS();
      throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
    }
    else
    {
      computeProba(Nbpoint);
      computeMI(MI);
      //std::cout<<iteration<<"\tMI= "<<MI<<std::endl;
      computeHessien(H);
      computeGradient();

      vpMatrix::computeHLM(H,lambda,HLM);
      try
      {
        switch(hessianComputation)
        {
        case vpTemplateTrackerMI::USE_HESSIEN_DESIRE:
          dp=gain*HLMdesireInverse*G;
          break;
        case vpTemplateTrackerMI::USE_HESSIEN_BEST_COND:
          if(HLM.cond()>HLMdesire.cond())
            dp=gain*HLMdesireInverse*G;
          else
            dp=gain*0.2*HLM.inverseByLU()*G;
          break;
        default:
          dp=gain*0.2*HLM.inverseByLU()*G;
          break;
        }
      }
      catch(vpException &e)
      {
        //std::cerr<<"probleme inversion"<<std::endl;
        deletePosEvalRMS();
        throw(e);
      }
    }

    switch(minimizationMethod)
    {
    case vpTemplateTrackerMIForwardAdditional::USE_LMA:
    {
      vpColVector p_test_LMA(nbParam);
      if(ApproxHessian==HESSIAN_NONSECOND)
        p_test_LMA=p-100000.1*dp;
      else
        p_test_LMA=p+1.*dp;
      MI=-getCost(I,p);
      double MI_LMA=-getCost(I,p_test_LMA);
      if(MI_LMA>MI)
      {
        p=p_test_LMA;
        lambda=(lambda/10.<1e-6)?lambda/10.:1e-6;
      }
      else
      {
        lambda=(lambda*10.<1e6)?1e6:lambda*10.;
      }
    }
      break;
    case vpTemplateTrackerMIForwardAdditional::USE_GRADIENT:
    {
      dp=-gain*6.0*G;
      if(useBrent)
      {
        alpha=2.;
        computeOptimalBrentGain(I,p,-MI,dp,alpha);
        dp=alpha*dp;
      }
      p+=1.*dp;
      break;
    }

    case vpTemplateTrackerMIForwardAdditional::USE_QUASINEWTON:
    {
      double s_scal_y;
      if(iterationGlobale!=0)
      {
        vpColVector s_quasi=p-p_prec;
        vpColVector y_quasi=G-G_prec;
        s_scal_y=s_quasi.t()*y_quasi;
        //if(s_scal_y!=0)//BFGS
        //	KQuasiNewton=KQuasiNewton-(s_quasi*y_quasi.t()*KQuasiNewton+KQuasiNewton*y_quasi*s_quasi.t())/s_scal_y+(1.+y_quasi.t()*(KQuasiNewton*y_quasi)/s_scal_y)*s_quasi*s_quasi.t()/s_scal_y;
        //if(s_scal_y!=0)//DFP
        if(std::fabs(s_scal_y) > std::numeric_limits<double>::epsilon())
          KQuasiNewton=KQuasiNewton+0.001*(s_quasi*s_quasi.t()/s_scal_y-KQuasiNewton*y_quasi*y_quasi.t()*KQuasiNewton/(y_quasi.t()*KQuasiNewton*y_quasi));
      }
      dp=-KQuasiNewton*G;
      p_prec=p;
      G_prec=G;
      p-=1.01*dp;
    }
      break;

    default:
    {
      if(ApproxHessian==HESSIAN_NONSECOND)
        dp=-0.1*dp;
      if(useBrent)
      {
        alpha=2.;
        computeOptimalBrentGain(I,p,-MI,dp,alpha);
        //std::cout<<alpha<<std::endl;
        dp=alpha*dp;
      }

      p+=1.*dp;
      break;
    }
    }

    computeEvalRMS(p);
    iteration++;
    iterationGlobale++;

  }
  while( (std::fabs(MI-MIprec) > std::fabs(MI)*std::numeric_limits<double>::epsilon()) &&(iteration< iterationMax)&&(evolRMS>threshold_RMS) );
  //while( (MI!=MIprec) &&(iteration< iterationMax)&&(evolRMS>threshold_RMS) );
  if(Nbpoint==0) {
    //std::cout<<"plus de point dans template suivi"<<std::endl;
    deletePosEvalRMS();
    throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
  }

  nbIteration=iteration;
  MI_postEstimation=-getCost(I,p);
  if(MI_preEstimation>MI_postEstimation)
  {
    MI_postEstimation = -1;
  }
  deletePosEvalRMS();
}
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;


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

#if defined(USE_OPENMP_MI_INVCOMP) && defined(VISP_HAVE_OPENMP)
  int Ncb_ = (int)Ncb;
  int nbParam_ = (int)nbParam;
  int sizePrtBis = Ncb_*Ncb_;
  int sizedPrtBis = Ncb_*Ncb_*nbParam_;
  int sized2PrtBis = Ncb_*Ncb_*nbParam_*nbParam_;
#endif

  vpMatrix Hnorm(nbParam,nbParam);

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

#ifndef USE_OPENMP_MI_INVCOMP
    zeroProbabilities();
#endif

    Warp->computeCoeff(p);

#if defined(USE_OPENMP_MI_INVCOMP) && defined(VISP_HAVE_OPENMP)
#pragma omp parallel
#endif
    {
#if defined(USE_OPENMP_MI_INVCOMP) && defined(VISP_HAVE_OPENMP)
      double *Prtbis = new double[sizePrtBis];
      double *dPrtbis = new double[sizedPrtBis];
      double *d2Prtbis = new double[sized2PrtBis];

      unsigned int indd, indd2;
      indd = indd2 = 0;
      for(int i=0;i<Ncb_*Ncb_;i++){
        Prtbis[i]=0.0;
        Prt[i]=0.0;
        for(int j=0;j<nbParam_;j++){
          dPrtbis[indd]=0.0;
          dPrt[indd]=0.0;
          indd++;
          for(int k=0;k<nbParam_;k++){
            d2Prtbis[indd2]=0.0;
            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
                //                            vpTemplateTrackerMIBSpline::PutTotPVBspline3(ptb, er, ptTemplateSupp[point].BtInit, size);
                //                            // ###################
                //                        }
              }
              else{
#if defined(USE_OPENMP_MI_INVCOMP) && defined(VISP_HAVE_OPENMP)
                vpTemplateTrackerMIBSpline::PutTotPVBspline4(Prtbis, dPrtbis, d2Prtbis, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam);
#else
                vpTemplateTrackerMIBSpline::PutTotPVBspline4(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam);
#endif
              }
            }
            else{
#if defined(USE_OPENMP_MI_INVCOMP) && defined(VISP_HAVE_OPENMP)
              vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(Prtbis, cr, er, ct, et, Ncb ,nbParam, bspline);
#else
              vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(Prt, cr, er, ct, et, Ncb,nbParam, bspline);
#endif
            }
          }

        }
      }

#if defined(USE_OPENMP_MI_INVCOMP) && defined(VISP_HAVE_OPENMP)
#pragma omp critical
      {
        unsigned int indd, indd2;
        indd = indd2 = 0;
        for(int i=0;i<Ncb*Ncb;i++){
          Prt[i]+=Prtbis[i]/(double)Nbpoint;
          for(int j=0;j<nbParam_;j++){
            dPrt[indd]+=dPrtbis[indd]/(double)Nbpoint;
            indd++;
            for(int k=0;k<nbParam_;k++){
              d2Prt[indd2]+=d2Prtbis[indd2]/(double)Nbpoint;
              indd2++;
            }
          }
        }

        delete [] Prtbis;
        delete [] dPrtbis;
        delete [] d2Prtbis;
      }

#pragma omp barrier
#endif
    }

    if(Nbpoint==0)
    {
      diverge=true;
      MI=0;
      deletePosEvalRMS();
      throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));

    }
    else
    {
      //            computeProba(Nbpoint);

#ifndef USE_OPENMP_MI_INVCOMP
      unsigned int indd, indd2;
      indd = indd2 = 0;
      unsigned int Ncb_ = (unsigned int)Ncb;
      for(unsigned int i=0;i<Ncb_*Ncb_;i++){
        Prt[i]=Prt[i]/Nbpoint;
        for(unsigned int j=0;j<nbParam;j++){
          dPrt[indd]=dPrt[indd]/Nbpoint;
          indd++;
          for(unsigned int k=0;k<nbParam;k++){
            d2Prt[indd2]=d2Prt[indd2]/Nbpoint;
            indd2++;
          }
        }
      }
#endif

      computeMI(MI);

      if(hessianComputation!=vpTemplateTrackerMI::USE_HESSIEN_DESIRE){
        computeHessienNormalized(Hnorm);
        computeHessien(H);
      }
      computeGradient();

      vpMatrix::computeHLM(H,lambda,HLM);

      try
      {
        switch(hessianComputation)
        {
        case vpTemplateTrackerMI::USE_HESSIEN_DESIRE:
          dp=gain*HLMdesireInverse*G;
          break;
        case vpTemplateTrackerMI::USE_HESSIEN_BEST_COND:
          if(HLM.cond()>HLMdesire.cond())
            dp=gain*HLMdesireInverse*G;
          else
            dp=gain*0.2*HLM.inverseByLU()*G;
          break;
        default:
          dp=gain*0.2*HLM.inverseByLU()*G;
          break;
        }
      }
      catch(vpException &e)
      {
        //std::cerr<<"probleme inversion"<<std::endl;
        throw(e);
      }
    }

    switch(minimizationMethod)
    {
    case vpTemplateTrackerMIInverseCompositional::USE_LMA:
    {
      vpColVector dp_test_LMA(nbParam);
      vpColVector dpinv_test_LMA(nbParam);
      vpColVector p_test_LMA(nbParam);
      if(ApproxHessian==HESSIAN_NONSECOND)
        dp_test_LMA=-100000.1*dp;
      else
        dp_test_LMA=1.*dp;
      Warp->getParamInverse(dp_test_LMA,dpinv_test_LMA);
      Warp->pRondp(p,dpinv_test_LMA,p_test_LMA);

      MI=-getCost(I,p);
      double MI_LMA=-getCost(I,p_test_LMA);
      if(MI_LMA>MI)
      {
        dp=dp_test_LMA;
        lambda=(lambda/10.<1e-6)?lambda/10.:1e-6;
      }
      else
      {
        dp=0;
        lambda=(lambda*10.<1e6)?1e6:lambda*10.;
      }
    }
      break;
    case vpTemplateTrackerMIInverseCompositional::USE_GRADIENT:
      dp=-gain*0.3*G*20;
      break;

    case vpTemplateTrackerMIInverseCompositional::USE_QUASINEWTON:
    {
      double s_scal_y;
      if(iterationGlobale!=0)
      {
        vpColVector s_quasi=p-p_prec;
        vpColVector y_quasi=G-G_prec;
        s_scal_y=s_quasi.t()*y_quasi;
        //std::cout<<"mise a jour K"<<std::endl;
        /*if(s_scal_y!=0)//BFGS
                    KQuasiNewton=KQuasiNewton+0.01*(-(s_quasi*y_quasi.t()*KQuasiNewton+KQuasiNewton*y_quasi*s_quasi.t())/s_scal_y+(1.+y_quasi.t()*(KQuasiNewton*y_quasi)/s_scal_y)*s_quasi*s_quasi.t()/s_scal_y);*/
        //if(s_scal_y!=0)//DFP
        if(std::fabs(s_scal_y) > std::numeric_limits<double>::epsilon())//DFP
        {
          KQuasiNewton=KQuasiNewton+0.0001*(s_quasi*s_quasi.t()/s_scal_y-KQuasiNewton*y_quasi*y_quasi.t()*KQuasiNewton/(y_quasi.t()*KQuasiNewton*y_quasi));
          //std::cout<<"mise a jour K"<<std::endl;
        }
      }
      dp=gain*KQuasiNewton*G;
      //std::cout<<KQuasiNewton<<std::endl<<std::endl;
      p_prec=p;
      G_prec=G;
      //p-=1.01*dp;
    }
      break;

    default:
    {
      if(useBrent)
      {
        alpha=2.;
        computeOptimalBrentGain(I,p,-MI,dp,alpha);
        dp=alpha*dp;
      }
      if(ApproxHessian==HESSIAN_NONSECOND)
        dp=-1.*dp;

      break;
    }
    }

    Warp->getParamInverse(dp,dpinv);
    Warp->pRondp(p,dpinv,p);

    iteration++;
    iterationGlobale++;

    computeEvalRMS(p);

    //        std::cout << p.t() << std::endl;
  }
  while( (!diverge) && (std::fabs(MI-MIprec) > std::fabs(MI)*std::numeric_limits<double>::epsilon()) &&(iteration< iterationMax)&&(evolRMS>threshold_RMS) );
  //while( (!diverge) && (MI!=MIprec) &&(iteration< iterationMax)&&(evolRMS>threshold_RMS) );

  nbIteration=iteration;

  if(diverge)
  {
    if(computeCovariance){
      covarianceMatrix = vpMatrix(Warp->getNbParam(),Warp->getNbParam());
      covarianceMatrix = -1;
      MI_postEstimation = -1;
      NMI_postEstimation = -1;
    }
    deletePosEvalRMS();

    //        throw(vpTrackingException(vpTrackingException::badValue, "Tracking failed")) ;
  }
  else
  {
    MI_postEstimation=-getCost(I,p);
    NMI_postEstimation=-getNormalizedCost(I,p);
    //        std::cout << "MI apres: " << MI_postEstimation << std::endl;
    //        std::cout << "NMI apres: " << NMI_postEstimation << std::endl;
    if(MI_preEstimation>MI_postEstimation)
    {
      p=p_avant_estimation;
      MI_postEstimation = MI_preEstimation;
      NMI_postEstimation = NMI_preEstimation;
      covarianceMatrix = vpMatrix(Warp->getNbParam(),Warp->getNbParam());
      covarianceMatrix = -1;
    }

    deletePosEvalRMS();

    if(computeCovariance){
      try{
        covarianceMatrix = (-H).inverseByLU();
        //            covarianceMatrix = (-Hnorm).inverseByLU();
      }
      catch(...){
        covarianceMatrix = vpMatrix(Warp->getNbParam(),Warp->getNbParam());
        covarianceMatrix = -1;
        MI_postEstimation = -1;
        NMI_postEstimation = -1;
        deletePosEvalRMS();
        //            throw(vpMatrixException(vpMatrixException::matrixError, "Covariance not inversible")) ;
      }
    }
  }
}