vpMeEllipse.cpp

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2019 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 as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * 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://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * 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 <visp3/me/vpMeEllipse.h>

#include <visp3/core/vpDebug.h>
#include <visp3/core/vpException.h>
#include <visp3/core/vpImagePoint.h>
#include <visp3/core/vpRobust.h>
#include <visp3/me/vpMe.h>

#include <cmath>  // std::fabs
#include <limits> // numeric_limits
#include <vector>

vpMeEllipse::vpMeEllipse()
  : K(), iPc(), a(0.), b(0.), e(0.),
    iP1(), iP2(), alpha1(0), alpha2(2 * M_PI),
    ce(0.), se(0.), angle(), m00(0.),
    #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
    mu11(0.), mu20(0.), mu02(0.),
    m10(0.), m01(0.),
    m11(0.), m02(0.), m20(0.),
    #endif
    thresholdWeight(0.2),
    #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
    expecteddensity(0.),
    #endif
    m_alphamin(0.), m_alphamax(0.), m_uc(0.), m_vc(0.), m_n20(0.), m_n11(0.), m_n02(0.),
    m_expectedDensity(0), m_numberOfGoodPoints(0), m_trackArc(false), m_arcEpsilon(1e-6)
{
  // Resize internal parameters vector
  // K0 u^2 + K1 v^2 + 2 K2 u v + 2 K3 u + 2 K4 v + K5 =  0
  K.resize(6);
  iP1.set_ij(0,0);
  iP2.set_ij(0,0);
}

vpMeEllipse::vpMeEllipse(const vpMeEllipse &me_ellipse)
  : vpMeTracker(me_ellipse),
    K(me_ellipse.K), iPc(me_ellipse.iPc), a(me_ellipse.a), b(me_ellipse.b), e(me_ellipse.e),
    iP1(me_ellipse.iP1), iP2(me_ellipse.iP2), alpha1(me_ellipse.alpha1), alpha2(me_ellipse.alpha2),
    ce(me_ellipse.ce), se(me_ellipse.se), angle(me_ellipse.angle), m00(me_ellipse.m00),
    #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
    mu11(me_ellipse.mu11), mu20(me_ellipse.mu20), mu02(me_ellipse.mu02),
    m10(me_ellipse.m10), m01(me_ellipse.m01), m11(me_ellipse.m11),
    m02(me_ellipse.m02), m20(me_ellipse.m20),
    #endif
    thresholdWeight(me_ellipse.thresholdWeight),
    #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
    expecteddensity(me_ellipse.expecteddensity),
    #endif
    m_alphamin(me_ellipse.m_alphamin), m_alphamax(me_ellipse.m_alphamax),
    m_uc(me_ellipse.m_uc), m_vc(me_ellipse.m_vc),
    m_n20(me_ellipse.m_n20), m_n11(me_ellipse.m_n11), m_n02(me_ellipse.m_n02),
    m_expectedDensity(me_ellipse.m_expectedDensity), m_numberOfGoodPoints(me_ellipse.m_numberOfGoodPoints), m_trackArc(me_ellipse.m_trackArc)
{
}

vpMeEllipse::~vpMeEllipse()
{
  list.clear();
  angle.clear();
}

double vpMeEllipse::computeTheta(const vpImagePoint &iP) const
{
  double u = iP.get_u();
  double v = iP.get_v();

  return (computeTheta(u, v));
}

double vpMeEllipse::computeTheta(double u, double v) const
{
  double A = K[0] * u + K[2] * v + K[3];
  double B = K[1] * v + K[2] * u + K[4];

  double theta = atan2(B, A); // Angle between the tangent and the u axis.
  if (theta < 0) {  // theta in [0;Pi]  // FC : pourquoi ? pour me sans doute
    theta += M_PI;
  }
  return (theta);
}

void vpMeEllipse::updateTheta()
{
  vpMeSite p_me;
  vpImagePoint iP;
  for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
    p_me = *it;
    // (i,j) frame used for vpMESite
    iP.set_ij(p_me.ifloat, p_me.jfloat);
    p_me.alpha = computeTheta(iP);
    *it = p_me;
  }
}

void vpMeEllipse::computePointOnEllipse(const double angle, vpImagePoint &iP)
{
  // Two versions are available. If you change from one version to the other
  // one, do not forget to change also the reciprocical function
  // computeAngleOnEllipse() just below and, for a correct display of an arc
  // of ellipse, adapt vpMeEllipse::display() below and
  // vp_display_display_ellipse() in modules/core/src/display/vpDisplay_impl.h
  // so that the two extremities of the arc are correctly shown.

  // Version that gives a regular angular sampling on the ellipse, so less
  // points at its extremities
  /*
  double co = cos(angle);
  double si = sin(angle);
  double coef = a * b / sqrt(b * b * co * co + a * a * si * si);
  double u = co * coef;
  double v = si * coef;
  iP.set_u(uc + ce * u - se * v);
  iP.set_v(vc + se * u + ce * v);
  */

  // Version from "the two concentric circles" method that gives more points
  // at the ellipse extremities for a regular angle sampling. It is better to
  // display an ellipse, not necessarily to track it

  // (u,v) are the coordinates on the canonical centered ellipse;
  double u = a * cos(angle);
  double v = b * sin(angle);
  // a rotation of e and a translation by (uc,vc) are done
  // to get the coordinates of the point on the shifted ellipse
  iP.set_uv(m_uc + ce * u - se * v, m_vc + se * u + ce * v);
}

double vpMeEllipse::computeAngleOnEllipse(const vpImagePoint &pt) const
{
  // Two versions are available. If you change from one version to the other
  // one, do not forget to change also the reciprocical function
  // computePointOnEllipse() just above. Adapt also the display; see comment
  // at the beginning of computePointOnEllipse()

  // Regular angle smapling method
  /*
  double du = pt.get_u() - uc;
  double dv = pt.get_v() - vc;
  double ang = atan2(dv,du) - e;
  if (ang > M_PI) {
    ang -= 2.0 * M_PI;
  }
  else if (ang < -M_PI) {
    ang += 2.0 * M_PI;
  }
  */
  // for the "two concentric circles method" starting from the previous one
  // (just to remember the link between both methods:
  // tan(theta_2cc) = a/b tan(theta_rs))
  /*
  double co = cos(ang);
  double si = sin(ang);
  double coeff = 1.0/sqrt(b*b*co*co+a*a*si*si);
  si *= a*coeff;
  co *= b*coeff;
  ang = atan2(si,co);
  */
  // For the "two concentric circles" method starting from scratch
  double du = pt.get_u() - m_uc;
  double dv = pt.get_v() - m_vc;
  double co = (du * ce + dv * se)/a;
  double si = (- du * se + dv * ce)/b;
  double angle = atan2(si,co);

  return(angle);
}

void vpMeEllipse::computeAbeFromNij()
{
  double num = m_n20 - m_n02;
  double d = num * num + 4.0 * m_n11 * m_n11;   // always >= 0
  if (d <= std::numeric_limits<double>::epsilon()) {
    e = 0.0;  // case n20 = n02 and n11 = 0 : circle, e undefined
    ce = 1.0;
    se = 0.0;
    a = b = 2.0*sqrt(m_n20);   // = sqrt(2.0*(n20+n02))
  }
  else { // real ellipse
    e = atan2(2.0*m_n11, num)/2.0;  // e in [-Pi/2 ; Pi/2]
    ce = cos(e);
    se = sin(e);

    d = sqrt(d); // d in sqrt always >= 0
    num = m_n20 + m_n02;
    a = sqrt(2.0*(num + d)); // term in sqrt always > 0
    b = sqrt(2.0*(num - d)); // term in sqrt always > 0
  }
}

void vpMeEllipse::computeKiFromNij()
{
  K[0] = m_n02;
  K[1] = m_n20;
  K[2] = -m_n11;
  K[3] = m_n11 * m_vc - m_n02 * m_uc;
  K[4] = m_n11 * m_uc - m_n20 * m_vc;
  K[5] = m_n02 * m_uc * m_uc + m_n20 * m_vc * m_vc - 2.0 * m_n11 * m_uc * m_vc
      + 4.0 * (m_n11 * m_n11 - m_n20 * m_n02);
}

void vpMeEllipse::computeNijFromAbe()
{
  m_n20 = 0.25 * (a * a * ce * ce + b * b * se * se);
  m_n11 = 0.25 * se * ce * (a * a - b * b);
  m_n02 = 0.25 * (a * a * se * se + b * b * ce * ce);
}

void vpMeEllipse::getParameters()
{
  // Equations below from Chaumette PhD and TRO 2004 paper
  double num = K[0] * K[1] - K[2] * K[2]; // > 0 for an ellipse
  if (num <= 0) {
    throw(vpException(vpException::fatalError, "The points do not belong to an ellipse!"));
  }

  m_uc = (K[2] * K[4] - K[1] * K[3]) / num;
  m_vc = (K[2] * K[3] - K[0] * K[4]) / num;
  iPc.set_uv(m_uc, m_vc);

  double d = (K[0] * m_uc * m_uc + K[1] * m_vc * m_vc + 2.0 * K[2] * m_uc * m_vc - K[5])
      / (4.0 * num);
  m_n20 =  K[1] * d;  // always > 0
  m_n11 = -K[2] * d;
  m_n02 =  K[0] * d;  // always > 0

  computeAbeFromNij();

  // normalization so that K0 = n02, K1 = n20, etc (Eq (25) of TRO paper)
  d = m_n02/K[0];  // fabs(K[0]) > 0
  for (unsigned int i=0; i < 6; i++) {
    K[i] *= d;
  }
  if (vpDEBUG_ENABLE(3)) {
    printParameters();
  }
}

void vpMeEllipse::printParameters() const
{
  std::cout << "K :" << K.t() << std::endl;
  std::cout << "xc = " << m_uc << ", yc = " << m_vc  << std::endl;
  std::cout << "n20 = " << m_n20 << ", n11 = " << m_n11 << ", n02 = " << m_n02 <<std::endl;
  std::cout << "A = " << a << ", B = " << b << ", E (dg) " << vpMath::deg(e) <<std::endl;
}

void vpMeEllipse::sample(const vpImage<unsigned char> &I, bool doNotTrack)
{
  // Warning: similar code in vpMbtMeEllipse::sample()
  if (!me) {
    throw(vpException(vpException::fatalError, "Moving edges on ellipse not initialized"));
  }
  // Delete old lists
  list.clear();
  angle.clear();

  int nbrows = static_cast<int>(I.getHeight());
  int nbcols = static_cast<int>(I.getWidth());

  if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
    std::cout << "In vpMeEllipse::sample: ";
    std::cout << "function called with sample step = 0, set to 10 dg";
    me->setSampleStep(10.0);
  }
  double incr = vpMath::rad(me->getSampleStep()); // angle increment
  // alpha2 - alpha1 = 2 * M_PI for a complete ellipse
  m_expectedDensity = static_cast<unsigned int>(floor((alpha2 - alpha1) / incr));
#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
  expecteddensity = static_cast<double>(m_expectedDensity);
#endif

  // starting angle for sampling
  double ang = alpha1 + ((alpha2 - alpha1) - static_cast<double>(m_expectedDensity) * incr)/2.0;
  // sample positions
  for (unsigned int i = 0; i < m_expectedDensity; i++) {
    vpImagePoint iP;
    computePointOnEllipse(ang,iP);
    // If point is in the image, add to the sample list
    // Check done in (i,j) frame)
    if (!outOfImage(vpMath::round(iP.get_i()), vpMath::round(iP.get_j()), 0, nbrows, nbcols)) {
      vpDisplay::displayCross(I, iP, 5, vpColor::red);

      double theta = computeTheta(iP);
      vpMeSite pix;
      // (i,j) frame used for vpMeSite
      pix.init(iP.get_i(), iP.get_j(), theta);
      pix.setDisplay(selectDisplay);
      pix.setState(vpMeSite::NO_SUPPRESSION);
      list.push_back(pix);
      angle.push_back(ang);
    }
    ang += incr;
  }
  if (!doNotTrack) {
    vpMeTracker::initTracking(I);
  }
}

unsigned int vpMeEllipse::plugHoles(const vpImage<unsigned char> &I)
{
  if (!me) {
    throw(vpException(vpException::fatalError, "Moving edges on ellipse tracking not initialized"));
  }
  unsigned int nb_pts_added = 0;
  int nbrows = static_cast<int>(I.getHeight());
  int nbcols = static_cast<int>(I.getWidth());

  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(me->getSampleStep());
  // Detect holes and try to complete them
  // FC : Currently only one point is looked at the middle of each hole
  // (to avoid multiple insertions that are time consuming).
  // A different choice could be done.
  std::list<double>::iterator angleList = angle.begin();
  double ang = *angleList;
  for (std::list<vpMeSite>::iterator meList = list.begin(); meList != list.end();) {
    ++angleList;
    ++meList;
    double nextang = *angleList;
    // The minimal size of a hole (1 point lost for sure).
    // could be increased to reduce time processing
    if ((nextang - ang) > 1.6 * incr) {
      ang = (nextang + ang) / 2.0; // mid angle
      vpImagePoint iP;
      computePointOnEllipse(ang,iP);
      if (!outOfImage(vpMath::round(iP.get_i()), vpMath::round(iP.get_j()), 0, nbrows, nbcols)) {
        double theta = computeTheta(iP);
        vpMeSite pix;
        pix.init(iP.get_i(), iP.get_j(), theta);
        pix.setDisplay(selectDisplay);
        pix.setState(vpMeSite::NO_SUPPRESSION);
        pix.track(I, me, false);
        if (pix.getState() == vpMeSite::NO_SUPPRESSION) { // good point
          nb_pts_added ++;
          iP.set_ij(pix.ifloat, pix.jfloat);
          double new_ang = computeAngleOnEllipse(iP);
          if ((new_ang - ang) > M_PI) {
            new_ang -= 2.0 * M_PI;
          }
          else if ((ang - new_ang) > M_PI) {
            new_ang += 2.0 * M_PI;
          }
          list.insert(meList,pix);
          angle.insert(angleList,new_ang);
          if (vpDEBUG_ENABLE(3)) {
            vpDisplay::displayCross(I, iP, 5, vpColor::blue);
          }
        }
      }
    }
    ang = nextang;
  }

  // Try to fill the first extremity: from alpha_min - incr to alpha1
  unsigned int nbpts = static_cast<unsigned int>(floor((m_alphamin-alpha1)/incr));
  ang = m_alphamin - incr;
  for (unsigned int i = 0; i < nbpts; i++) {
    vpImagePoint iP;
    computePointOnEllipse(ang ,iP);
    if (!outOfImage(vpMath::round(iP.get_i()), vpMath::round(iP.get_j()), 0, nbrows, nbcols)) {
      double theta = computeTheta(iP);
      vpMeSite pix;
      pix.init(iP.get_i(), iP.get_j(), theta);
      pix.setDisplay(selectDisplay);
      pix.setState(vpMeSite::NO_SUPPRESSION);
      pix.track(I, me, false);
      if (pix.getState() == vpMeSite::NO_SUPPRESSION) {
        nb_pts_added ++;
        iP.set_ij(pix.ifloat, pix.jfloat);
        double new_ang = computeAngleOnEllipse(iP);
        if ((new_ang - ang) > M_PI) {
          new_ang -= 2.0 * M_PI;
        }
        else if ((ang - new_ang) > M_PI) {
          new_ang += 2.0 * M_PI;
        }
        list.push_front(pix);
        angle.push_front(new_ang);
        if (vpDEBUG_ENABLE(3)) {
          vpDisplay::displayCross(I, iP, 5, vpColor::blue);
        }
      }
    }
    ang -= incr;
  }

  // Try to fill the second extremity: from alphamax + incr to alpha2
  nbpts = static_cast<unsigned int>(floor((alpha2-m_alphamax)/incr));
  ang = m_alphamax + incr;
  for (unsigned int i = 0; i < nbpts; i++) {
    vpImagePoint iP;
    computePointOnEllipse(ang,iP);
    if (!outOfImage(vpMath::round(iP.get_i()), vpMath::round(iP.get_j()), 0, nbrows, nbcols)) {
      double theta = computeTheta(iP);
      vpMeSite pix;
      pix.init(iP.get_i(), iP.get_j(), theta);
      pix.setDisplay(selectDisplay);
      pix.setState(vpMeSite::NO_SUPPRESSION);
      pix.track(I, me, false);
      if (pix.getState() == vpMeSite::NO_SUPPRESSION) {
        nb_pts_added ++;
        iP.set_ij(pix.ifloat, pix.jfloat);
        double new_ang = computeAngleOnEllipse(iP);
        if ((new_ang - ang) > M_PI) {
          new_ang -= 2.0 * M_PI;
        }
        else if ((ang - new_ang) > M_PI) {
          new_ang += 2.0 * M_PI;
        }
        list.push_back(pix);
        angle.push_back(new_ang);
        if (vpDEBUG_ENABLE(3)) {
          vpDisplay::displayCross(I, iP, 5, vpColor::blue);
        }
      }
    }
    ang += incr;
  }
  me->setRange(memory_range);
  me->setMu1(memory_mu1);
  me->setMu2(memory_mu2);

  if (vpDEBUG_ENABLE(3)) {
    printf("In plugHoles(): nb of added points : %d\n", nb_pts_added);
  }
  return nb_pts_added;
}

void vpMeEllipse::leastSquare(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &iP)
{
  // Homogeneous system A x = 0  ; x is the nullspace of A
  // K0 u^2 + K1 v^2 + 2 K2 u v + 2 K3 u + 2 K4 v + K5 = 0
  // A = (u^2 v^2 2uv 2u 2v 1), x = (K0 K1 K2 K3 K4 K5)^T

  // It would be a bad idea to solve the same system using A x = b where
  // A = (u^2 v^2 2uv 2u 2v), b = (-1), x = (K0 K1 K2 K3 K4)^T since it
  // cannot consider the case where the origin belongs to the ellipse.
  // Another possibility would be to consider K0+K1=1 which is always valid,
  // leading to the system A x = b where
  // A = (u^2-v^2 2uv 2u 2v 1), b = (-v^2), x = (K0 K2 K3 K4 K5)^T

  double um = I.getWidth() / 2.;
  double vm = I.getHeight() / 2.;
  unsigned int n = static_cast<unsigned int>(iP.size());

  vpMatrix A(n, 6);

  for (unsigned int k = 0; k < n; k++) {
    // normalization so that (u,v) in [-1;1]
    double u = (iP[k].get_u() - um) / um;
    double v = (iP[k].get_v() - vm) / vm;
    A[k][0] = u * u;
    A[k][1] = v * v;
    A[k][2] = 2.0 * u * v;
    A[k][3] = 2.0 * u;
    A[k][4] = 2.0 * v;
    A[k][5] = 1.0;
  }
  vpMatrix KerA;
  unsigned int dim = A.nullSpace(KerA, 1);
  if (dim > 1) { // case with less than 5 independent points
    // FC : should create a rankError exception
    throw(vpException(vpException::fatalError, "Linear system for computing the ellipse equation ill conditionned"));
  }
  // the term um*vm is for counterbalancing the bad conditioning of the
  // inverse normalization below
  for (unsigned int i = 0; i < 6 ; i++) K[i] = um * vm * KerA[i][0];

  // Inverse normalization to go back to pixel values
  K[0] /= um * um;
  K[1] /= vm * vm;
  K[2] /= um * vm;
  K[3] = K[3]/um - K[0] * um - K[2] * vm;
  K[4] = K[4]/vm - K[1] * vm - K[2] * um;
  K[5] = K[5] - K[0] * um * um - K[1] * vm * vm - 2.0 * K[2] * um * vm - 2.0 * K[3] * um - 2.0 * K[4] * vm;

  getParameters();
}

void vpMeEllipse::leastSquareRobust(const vpImage<unsigned char> &I)
{
  // Homogeneous system A x = 0  ; x is the nullspace of A
  // K0 u^2 + K1 v^2 + 2 K2 u v + 2 K3 u + 2 K4 v + K5 = 0
  // A = (u^2 v^2 2uv 2u 2v 1), x = (K0 K1 K2 K3 K4 K5)^T

  // It would be a bad idea to solve the same system using A x = b where
  // A = (u^2 v^2 2uv 2u 2v), b = (-1), x = (K0 K1 K2 K3 K4)^T since it
  // cannot consider the case where the origin belongs to the ellipse.
  // Another possibility would be to consider K0+K1=1 which is always valid,
  // leading to the system A x = b where
  // A = (u^2-v^2 2uv 2u 2v 1), b = (-v^2), x = (K0 K2 K3 K4 K5)^T

  const unsigned int nos = numberOfSignal();

  // Note that the (nos-k) last rows of A, xp and yp are not used.
  // Hopefully, this is not an issue.

  vpMatrix A(nos, 6);
  // Useful to compute the weights in the robust estimation
  vpColVector xp(nos), yp(nos);

  unsigned int k = 0;
  double um = I.getWidth() / 2.;
  double vm = I.getHeight() / 2.;
  for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
    vpMeSite p_me = *it;
    if (p_me.getState() == vpMeSite::NO_SUPPRESSION) {
      // from (i,j) to (u,v) frame + normalization so that (u,v) in [-1;1]
      double u = (p_me.jfloat  - um) / um;
      double v = (p_me.ifloat  - vm) / vm;
      A[k][0] = u * u;
      A[k][1] = v * v;
      A[k][2] = 2.0 * u * v;
      A[k][3] = 2.0 * u;
      A[k][4] = 2.0 * v;
      A[k][5] = 1.0;
      // Useful to compute the weights in the robust estimation
      xp[k] = p_me.jfloat;
      yp[k] = p_me.ifloat;

      k++;
    }
  }
  if (k < 5) {
    throw(vpException(vpException::dimensionError, "Not enough moving edges to track the ellipse"));
  }

  vpRobust r;
  // r.setThreshold(0.02);  // Old version where this threshold was highly
  // sensitive since the residues do not represent the Euclidean distance
  // from the point to the ellipse
  r.setMinMedianAbsoluteDeviation(1.0);  // image noise in pixels for the geometrical distance
  vpColVector w(k);
  w = 1.0;
  unsigned int iter = 0;
  double var = 1.0;
  vpColVector Kprev(6);
  vpMatrix DA(k, 6);
  vpMatrix KerDA;

  // stop after 4 it or if variation of K between 2 iterations is more than 0.1 %
  while ((iter < 4) && (var > 0.001)) {
    for (unsigned int i = 0; i < k ; i++) {
      for (unsigned int j = 0; j < 6 ; j++) {
        DA[i][j] = w[i] * A[i][j];
      }
    }
    unsigned int dim = DA.nullSpace(KerDA, 1);
    if (dim > 1) { // case with less than 5 independent points
      // FC : should create a rankError exception
      throw(vpException(vpException::fatalError, "Linear system for computing the ellipse equation ill conditionned"));
    }

    for (unsigned int i=0; i<6 ; i++) K[i] = KerDA[i][0]; // norm(K) = 1
    var = (K-Kprev).frobeniusNorm();
    Kprev = K;
    // the term um*vm is for counterbalancing the bad conditioning of the
    // inverse normalization just below
    K *= (um * vm);
    // vpColVector residu(k);  // old version for considering the algebraic distance
    // residu = A * K;
    // Better version considering the geometric distance
    // Inverse normalization to go back to pixels
    K[0] /= um * um;
    K[1] /= vm * vm;
    K[2] /= um * vm;
    K[3] = K[3]/um - K[0] * um - K[2] * vm;
    K[4] = K[4]/vm - K[1] * vm - K[2] * um;
    K[5] = K[5] - K[0] * um * um - K[1] * vm * vm - 2.0 * K[2] * um * vm - 2.0 * K[3] * um - 2.0 * K[4] * vm;
    getParameters();  // since a, b, and e are used just after

    vpColVector residu(k);
    for (unsigned int i=0; i < k ; i++) {
      vpImagePoint ip1, ip2;
      ip1.set_uv(xp[i],yp[i]);
      double ang = computeAngleOnEllipse(ip1);
      computePointOnEllipse(ang, ip2);
      // residu = 0 if point is exactly on the ellipse, not otherwise
      residu[i] = vpImagePoint::distance(ip1, ip2);
    }
    // end of new version
    r.MEstimator(vpRobust::TUKEY, residu, w);
    iter++;
  }
  /*  FC : for old version with algebraic distance
  // Inverse normalization to go back to pixels
  K[0] /= um * um;
  K[1] /= vm * vm;
  K[2] /= um * vm;
  K[3] = K[3]/um - K[0] * um - K[2] * vm;
  K[4] = K[4]/vm - K[1] * vm - K[2] * um;
  K[5] = K[5] - K[0] * um * um - K[1] * vm * vm - 2.0 * K[2] * um * vm - 2.0 * K[3] * um - 2.0 * K[4] * vm;
  getParameters();
  */
  double previous_ang = - 4.0 * M_PI;
  double incr = vpMath::rad(me->getSampleStep());
  // Remove bad points, too near points, and outliers from the lists
  k = m_numberOfGoodPoints = 0;
  std::list<double>::iterator angleList = angle.begin();
  for (std::list<vpMeSite>::iterator meList = list.begin(); meList != list.end();) {
    vpMeSite p_me = *meList;
    if (p_me.getState() == vpMeSite::NO_SUPPRESSION) {
      if (w[k] > thresholdWeight) { // inlier
        // Management of the angle to keep only the points in the interval
        // [alpha1 ; alpha2]
        double ang = *angleList;
        vpImagePoint iP;
        iP.set_ij(p_me.ifloat,p_me.jfloat);
        double new_ang = computeAngleOnEllipse(iP);
        if ((new_ang - ang) > M_PI) {
          new_ang -= 2.0 * M_PI;
        }
        else if ((ang - new_ang) > M_PI) {
          new_ang += 2.0 * M_PI;
        }
        if ((new_ang >= alpha1) && (new_ang <= alpha2) ) {
          // good point if not too near from the previous one in the list
          // if so,  udate of its angle
          if ((new_ang - previous_ang) >= (0.6 * incr)) {
            *angleList = previous_ang = new_ang;
            m_numberOfGoodPoints++;
            ++meList;
            ++angleList;
            if (vpDEBUG_ENABLE(3)) {
              vpDisplay::displayCross(I, iP, 10, vpColor::red, 1);
              printf("In LQR: angle : %lf, i = %.0lf, j = %.0lf\n",
                     vpMath::deg(new_ang), iP.get_i(), iP.get_j());
            }
          }
          else {
            if (vpDEBUG_ENABLE(3)) {
              vpDisplay::displayCross(I, iP, 10, vpColor::orange, 1);
              printf("too near : angle  %lf, i %.0f , j : %0.f\n",
                     vpMath::deg(new_ang), p_me.ifloat, p_me.jfloat);
            }
            meList = list.erase(meList);
            angleList = angle.erase(angleList);
          }
        }
        else { // point not in the interval [alpha1 ; alpha2]
          if (vpDEBUG_ENABLE(3)) {
            vpDisplay::displayCross(I, iP, 10, vpColor::green, 1);
            printf("not in interval: angle : %lf, i %.0f , j : %0.f\n",
                   vpMath::deg(new_ang), p_me.ifloat, p_me.jfloat);
          }
          meList = list.erase(meList);
          angleList = angle.erase(angleList);
        }
      }
      else { // outlier
        if (vpDEBUG_ENABLE(3)) {
          vpImagePoint iP;
          iP.set_ij(p_me.ifloat,p_me.jfloat);
          printf("point %d outlier i : %.0f , j : %0.f, poids : %lf\n",
                 k, p_me.ifloat, p_me.jfloat, w[k]);
          vpDisplay::displayCross(I, iP, 10, vpColor::cyan, 1);
        }
        meList = list.erase(meList);
        angleList = angle.erase(angleList);
      }
      k++;
    }
    else {  // points not selected as me
      meList = list.erase(meList);
      angleList = angle.erase(angleList);
      if (vpDEBUG_ENABLE(3)) {
        vpImagePoint iP;
        iP.set_ij(p_me.ifloat,p_me.jfloat);
        printf("point not me i : %.0f , j : %0.f\n", p_me.ifloat, p_me.jfloat);
        vpDisplay::displayCross(I, iP, 10, vpColor::blue, 1);
      }
    }
  }
  // set extremities of the angle list
  m_alphamin = angle.front();
  m_alphamax = angle.back();

  if (vpDEBUG_ENABLE(3)) {
    printf("alphamin : %lf, alphamax : %lf\n", vpMath::deg(m_alphamin), vpMath::deg(m_alphamax));
    printf("dans leastSquareRobust : nb pts ok  = %d \n", m_numberOfGoodPoints);
  }
}

void vpMeEllipse::display(const vpImage<unsigned char> &I, vpColor col)
{
  vpMeEllipse::display(I, iPc, a, b, e, alpha1, alpha2, col);
}

void vpMeEllipse::initTracking(const vpImage<unsigned char> &I, bool trackArc)
{
  const unsigned int n = 5;
  std::vector<vpImagePoint> iP(n);
  m_trackArc = trackArc;

  if (m_trackArc) {
    std::cout << "First and fifth points specify the extremities of the arc of ellipse (clockwise)" << std::endl;
  }
  for (unsigned int k = 0; k < n; k++) {
    std::cout << "Click point " << k + 1 << "/" << n << " on the ellipse " << std::endl;
    vpDisplay::getClick(I, iP[k], true);
    vpDisplay::displayCross(I, iP[k], 10, vpColor::red);
    vpDisplay::flush(I);
    std::cout << iP[k] << std::endl;
  }
  initTracking(I, iP, trackArc);
}

void vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &iP, bool trackArc)
{
  m_trackArc = trackArc;
  // useful for sample(I):
  leastSquare(I, iP);
  if (trackArc) {
    // useful for track(I):
    iP1 = iP.front();
    iP2 = iP.back();
    // useful for sample(I):
    alpha1 = computeAngleOnEllipse(iP1);
    alpha2 = computeAngleOnEllipse(iP2);
    if ((alpha2 <= alpha1) || (std::fabs(alpha2 - alpha1) < m_arcEpsilon)) {
      alpha2 += 2.0 * M_PI;
    }
  }
  else {
    alpha1 = 0.0;
    alpha2 = 2 * M_PI;
    // useful for track(I):
    vpImagePoint ip;
    computePointOnEllipse(alpha1, ip);
    iP1 = iP2 = ip;
  }

  sample(I);
  track(I);
  vpMeTracker::display(I);
  vpDisplay::flush(I);
}

void vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const vpColVector &param, vpImagePoint *pt1, const vpImagePoint *pt2)
{
  if (pt1 != NULL && pt2 != NULL) {
    m_trackArc = true;
  }
  // useful for sample(I) : uc, vc, a, b, e, Ki, alpha1, alpha2
  m_uc = param[0];
  m_vc = param[1];
  m_n20 = param[2];
  m_n11 = param[3];
  m_n02 = param[4];
  computeAbeFromNij();
  computeKiFromNij();

  if (m_trackArc) {
    alpha1 = computeAngleOnEllipse(*pt1);
    alpha2 = computeAngleOnEllipse(*pt2);
    if ((alpha2 <= alpha1) || (std::fabs(alpha2 - alpha1) < m_arcEpsilon)) {
      alpha2 += 2.0 * M_PI;
    }
    // useful for track(I)
    iP1 = *pt1;
    iP2 = *pt2;
  }
  else {
    alpha1 = 0.0;
    alpha2 = 2.0 * M_PI;
    // useful for track(I)
    vpImagePoint ip;
    computePointOnEllipse(alpha1, ip);
    iP1 = iP2 = ip;
  }
  // useful for display(I) so useless if no display before track(I)
  iPc.set_uv(m_uc, m_vc);

  sample(I);
  track(I);
  vpMeTracker::display(I);
  vpDisplay::flush(I);
}

void vpMeEllipse::track(const vpImage<unsigned char> &I)
{
  vpMeTracker::track(I);

  // recompute alpha1 and alpha2 in case they have been changed by setEndPoints()
  if (m_trackArc) {
    alpha1 = computeAngleOnEllipse(iP1);
    alpha2 = computeAngleOnEllipse(iP2);
    if ((alpha2 <= alpha1) || (std::fabs(alpha2 - alpha1) < m_arcEpsilon)) {
      alpha2 += 2.0 * M_PI;
    }
  }
  // Compute the ellipse parameters from the tracked points and manage the lists
  leastSquareRobust(I);
  if (vpDEBUG_ENABLE(3)) {
    printf("nb of Good points %u, density %d, alphamin %lf, alphamax %lf\n",
           m_numberOfGoodPoints, m_expectedDensity,
           vpMath::deg(m_alphamin), vpMath::deg(m_alphamax));
  }

  // Try adding points at the extremities and in the holes if needed
  if (m_numberOfGoodPoints < m_expectedDensity) { // at least one point has been lost
    if (plugHoles(I) > 0) {
      leastSquareRobust(I);  // if new points have been added, recompute the ellipse parameters and manage again the lists
    }
  }
  if (vpDEBUG_ENABLE(3)) {
    printf("nb of Good points %u, density %d, alphamin %lf, alphamax %lf\n",
           m_numberOfGoodPoints, m_expectedDensity,
           vpMath::deg(m_alphamin), vpMath::deg(m_alphamax));
  }

  // resample if needed in case of unsufficient number of points or
  // bad repartition
  // FC : (thresholds ad hoc and sensitive...)
  if ( ((3 * m_numberOfGoodPoints) < m_expectedDensity) || (m_numberOfGoodPoints <= 5)  || ( (m_alphamax - m_alphamin) < vpMath::rad(120.0) ) ) {
    if (vpDEBUG_ENABLE(3)) {
      printf("Before RESAMPLE !!! nb points %d \n", m_numberOfGoodPoints);
      printf("A click to continue \n");
      vpDisplay::flush(I);
      vpDisplay::getClick(I);
      vpDisplay::display(I);
    }

    sample(I);
    vpMeTracker::track(I);
    leastSquareRobust(I);
    if (vpDEBUG_ENABLE(3)) {
      printf("nb of Good points %u, density %d, alphamax %lf, alphamin %lf\n",
             m_numberOfGoodPoints, m_expectedDensity, vpMath::deg(m_alphamax), vpMath::deg(m_alphamin));
    }

    // Stop in case of failure after resample
    if ( ((3 * m_numberOfGoodPoints) < m_expectedDensity) || (m_numberOfGoodPoints <= 5)  || ( (m_alphamax - m_alphamin) < vpMath::rad(120.0) ) ) {
      // FC : dimensionError pas vraiment le bon terme...
      throw(vpException(vpException::dimensionError, "Impossible to track the ellipse"));
    }
  }

  if (vpDEBUG_ENABLE(3)) {
    printParameters();
  }
  // remet a jour l'angle delta pour chaque vpMeSite de la liste
  updateTheta();
  // not in getParameters since computed only once for each image
  m00 = M_PI * a * b;

  // Useful only for tracking an arc of ellipse, but done to give them a value
  computePointOnEllipse(alpha1, iP1);
  computePointOnEllipse(alpha2, iP2);

#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
  computeMoments();
#endif

  if (vpDEBUG_ENABLE(3)) {
    display(I, vpColor::red);
    vpMeTracker::display(I);
    vpDisplay::flush(I);
  }
}


#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS

void vpMeEllipse::computeMoments()
{
  // m00 = M_PI * a * b;  // moved in track(I)

  m10 = m_uc * m00;
  m01 = m_vc * m00;

  mu20 = m_n20 * m00;
  mu11 = m_n11 * m00;
  mu02 = m_n02 * m00;

  m20 = mu20 + m10 * m_uc;
  m11 = mu11 + m10 * m_vc;
  m02 = mu02 + m01 * m_vc;
}

void vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &center_p,
                               double a_p, double b_p, double e_p, double alpha1_p, double alpha2_p)
{
  m_trackArc = true;
  // useful for sample(I): uc, vc, a, b, e, Ki, alpha1, alpha2
  m_uc = center_p.get_u();
  m_vc = center_p.get_v();
  a = a_p;
  b = b_p;
  e = e_p;
  ce = cos(e);
  se = sin(e);
  computeNijFromAbe();
  computeKiFromNij();

  alpha1 = alpha1_p;
  alpha2 = alpha2_p;
  if (alpha2 < alpha1) {
    alpha2 += 2 * M_PI;
  }
  // useful for track(I)
  vpImagePoint ip;
  computePointOnEllipse(alpha1, ip);
  iP1 = ip;
  computePointOnEllipse(alpha2, ip);
  iP2 = ip;
  // currently not used after, but done to be complete
  // would be needed for displaying the ellipse here
  iPc = center_p;

  sample(I);
  track(I);
  vpMeTracker::display(I);
  vpDisplay::flush(I);
}

void vpMeEllipse::initTracking(const vpImage<unsigned char> &I, unsigned int n, vpImagePoint *iP)
{
  std::vector<vpImagePoint> v_iP(n);

  for (unsigned int i = 0; i < n; i++) {
    v_iP[i] = iP[i];
  }
  initTracking(I, v_iP);
}

void vpMeEllipse::initTracking(const vpImage<unsigned char> &I, unsigned int n, unsigned *i, unsigned *j)
{
  std::vector<vpImagePoint> v_iP(n);

  for (unsigned int k=0; k < n; k++) {
    v_iP[k].set_ij(i[k],j[k]);
  }
  initTracking(I, v_iP);
}
#endif // Deprecated

void vpMeEllipse::display(const vpImage<unsigned char> &I,
                          const vpImagePoint &center, const double &A, const double &B, const double &E,
                          const double &smallalpha, const double &highalpha,
                          const vpColor &color, unsigned int thickness)
{
  vpDisplay::displayEllipse(I, center, A, B, E, smallalpha, highalpha, false, color, thickness, true, true);
}

void vpMeEllipse::display(const vpImage<vpRGBa> &I,
                          const vpImagePoint &center, const double &A, const double &B, const double &E,
                          const double &smallalpha, const double &highalpha,
                          const vpColor &color, unsigned int thickness)
{
  vpDisplay::displayEllipse(I, center, A, B, E, smallalpha, highalpha, false, color, thickness, true, true);
}