vpMeLine.cpp

/****************************************************************************
 *
 * $Id: vpMeLine.cpp 3672 2012-04-04 15:49:57Z ayol $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2012 by INRIA. All rights reserved.
 * 
 * This software is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * ("GPL") version 2 as published by the Free Software Foundation.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact INRIA about acquiring a ViSP Professional 
 * Edition License.
 *
 * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
 * 
 * This software was developed at:
 * INRIA Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 * http://www.irisa.fr/lagadic
 *
 * If you have questions regarding the use of this file, please contact
 * INRIA at visp@inria.fr
 * 
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 *
 * Description:
 * Moving edges.
 *
 * Authors:
 * Eric Marchand
 *
 *****************************************************************************/


#include <visp/vpMeTracker.h>
#include <visp/vpMe.h>
#include <visp/vpMeSite.h>
#include <visp/vpMeLine.h>
#include <visp/vpRobust.h>
#include <visp/vpTrackingException.h>
#include <visp/vpImagePoint.h>
#include <visp/vpMath.h>
#include <stdlib.h>
#include <cmath>    // std::fabs
#include <limits>   // numeric_limits
#define INCR_MIN 1

static void
normalizeAngle(double &delta)
{
  while (delta > M_PI) { delta -= M_PI ; }
  while (delta < -M_PI) { delta += M_PI ; }
}

void
computeDelta(double &delta, int i1, int j1, int i2, int j2)
{

  double B = double(i1-i2) ;
  double A = double(j1-j2) ;

  delta =  atan2(B,A) ;
  delta -= M_PI/2.0 ;
  normalizeAngle(delta) ;

}

static void
project(double a, double b, double c, 
        double i, double j, double &ip,double  &jp)
{
  if (fabs(a)>fabs(b))
  {
    jp = (vpMath::sqr(a)*j - a*b*i - c*b)/(vpMath::sqr(a)+vpMath::sqr(b)) ;
    ip = (-c-b*jp)/a;
  }
  else
  {
    ip = (vpMath::sqr(b)*i-a*b*j-c*a)/(vpMath::sqr(a)+vpMath::sqr(b)) ;
    jp = (-c-a*ip)/b;
  }
}

vpMeLine::vpMeLine():vpMeTracker()
{
  sign = 1;
  angle_1 = 90;
  _useIntensityForRho = true;
}
vpMeLine::vpMeLine(const vpMeLine &meline):vpMeTracker(meline)
{
  rho = meline.rho;
  theta = meline.theta;
  delta = meline.delta;
  delta_1 = meline.delta_1;
  angle = meline.angle;
  angle_1 = meline.angle_1;
  sign = meline.sign;

  a = meline.a;
  b = meline.b;
  c = meline.c;
  _useIntensityForRho = meline._useIntensityForRho;
  PExt[0] = meline.PExt[0];
  PExt[1] = meline.PExt[1];
}

vpMeLine::~vpMeLine()
{
  list.clear();
}

void
vpMeLine::sample(const vpImage<unsigned char>& I)
{
  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  ;
}


void
vpMeLine::display(const vpImage<unsigned char>&I, vpColor col)
{
  vpMeLine::display(I,PExt[0],PExt[1],list,a,b,c,col);
}


void
vpMeLine::initTracking(const vpImage<unsigned char> &I)
{
  vpImagePoint ip1, ip2;

  std::cout << "Click on the line first point..." <<std::endl ;
  while (vpDisplay::getClick(I, ip1)!=true) ;
  std::cout << "Click on the line second point..." <<std::endl ;
  while (vpDisplay::getClick(I, ip2)!=true) ;

  try
  {
    initTracking(I, ip1, ip2) ;
  }
  catch(...)
  {
    vpERROR_TRACE("Error caught") ;
    throw ;
  }

}


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.setIdentity() ;
  vpMatrix DA, DAmemory ;
  vpColVector DAx ;
  vpColVector w(numberOfSignal()) ;
  vpColVector B(numberOfSignal()) ;
  w =1 ;
  vpMeSite p ;
  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 = *it;
      if (p.getState() == vpMeSite::NO_SUPPRESSION)
      {
        A[k][0] = p.ifloat ;
        A[k][1] = 1 ;
        B[k] = -p.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 = *it;
      if (p.getState() == vpMeSite::NO_SUPPRESSION)
      {
        if (w[k] < 0.2)
        {
          p.setState(vpMeSite::M_ESTIMATOR);
          
          *it = p;
        }
        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 = *it;
      if (p.getState() == vpMeSite::NO_SUPPRESSION)
      {
        A[k][0] = p.jfloat ;
        A[k][1] = 1 ;
        B[k] = -p.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 = *it;
      if (p.getState() == vpMeSite::NO_SUPPRESSION)
      {
        if (w[k] < 0.2)
        {
          p.setState(vpMeSite::M_ESTIMATOR);
          
          *it = p;
        }
        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) ;
}



void
vpMeLine::initTracking(const vpImage<unsigned char> &I,
                       const vpImagePoint &ip1,
                       const vpImagePoint &ip2)
{
  vpCDEBUG(1) <<" begin vpMeLine::initTracking()"<<std::endl ;

  int i1s, j1s, i2s, j2s;

  i1s = vpMath::round( ip1.get_i() );
  i2s = vpMath::round( ip2.get_i() );
  j1s = vpMath::round( ip1.get_j() );
  j2s = vpMath::round( ip2.get_j() );

  try{

    //  1. On fait ce qui concerne les droites (peut etre vide)
    {
      // Points extremites
      PExt[0].ifloat = (float)ip1.get_i() ;
      PExt[0].jfloat = (float)ip1.get_j() ;
      PExt[1].ifloat = (float)ip2.get_i() ;
      PExt[1].jfloat = (float)ip2.get_j() ;

      double angle = atan2((double)(i1s-i2s),(double)(j1s-j2s)) ;
      a = cos(angle) ;
      b = sin(angle) ;

      // Real values of a, b can have an other sign. So to get the good values
      // of a and b in order to initialise then c, we call track(I) just below

      computeDelta(delta,i1s,j1s,i2s,j2s) ;
      delta_1 = delta;

      //      vpTRACE("a: %f b: %f c: %f -b/a: %f delta: %f", a, b, c, -(b/a), delta);

      sample(I) ;

    }
    //  2. On appelle ce qui n'est pas specifique
    {
      vpMeTracker::initTracking(I) ;
    }
    // Call track(I) to give the good sign to a and b and to initialise c which can be used for the display
    track(I);
  }
  catch(...)
  {
    vpERROR_TRACE("Error caught") ;
    throw ;
  }
  vpCDEBUG(1) <<" end vpMeLine::initTracking()"<<std::endl ;
}


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

    if (s.getState() != vpMeSite::NO_SUPPRESSION)
      it = list.erase(it);
    else
      ++it;
  }
}


void
vpMeLine::setExtremities()
{
  double imin = +1e6 ;
  double jmin = +1e6;
  double imax = -1 ;
  double jmax = -1 ;


  // Loop through list of sites to track
  for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
    vpMeSite s = *it;//current reference pixel
    if (s.ifloat < imin)
    {
      imin = s.ifloat ;
      jmin = s.jfloat ;
    }

    if (s.ifloat > imax)
    {
      imax = s.ifloat ;
      jmax = s.jfloat ;
    }
  }

  PExt[0].ifloat = imin ;
  PExt[0].jfloat = jmin ;
  PExt[1].ifloat = imax ;
  PExt[1].jfloat = jmax ;

  if (fabs(imin-imax) < 25)
  {
    for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
      vpMeSite s = *it;//current reference pixel
      if (s.jfloat < jmin)
      {
        imin = s.ifloat ;
        jmin = s.jfloat ;
      }

      if (s.jfloat > jmax)
      {
        imax = s.ifloat ;
        jmax = s.jfloat ;
      }
    }
    PExt[0].ifloat = imin ;
    PExt[0].jfloat = jmin ;
    PExt[1].ifloat = imax ;
    PExt[1].jfloat = jmax ;
  }
}

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

  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 = (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 ; P.i = (int)P.ifloat ;
    P.jfloat = P.jfloat + dj*sample ; 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 ; P.i = (int)P.ifloat ;
    P.jfloat = P.jfloat - dj*sample ; 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  ;
}


void
vpMeLine::reSample(const vpImage<unsigned char> &I)
{

  double i1,j1,i2,j2 ;

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

void
vpMeLine::updateDelta()
{
  vpMeSite p ;

  double angle = delta + M_PI/2;
  double diff = 0;

  while (angle<0) angle += M_PI;
  while (angle>M_PI) angle -= M_PI;

  angle = vpMath::round(angle * 180 / M_PI) ;

  //if(fabs(angle) == 180 )
  if(std::fabs(std::fabs(angle) - 180) <= std::numeric_limits<double>::epsilon())
  {
    angle= 0 ;
  }

  //std::cout << "angle theta : " << theta << std::endl ;
  diff = fabs(angle - angle_1);
  if (diff > 90)
    sign *= -1;

  angle_1 = angle;

  for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
    p = *it;
    p.alpha = delta ;
    p.mask_sign = sign;
    *it = p;
  }
  delta_1 = delta;
}


void
vpMeLine::track(const vpImage<unsigned char> &I)
{
  vpCDEBUG(1) <<"begin vpMeLine::track()"<<std::endl ;

  //  1. On fait ce qui concerne les droites (peut etre vide)
  {
  }
  //  2. On appelle ce qui n'est pas specifique
  {
    vpMeTracker::track(I) ;
  }

  // 3. On revient aux droites
  {
    // supression des points rejetes par les ME
    suppressPoints() ;
    setExtremities() ;


    // Estimation des parametres de la droite aux moindres carre
    try
    {
      leastSquare() ;
    }
    catch(...)
    {
      vpERROR_TRACE("Error caught") ;
      throw ;
    }


    // recherche de point aux extremite de la droites
    // dans le cas d'un glissement
    seekExtremities(I) ;

    setExtremities() ;
    try
    {
      leastSquare() ;
    }
    catch(...)
    {
      vpERROR_TRACE("Error caught") ;
      throw ;
    }

    // suppression des points rejetes par la regression robuste
    suppressPoints() ;
    setExtremities() ;

    //reechantillonage si necessaire
    reSample(I) ;

    // remet a jour l'angle delta pour chaque  point de la liste

    updateDelta() ;

    // Remise a jour de delta dans la liste de site me
    if (vpDEBUG_ENABLE(2))
    {
      display(I,vpColor::red) ;
      vpMeTracker::display(I) ;
      vpDisplay::flush(I) ;
    }


  }

  computeRhoTheta(I) ;

  vpCDEBUG(1) <<"end vpMeLine::track()"<<std::endl ;
}

void vpMeLine::update_indices(double theta,int i,int j,int incr,int& i1,int& i2,int& j1,int& j2){
  i1 = (int)(i + cos(theta) *incr) ;
  j1 = (int)(j + sin(theta) *incr) ;

  i2 = (int)(i - cos(theta) *incr) ;
  j2 = (int)(j - sin(theta) *incr) ;
}

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

}

double
vpMeLine::getRho() const
{
  return  rho ;
}

double
vpMeLine::getTheta() const
{
  return theta ;
}

void
vpMeLine::getExtremities(vpImagePoint &ip1, vpImagePoint &ip2)
{
  /*Return the coordinates of the extremities of the line*/
  ip1.set_i( PExt[0].ifloat );
  ip1.set_j( PExt[0].jfloat );
  ip2.set_i( PExt[1].ifloat );
  ip2.set_j( PExt[1].jfloat );
}


bool
vpMeLine::intersection(const vpMeLine &line1, const vpMeLine &line2, 
                       vpImagePoint &ip)
{
  double denom = 0;
  double a1 = line1.a;
  double b1 = line1.b;
  double c1 = line1.c;
  double a2 = line2.a;
  double b2 = line2.b;
  double c2 = line2.c;
  double i=0, j=0;

  try{

    if (a1 > 0.1)
    {
      denom = (-(a2/a1) * b1 + b2);

      //if (denom == 0)
      if (std::fabs(denom) <= std::numeric_limits<double>::epsilon())
      {
        std::cout << "!!!!!!!!!!!!! Problem : Lines are parallel !!!!!!!!!!!!!" << std::endl;
        return (false);
      }

      //if (denom != 0 )
      if (std::fabs(denom) > std::numeric_limits<double>::epsilon())
      {
        j = ( (a2/a1)*c1 - c2 ) / denom;
        i = (-b1*j - c1) / a1;
      }
    }

    else
    {
      denom = (-(b2/b1) * a1 + a2);

      //if (denom == 0)
      if (std::fabs(denom) <= std::numeric_limits<double>::epsilon())
      {
        std::cout << "!!!!!!!!!!!!! Problem : Lines are parallel !!!!!!!!!!!!!" << std::endl;
        return (false);
      }

      //if (denom != 0 )
      if (std::fabs(denom) > std::numeric_limits<double>::epsilon())
      {
        i = ( (b2/b1)*c1 - c2 ) / denom;
        j = (-a1*i - c1) / b1;
      }
    }
    ip.set_i( i );
    ip.set_j( j );
    
    return (true);
  }
  catch(...)
  {
    return (false);
  }
}

void vpMeLine::display(const vpImage<unsigned char>& I,const vpMeSite &PExt1, const vpMeSite &PExt2,
            const double &A, const double &B, const double &C,
            vpColor color,  unsigned int thickness)
{
  vpImagePoint ip1, ip2;

  if (fabs(A) < fabs(B)) {
    double i1, j1, i2, j2;
    i1 = 0;
    j1 = (-A*i1 -C) / B;
    i2 = I.getHeight() - 1.0;
    j2 = (-A*i2 -C) / B;

    ip1.set_i( i1 );
    ip1.set_j( j1 );
    ip2.set_i( i2 );
    ip2.set_j( j2 );
    vpDisplay::displayLine(I, ip1, ip2, color);
    //vpDisplay::flush(I);

  }
  else {
    double i1, j1, i2, j2;
    j1 = 0;
    i1 = -(B * j1 + C) / A;
    j2 = I.getWidth() - 1.0;
    i2 = -(B * j2 + C) / A;

    ip1.set_i( i1 );
    ip1.set_j( j1 );
    ip2.set_i( i2 );
    ip2.set_j( j2 );
    vpDisplay::displayLine(I, ip1, ip2, color);
    //vpDisplay::flush(I);
  }

  ip1.set_i( PExt1.ifloat );
  ip1.set_j( PExt1.jfloat );
  vpDisplay::displayCross(I, ip1, 10, vpColor::green,thickness);

  ip1.set_i( PExt2.ifloat );
  ip1.set_j( PExt2.jfloat );
  vpDisplay::displayCross(I, ip1, 10, vpColor::green,thickness);
}

void vpMeLine::display(const vpImage<unsigned char>& I,const vpMeSite &PExt1, const vpMeSite &PExt2,
            const std::list<vpMeSite> &site_list,
            const double &A, const double &B, const double &C,
            vpColor color,  unsigned int thickness)
{
  vpImagePoint ip;
  
  for(std::list<vpMeSite>::const_iterator it=site_list.begin(); it!=site_list.end(); ++it){
    vpMeSite pix = *it;
    ip.set_i( pix.ifloat );
    ip.set_j( pix.jfloat );


    if (pix.getState() == vpMeSite::M_ESTIMATOR)
      vpDisplay::displayCross(I, ip, 5, vpColor::green,thickness);
    else
      vpDisplay::displayCross(I, ip, 5, color,thickness);

    //vpDisplay::flush(I);
  }
  
  vpImagePoint ip1, ip2;

  if (fabs(A) < fabs(B)) {
    double i1, j1, i2, j2;
    i1 = 0;
    j1 = (-A*i1 -C) / B;
    i2 = I.getHeight() - 1.0;
    j2 = (-A*i2 -C) / B;

    ip1.set_i( i1 );
    ip1.set_j( j1 );
    ip2.set_i( i2 );
    ip2.set_j( j2 );
    vpDisplay::displayLine(I, ip1, ip2, color);
    //vpDisplay::flush(I);

  }
  else {
    double i1, j1, i2, j2;
    j1 = 0;
    i1 = -(B * j1 + C) / A;
    j2 = I.getWidth() - 1.0;
    i2 = -(B * j2 + C) / A;

    ip1.set_i( i1 );
    ip1.set_j( j1 );
    ip2.set_i( i2 );
    ip2.set_j( j2 );
    vpDisplay::displayLine(I, ip1, ip2, color);
    //vpDisplay::flush(I);
  }

  ip1.set_i( PExt1.ifloat );
  ip1.set_j( PExt1.jfloat );
  vpDisplay::displayCross(I, ip1, 10, vpColor::green,thickness);

  ip1.set_i( PExt2.ifloat );
  ip1.set_j( PExt2.jfloat );
  vpDisplay::displayCross(I, ip1, 10, vpColor::green,thickness);
}