vpMbEdgeTracker.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:
 * Make the complete tracking of an object by using its CAD model
 *
 * Authors:
 * Nicolas Melchior
 * Romain Tallonneau
 * Eric Marchand
 *
 *****************************************************************************/

#include <visp3/core/vpDebug.h>
#include <visp3/core/vpException.h>
#include <visp3/core/vpExponentialMap.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpMatrixException.h>
#include <visp3/core/vpPixelMeterConversion.h>
#include <visp3/core/vpPolygon3D.h>
#include <visp3/core/vpTrackingException.h>
#include <visp3/core/vpVelocityTwistMatrix.h>
#include <visp3/mbt/vpMbEdgeTracker.h>
#include <visp3/mbt/vpMbtDistanceLine.h>
#include <visp3/mbt/vpMbtXmlGenericParser.h>
#include <visp3/vision/vpPose.h>

#include <float.h>
#include <limits>
#include <map>
#include <sstream>
#include <string>

vpMbEdgeTracker::vpMbEdgeTracker()
  : me(), lines(1), circles(1), cylinders(1), nline(0), ncircle(0), ncylinder(0), nbvisiblepolygone(0),
    percentageGdPt(0.4), scales(1), Ipyramid(0), scaleLevel(0), nbFeaturesForProjErrorComputation(0), m_factor(),
    m_robustLines(), m_robustCylinders(), m_robustCircles(), m_wLines(), m_wCylinders(), m_wCircles(), m_errorLines(),
    m_errorCylinders(), m_errorCircles(), m_L_edge(), m_error_edge(), m_w_edge(), m_weightedError_edge(),
    m_robust_edge(), m_featuresToBeDisplayedEdge()
{
  scales[0] = true;

#ifdef VISP_HAVE_OGRE
  faces.getOgreContext()->setWindowName("MBT Edge");
#endif
}

vpMbEdgeTracker::~vpMbEdgeTracker()
{
  vpMbtDistanceLine *l;
  vpMbtDistanceCylinder *cy;
  vpMbtDistanceCircle *ci;

  for (unsigned int i = 0; i < scales.size(); i += 1) {
    if (scales[i]) {
      for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
        l = *it;
        if (l != NULL) {
          delete l;
        }
        l = NULL;
      }

      for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
           ++it) {
        cy = *it;
        if (cy != NULL) {
          delete cy;
        }
        cy = NULL;
      }

      for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
        ci = *it;
        if (ci != NULL) {
          delete ci;
        }
        ci = NULL;
      }

      lines[i].clear();
      cylinders[i].clear();
      circles[i].clear();
    }
  }

  cleanPyramid(Ipyramid);
}

void vpMbEdgeTracker::setMovingEdge(const vpMe &p_me)
{
  this->me = p_me;

  for (unsigned int i = 0; i < scales.size(); i += 1) {
    if (scales[i]) {
      for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
        vpMbtDistanceLine *l = *it;
        l->setMovingEdge(&(this->me));
      }

      for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
           ++it) {
        vpMbtDistanceCylinder *cy = *it;
        cy->setMovingEdge(&(this->me));
      }

      for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
        vpMbtDistanceCircle *ci = *it;
        ci->setMovingEdge(&(this->me));
      }
    }
  }
}

void vpMbEdgeTracker::computeVVS(const vpImage<unsigned char> &_I, unsigned int lvl)
{
  double residu_1 = 1e3;
  double r = 1e3 - 1;

  unsigned int iter = 0;

  computeVVSInit();
  unsigned int nbrow = m_error_edge.getRows();

  bool reloop = true;

  bool isoJoIdentity_ = isoJoIdentity; // Backup since it can be modified if L is not full rank
  if (isoJoIdentity_)
    oJo.eye();

  /*** First phase ***/

  while (reloop == true && iter < 10) {
    double count = 0;

    computeVVSFirstPhase(_I, iter, count, lvl);

    count = count / (double)nbrow;
    if (count >= 0.85) {
      reloop = false;
    }

    computeVVSFirstPhasePoseEstimation(iter, isoJoIdentity_);

    iter++;
  }

  //   std::cout << "\t First minimization in " << iter << " iteration give as
  //   initial cMo: \n" << cMo << std::endl;

  /*** Second phase ***/
  vpHomogeneousMatrix cMoPrev;
  vpColVector W_true(nbrow);
  vpMatrix L_true;
  vpMatrix LVJ_true;

  double mu = m_initialMu;
  vpColVector m_error_prev;
  vpColVector m_w_prev;

  // To avoid to create these matrices each iteration
  vpMatrix LTL;
  vpColVector LTR;
  vpColVector v;

  iter = 0;
  m_w_edge = 1;

  // while ( ((int)((residu_1 - r)*1e8) !=0 )  && (iter<30))
  while (std::fabs((residu_1 - r) * 1e8) > std::numeric_limits<double>::epsilon() && (iter < m_maxIter)) {
    computeVVSInteractionMatrixAndResidu(_I);

    bool reStartFromLastIncrement = false;
    computeVVSCheckLevenbergMarquardt(iter, m_error_edge, m_error_prev, cMoPrev, mu, reStartFromLastIncrement,
                                      &m_w_edge, &m_w_prev);

    if (!reStartFromLastIncrement) {
      computeVVSWeights();

      L_true = m_L_edge;
      vpVelocityTwistMatrix cVo;

      if (computeCovariance) {
        L_true = m_L_edge;
        if (!isoJoIdentity_) {
          cVo.buildFrom(m_cMo);
          LVJ_true = (m_L_edge * cVo * oJo);
        }
      }

      double wi = 0.0, eri = 0.0;
      double num = 0.0, den = 0.0;
      if ((iter == 0) || m_computeInteraction) {
        for (unsigned int i = 0; i < nbrow; i++) {
          wi = m_w_edge[i] * m_factor[i];
          W_true[i] = wi;
          eri = m_error_edge[i];
          num += wi * vpMath::sqr(eri);
          den += wi;

          m_weightedError_edge[i] = wi * eri;

          for (unsigned int j = 0; j < 6; j++) {
            m_L_edge[i][j] = wi * m_L_edge[i][j];
          }
        }
      } else {
        for (unsigned int i = 0; i < nbrow; i++) {
          wi = m_w_edge[i] * m_factor[i];
          W_true[i] = wi;
          eri = m_error_edge[i];
          num += wi * vpMath::sqr(eri);
          den += wi;

          m_weightedError_edge[i] = wi * eri;
        }
      }

      residu_1 = r;
      r = sqrt(num / den); // Le critere d'arret prend en compte le poids

      computeVVSPoseEstimation(isoJoIdentity_, iter, m_L_edge, LTL, m_weightedError_edge, m_error_edge, m_error_prev,
                               LTR, mu, v, &m_w_edge, &m_w_prev);

      cMoPrev = m_cMo;
      m_cMo = vpExponentialMap::direct(v).inverse() * m_cMo;

    } // endif(!restartFromLast)

    iter++;
  }

  computeCovarianceMatrixVVS(isoJoIdentity_, W_true, cMoPrev, L_true, LVJ_true, m_error_edge);

  updateMovingEdgeWeights();
}

void vpMbEdgeTracker::computeVVSFirstPhase(const vpImage<unsigned char> &_I, unsigned int iter, double &count,
                                           unsigned int lvl)
{
  vpMbtDistanceLine *l;
  vpMbtDistanceCylinder *cy;
  vpMbtDistanceCircle *ci;

  double limite = 3;              // Une limite de 3 pixels
  limite = limite / m_cam.get_px(); // Transformation limite pixel en limite metre.

  unsigned int n = 0;

  // Parametre pour la premiere phase d'asservissement
  double e_prev = 0, e_cur, e_next;

  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
    if ((*it)->isTracked()) {
      l = *it;
      l->computeInteractionMatrixError(m_cMo);

      double fac = 1;
      if (iter == 0) {
        for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
             ++itindex) {
          int index = *itindex;
          if (l->hiddenface->isAppearing((unsigned int)index)) {
            fac = 0.2;
            break;
          }
          if (l->closeToImageBorder(_I, 10)) {
            fac = 0.1;
            break;
          }
        }
      }

      std::list<vpMeSite>::const_iterator itListLine;

      unsigned int indexFeature = 0;

      for (size_t a = 0; a < l->meline.size(); a++) {
        if (iter == 0 && l->meline[a] != NULL)
          itListLine = l->meline[a]->getMeList().begin();

        for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
          for (unsigned int j = 0; j < 6; j++) {
            m_L_edge[n + i][j] = l->L[indexFeature][j]; // On remplit la matrice d'interaction globale
          }
          m_error_edge[n + i] = l->error[indexFeature]; // On remplit la matrice d'erreur

          if (m_error_edge[n + i] <= limite)
            count = count + 1.0; // Si erreur proche de 0 on incremente cur

          m_w_edge[n + i] = 0;

          if (iter == 0) {
            m_factor[n + i] = fac;
            vpMeSite site = *itListLine;
            if (site.getState() != vpMeSite::NO_SUPPRESSION)
              m_factor[n + i] = 0.2;
            ++itListLine;
          }

          // If pour la premiere extremite des moving edges
          if (indexFeature == 0) {
            e_cur = l->error[0];
            if (l->nbFeature[a] > 1) {
              e_next = l->error[1];
              if (fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next)) {
                m_w_edge[n + i] = 1 /*0.5*/;
              }
              e_prev = e_cur;
            } else
              m_w_edge[n + i] = 1;
          }

          // If pour la derniere extremite des moving edges
          else if (indexFeature == l->nbFeatureTotal - 1) {
            e_cur = l->error[indexFeature];
            if (fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev)) {
              m_w_edge[n + i] += 1 /*0.5*/;
            }
          }

          else {
            e_cur = l->error[indexFeature];
            e_next = l->error[indexFeature + 1];
            if (fabs(e_cur - e_prev) < limite) {
              m_w_edge[n + i] += 0.5;
            }
            if (fabs(e_cur - e_next) < limite) {
              m_w_edge[n + i] += 0.5;
            }
            e_prev = e_cur;
          }
          indexFeature++;
        }
        n += l->nbFeature[a];
      }
    }
  }

  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
       ++it) {
    if ((*it)->isTracked()) {
      cy = *it;
      cy->computeInteractionMatrixError(m_cMo, _I);
      double fac = 1.0;

      std::list<vpMeSite>::const_iterator itCyl1;
      std::list<vpMeSite>::const_iterator itCyl2;
      if (iter == 0 && (cy->meline1 != NULL || cy->meline2 != NULL)) {
        itCyl1 = cy->meline1->getMeList().begin();
        itCyl2 = cy->meline2->getMeList().begin();
      }

      for (unsigned int i = 0; i < cy->nbFeature; i++) {
        for (unsigned int j = 0; j < 6; j++) {
          m_L_edge[n + i][j] = cy->L[i][j]; // On remplit la matrice d'interaction globale
        }
        m_error_edge[n + i] = cy->error[i]; // On remplit la matrice d'erreur

        if (m_error_edge[n + i] <= limite)
          count = count + 1.0; // Si erreur proche de 0 on incremente cur

        m_w_edge[n + i] = 0;

        if (iter == 0) {
          m_factor[n + i] = fac;
          vpMeSite site;
          if (i < cy->nbFeaturel1) {
            site = *itCyl1;
            ++itCyl1;
          } else {
            site = *itCyl2;
            ++itCyl2;
          }
          if (site.getState() != vpMeSite::NO_SUPPRESSION)
            m_factor[n + i] = 0.2;
        }

        // If pour la premiere extremite des moving edges
        if (i == 0) {
          e_cur = cy->error[0];
          if (cy->nbFeature > 1) {
            e_next = cy->error[1];
            if (fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next)) {
              m_w_edge[n + i] = 1 /*0.5*/;
            }
            e_prev = e_cur;
          } else
            m_w_edge[n + i] = 1;
        }
        if (i == cy->nbFeaturel1) {
          e_cur = cy->error[i];
          if (cy->nbFeaturel2 > 1) {
            e_next = cy->error[i + 1];
            if (fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next)) {
              m_w_edge[n + i] = 1 /*0.5*/;
            }
            e_prev = e_cur;
          } else
            m_w_edge[n + i] = 1;
        }

        // If pour la derniere extremite des moving edges
        else if (i == cy->nbFeaturel1 - 1) {
          e_cur = cy->error[i];
          if (fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev)) {
            m_w_edge[n + i] += 1 /*0.5*/;
          }
        }
        // If pour la derniere extremite des moving edges
        else if (i == cy->nbFeature - 1) {
          e_cur = cy->error[i];
          if (fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev)) {
            m_w_edge[n + i] += 1 /*0.5*/;
          }
        }

        else {
          e_cur = cy->error[i];
          e_next = cy->error[i + 1];
          if (fabs(e_cur - e_prev) < limite) {
            m_w_edge[n + i] += 0.5;
          }
          if (fabs(e_cur - e_next) < limite) {
            m_w_edge[n + i] += 0.5;
          }
          e_prev = e_cur;
        }
      }

      n += cy->nbFeature;
    }
  }

  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
    if ((*it)->isTracked()) {
      ci = *it;
      ci->computeInteractionMatrixError(m_cMo);
      double fac = 1.0;

      std::list<vpMeSite>::const_iterator itCir;
      if (iter == 0 && (ci->meEllipse != NULL)) {
        itCir = ci->meEllipse->getMeList().begin();
      }

      for (unsigned int i = 0; i < ci->nbFeature; i++) {
        for (unsigned int j = 0; j < 6; j++) {
          m_L_edge[n + i][j] = ci->L[i][j]; // On remplit la matrice d'interaction globale
        }
        m_error_edge[n + i] = ci->error[i]; // On remplit la matrice d'erreur

        if (m_error_edge[n + i] <= limite)
          count = count + 1.0; // Si erreur proche de 0 on incremente cur

        m_w_edge[n + i] = 0;

        if (iter == 0) {
          m_factor[n + i] = fac;
          vpMeSite site = *itCir;
          if (site.getState() != vpMeSite::NO_SUPPRESSION)
            m_factor[n + i] = 0.2;
          ++itCir;
        }

        // If pour la premiere extremite des moving edges
        if (i == 0) {
          e_cur = ci->error[0];
          if (ci->nbFeature > 1) {
            e_next = ci->error[1];
            if (fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next)) {
              m_w_edge[n + i] = 1 /*0.5*/;
            }
            e_prev = e_cur;
          } else
            m_w_edge[n + i] = 1;
        }

        // If pour la derniere extremite des moving edges
        else if (i == ci->nbFeature - 1) {
          e_cur = ci->error[i];
          if (fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev)) {
            m_w_edge[n + i] += 1 /*0.5*/;
          }
        }

        else {
          e_cur = ci->error[i];
          e_next = ci->error[i + 1];
          if (fabs(e_cur - e_prev) < limite) {
            m_w_edge[n + i] += 0.5;
          }
          if (fabs(e_cur - e_next) < limite) {
            m_w_edge[n + i] += 0.5;
          }
          e_prev = e_cur;
        }
      }

      n += ci->nbFeature;
    }
  }
}

void vpMbEdgeTracker::computeVVSFirstPhaseFactor(const vpImage<unsigned char> &I, unsigned int lvl)
{
  vpMbtDistanceLine *l;
  vpMbtDistanceCylinder *cy;
  vpMbtDistanceCircle *ci;

  unsigned int n = 0;
  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
    if ((*it)->isTracked()) {
      l = *it;
      l->computeInteractionMatrixError(m_cMo);

      double fac = 1;
      for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
           ++itindex) {
        int index = *itindex;
        if (l->hiddenface->isAppearing((unsigned int)index)) {
          fac = 0.2;
          break;
        }
        if (l->closeToImageBorder(I, 10)) {
          fac = 0.1;
          break;
        }
      }

      unsigned int indexFeature = 0;
      for (size_t a = 0; a < l->meline.size(); a++) {
        std::list<vpMeSite>::const_iterator itListLine;
        if (l->meline[a] != NULL) {
          itListLine = l->meline[a]->getMeList().begin();

          for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
            m_factor[n + i] = fac;
            vpMeSite site = *itListLine;
            if (site.getState() != vpMeSite::NO_SUPPRESSION)
              m_factor[n + i] = 0.2;
            ++itListLine;
            indexFeature++;
          }
          n += l->nbFeature[a];
        }
      }
    }
  }

  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
       ++it) {
    if ((*it)->isTracked()) {
      cy = *it;
      cy->computeInteractionMatrixError(m_cMo, I);

      std::list<vpMeSite>::const_iterator itCyl1;
      std::list<vpMeSite>::const_iterator itCyl2;
      if ((cy->meline1 != NULL || cy->meline2 != NULL)) {
        itCyl1 = cy->meline1->getMeList().begin();
        itCyl2 = cy->meline2->getMeList().begin();

        double fac = 1.0;
        for (unsigned int i = 0; i < cy->nbFeature; i++) {
          m_factor[n + i] = fac;
          vpMeSite site;
          if (i < cy->nbFeaturel1) {
            site = *itCyl1;
            ++itCyl1;
          } else {
            site = *itCyl2;
            ++itCyl2;
          }
          if (site.getState() != vpMeSite::NO_SUPPRESSION)
            m_factor[n + i] = 0.2;
        }
        n += cy->nbFeature;
      }
    }
  }

  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
    if ((*it)->isTracked()) {
      ci = *it;
      ci->computeInteractionMatrixError(m_cMo);

      std::list<vpMeSite>::const_iterator itCir;
      if (ci->meEllipse != NULL) {
        itCir = ci->meEllipse->getMeList().begin();
        double fac = 1.0;

        for (unsigned int i = 0; i < ci->nbFeature; i++) {
          m_factor[n + i] = fac;
          vpMeSite site = *itCir;
          if (site.getState() != vpMeSite::NO_SUPPRESSION)
            m_factor[n + i] = 0.2;
          ++itCir;
        }
        n += ci->nbFeature;
      }
    }
  }
}

void vpMbEdgeTracker::computeVVSFirstPhasePoseEstimation(unsigned int iter, bool &isoJoIdentity_)
{
  unsigned int nerror = m_weightedError_edge.getRows();

  double wi, eri;
  if ((iter == 0) || m_computeInteraction) {
    for (unsigned int i = 0; i < nerror; i++) {
      wi = m_w_edge[i] * m_factor[i];
      eri = m_error_edge[i];

      m_weightedError_edge[i] = wi * eri;

      for (unsigned int j = 0; j < 6; j++) {
        m_L_edge[i][j] = wi * m_L_edge[i][j];
      }
    }
  } else {
    for (unsigned int i = 0; i < nerror; i++) {
      wi = m_w_edge[i] * m_factor[i];
      eri = m_error_edge[i];

      m_weightedError_edge[i] = wi * eri;
    }
  }

  vpVelocityTwistMatrix cVo;

  // If all the 6 dof should be estimated, we check if the interaction matrix
  // is full rank. If not we remove automatically the dof that cannot be
  // estimated This is particularly useful when consering circles (rank 5) and
  // cylinders (rank 4)
  if (isoJoIdentity_) {
    cVo.buildFrom(m_cMo);

    vpMatrix K; // kernel
    unsigned int rank = (m_L_edge * cVo).kernel(K);
    if (rank == 0) {
      throw vpException(vpException::fatalError, "Rank=0, cannot estimate the pose !");
    }
    if (rank != 6) {
      vpMatrix I; // Identity
      I.eye(6);
      oJo = I - K.AtA();

      isoJoIdentity_ = false;
    }
  }

  vpColVector v;
  vpMatrix LTL;
  vpColVector LTR;

  if (isoJoIdentity_) {
    LTL = m_L_edge.AtA();
    computeJTR(m_L_edge, m_weightedError_edge, LTR);
    v = -0.7 * LTL.pseudoInverse(LTL.getRows() * std::numeric_limits<double>::epsilon()) * LTR;
  } else {
    cVo.buildFrom(m_cMo);
    vpMatrix LVJ = (m_L_edge * cVo * oJo);
    vpMatrix LVJTLVJ = (LVJ).AtA();
    vpColVector LVJTR;
    computeJTR(LVJ, m_weightedError_edge, LVJTR);
    v = -0.7 * LVJTLVJ.pseudoInverse(LVJTLVJ.getRows() * std::numeric_limits<double>::epsilon()) * LVJTR;
    v = cVo * v;
  }

  m_cMo = vpExponentialMap::direct(v).inverse() * m_cMo;
}

void vpMbEdgeTracker::computeVVSInit()
{
  // Nombre de moving edges
  unsigned int nbrow = 0;
  unsigned int nberrors_lines = 0;
  unsigned int nberrors_cylinders = 0;
  unsigned int nberrors_circles = 0;

  nbrow = initMbtTracking(nberrors_lines, nberrors_cylinders, nberrors_circles);

  if (nbrow == 0) {
    throw vpTrackingException(vpTrackingException::notEnoughPointError,
                              "No data found to compute the interaction matrix...");
  }

  m_L_edge.resize(nbrow, 6, false, false);
  m_error_edge.resize(nbrow, false);

  m_weightedError_edge.resize(nbrow, false);
  m_w_edge.resize(nbrow, false);
  m_w_edge = 1;
  m_factor.resize(nbrow, false);
  m_factor = 1;

  m_robustLines.setMinMedianAbsoluteDeviation(2.0 / m_cam.get_px());
  m_robustCylinders.setMinMedianAbsoluteDeviation(2.0 / m_cam.get_px());
  m_robustCircles.setMinMedianAbsoluteDeviation(vpMath::sqr(2.0 / m_cam.get_px()));

  m_wLines.resize(nberrors_lines, false);
  m_wLines = 1;
  m_wCylinders.resize(nberrors_cylinders, false);
  m_wCylinders = 1;
  m_wCircles.resize(nberrors_circles, false);
  m_wCircles = 1;

  m_errorLines.resize(nberrors_lines, false);
  m_errorCylinders.resize(nberrors_cylinders, false);
  m_errorCircles.resize(nberrors_circles, false);
}

void vpMbEdgeTracker::computeVVSInteractionMatrixAndResidu()
{
  throw vpException(vpException::fatalError, "vpMbEdgeTracker::"
                                             "computeVVSInteractionMatrixAndR"
                                             "esidu() should not be called!");
}

void vpMbEdgeTracker::computeVVSInteractionMatrixAndResidu(const vpImage<unsigned char> &_I)
{
  vpMbtDistanceLine *l;
  vpMbtDistanceCylinder *cy;
  vpMbtDistanceCircle *ci;

  unsigned int n = 0;
  unsigned int nlines = 0;
  unsigned int ncylinders = 0;
  unsigned int ncircles = 0;

  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
       ++it) {
    if ((*it)->isTracked()) {
      l = *it;
      l->computeInteractionMatrixError(m_cMo);
      for (unsigned int i = 0; i < l->nbFeatureTotal; i++) {
        for (unsigned int j = 0; j < 6; j++) {
          m_L_edge[n + i][j] = l->L[i][j];
          m_error_edge[n + i] = l->error[i];
          m_errorLines[nlines + i] = m_error_edge[n + i];
        }
      }
      n += l->nbFeatureTotal;
      nlines += l->nbFeatureTotal;
    }
  }

  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
       it != cylinders[scaleLevel].end(); ++it) {
    if ((*it)->isTracked()) {
      cy = *it;
      cy->computeInteractionMatrixError(m_cMo, _I);
      for (unsigned int i = 0; i < cy->nbFeature; i++) {
        for (unsigned int j = 0; j < 6; j++) {
          m_L_edge[n + i][j] = cy->L[i][j];
          m_error_edge[n + i] = cy->error[i];
          m_errorCylinders[ncylinders + i] = m_error_edge[n + i];
        }
      }

      n += cy->nbFeature;
      ncylinders += cy->nbFeature;
    }
  }

  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
       it != circles[scaleLevel].end(); ++it) {
    if ((*it)->isTracked()) {
      ci = *it;
      ci->computeInteractionMatrixError(m_cMo);
      for (unsigned int i = 0; i < ci->nbFeature; i++) {
        for (unsigned int j = 0; j < 6; j++) {
          m_L_edge[n + i][j] = ci->L[i][j];
          m_error_edge[n + i] = ci->error[i];
          m_errorCircles[ncircles + i] = m_error_edge[n + i];
        }
      }

      n += ci->nbFeature;
      ncircles += ci->nbFeature;
    }
  }
}

void vpMbEdgeTracker::computeVVSWeights()
{
  unsigned int nberrors_lines = m_errorLines.getRows(), nberrors_cylinders = m_errorCylinders.getRows(),
               nberrors_circles = m_errorCircles.getRows();

  if (nberrors_lines > 0)
    m_robustLines.MEstimator(vpRobust::TUKEY, m_errorLines, m_wLines);
  if (nberrors_cylinders > 0)
    m_robustCylinders.MEstimator(vpRobust::TUKEY, m_errorCylinders, m_wCylinders);
  if (nberrors_circles > 0)
    m_robustCircles.MEstimator(vpRobust::TUKEY, m_errorCircles, m_wCircles);

  m_w_edge.insert(0, m_wLines);
  m_w_edge.insert(m_wLines.getRows(), m_wCylinders);
  m_w_edge.insert(m_wLines.getRows() + m_wCylinders.getRows(), m_wCircles);
}

void vpMbEdgeTracker::computeProjectionError(const vpImage<unsigned char> &_I)
{
  projectionError = 0.0;
  unsigned int nbFeatures = 0;
  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
       ++it) {
    vpMbtDistanceLine *l = *it;
    if (l->isVisible() && l->isTracked()) {
      for (size_t a = 0; a < l->meline.size(); a++) {
        if (l->meline[a] != NULL) {
          double lineNormGradient;
          unsigned int lineNbFeatures;
          l->meline[a]->computeProjectionError(_I, lineNormGradient, lineNbFeatures, m_SobelX, m_SobelY,
                                               m_projectionErrorDisplay, m_projectionErrorDisplayLength,
                                               m_projectionErrorDisplayThickness);
          projectionError += lineNormGradient;
          nbFeatures += lineNbFeatures;
        }
      }
    }
  }

  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
       it != cylinders[scaleLevel].end(); ++it) {
    vpMbtDistanceCylinder *cy = *it;
    if (cy->isVisible() && cy->isTracked()) {
      if (cy->meline1 != NULL) {
        double cylinderNormGradient = 0;
        unsigned int cylinderNbFeatures = 0;
        cy->meline1->computeProjectionError(_I, cylinderNormGradient, cylinderNbFeatures, m_SobelX, m_SobelY,
                                            m_projectionErrorDisplay, m_projectionErrorDisplayLength,
                                            m_projectionErrorDisplayThickness);
        projectionError += cylinderNormGradient;
        nbFeatures += cylinderNbFeatures;
      }

      if (cy->meline2 != NULL) {
        double cylinderNormGradient = 0;
        unsigned int cylinderNbFeatures = 0;
        cy->meline2->computeProjectionError(_I, cylinderNormGradient, cylinderNbFeatures, m_SobelX, m_SobelY,
                                            m_projectionErrorDisplay, m_projectionErrorDisplayLength,
                                            m_projectionErrorDisplayThickness);
        projectionError += cylinderNormGradient;
        nbFeatures += cylinderNbFeatures;
      }
    }
  }

  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
       it != circles[scaleLevel].end(); ++it) {
    vpMbtDistanceCircle *c = *it;
    if (c->isVisible() && c->isTracked() && c->meEllipse != NULL) {
      double circleNormGradient = 0;
      unsigned int circleNbFeatures = 0;
      c->meEllipse->computeProjectionError(_I, circleNormGradient, circleNbFeatures, m_SobelX, m_SobelY,
                                           m_projectionErrorDisplay, m_projectionErrorDisplayLength,
                                           m_projectionErrorDisplayThickness);
      projectionError += circleNormGradient;
      nbFeatures += circleNbFeatures;
    }
  }

  if (nbFeatures > 0) {
    projectionError = vpMath::deg(projectionError / (double)nbFeatures);
  } else {
    projectionError = 90.0;
  }

  nbFeaturesForProjErrorComputation = nbFeatures;
  //  std::cout << "Norm Gradient = " << errorGradient << std::endl;
}

void vpMbEdgeTracker::testTracking()
{
  int nbExpectedPoint = 0;
  int nbGoodPoint = 0;
  int nbBadPoint = 0;

  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
       ++it) {
    vpMbtDistanceLine *l = *it;
    if (l->isVisible() && l->isTracked()) {
      for (size_t a = 0; a < l->meline.size(); a++) {
        if (l->meline[a] != NULL) {
          nbExpectedPoint += (int)l->meline[a]->expecteddensity;
          for (std::list<vpMeSite>::const_iterator itme = l->meline[a]->getMeList().begin();
               itme != l->meline[a]->getMeList().end(); ++itme) {
            vpMeSite pix = *itme;
            if (pix.getState() == vpMeSite::NO_SUPPRESSION)
              nbGoodPoint++;
            else
              nbBadPoint++;
          }
        }
      }
    }
  }

  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
       it != cylinders[scaleLevel].end(); ++it) {
    vpMbtDistanceCylinder *cy = *it;
    if ((cy->meline1 != NULL && cy->meline2 != NULL) && cy->isVisible() && cy->isTracked()) {
      nbExpectedPoint += (int)cy->meline1->expecteddensity;
      for (std::list<vpMeSite>::const_iterator itme1 = cy->meline1->getMeList().begin();
           itme1 != cy->meline1->getMeList().end(); ++itme1) {
        vpMeSite pix = *itme1;
        if (pix.getState() == vpMeSite::NO_SUPPRESSION)
          nbGoodPoint++;
        else
          nbBadPoint++;
      }
      nbExpectedPoint += (int)cy->meline2->expecteddensity;
      for (std::list<vpMeSite>::const_iterator itme2 = cy->meline2->getMeList().begin();
           itme2 != cy->meline2->getMeList().end(); ++itme2) {
        vpMeSite pix = *itme2;
        if (pix.getState() == vpMeSite::NO_SUPPRESSION)
          nbGoodPoint++;
        else
          nbBadPoint++;
      }
    }
  }

  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
       it != circles[scaleLevel].end(); ++it) {
    vpMbtDistanceCircle *ci = *it;
    if (ci->isVisible() && ci->isTracked() && ci->meEllipse != NULL) {
      nbExpectedPoint += ci->meEllipse->getExpectedDensity();
      for (std::list<vpMeSite>::const_iterator itme = ci->meEllipse->getMeList().begin();
           itme != ci->meEllipse->getMeList().end(); ++itme) {
        vpMeSite pix = *itme;
        if (pix.getState() == vpMeSite::NO_SUPPRESSION)
          nbGoodPoint++;
        else
          nbBadPoint++;
      }
    }
  }

  // Compare the number of good points with the min between the number of
  // expected points and number of points that are tracked
  int nb_min = (int)vpMath::minimum(percentageGdPt * nbExpectedPoint, percentageGdPt * (nbGoodPoint + nbBadPoint));
  // int nb_min = (std::min)(val1, val2);
  if (nbGoodPoint < nb_min || nbExpectedPoint < 2) {
    std::ostringstream oss;
    oss << "Not enough moving edges (" << nbGoodPoint << ") to track the object: expected " << nb_min
        << ". Try to reduce the threshold=" << percentageGdPt
        << " using vpMbTracker::setGoodMovingEdgesRatioThreshold()";
    throw vpTrackingException(vpTrackingException::fatalError, oss.str());
  }
}

void vpMbEdgeTracker::track(const vpImage<unsigned char> &I)
{
  initPyramid(I, Ipyramid);

  unsigned int lvl = (unsigned int)scales.size();
  do {
    lvl--;

    projectionError = 90.0;

    if (scales[lvl]) {
      vpHomogeneousMatrix cMo_1 = m_cMo;
      try {
        downScale(lvl);

        try {
          trackMovingEdge(*Ipyramid[lvl]);
        } catch (...) {
          vpTRACE("Error in moving edge tracking");
          throw;
        }

        // initialize the vector that contains the error and the matrix that
        // contains the interaction matrix AY: Useless as it is done in
        // coputeVVS()
        /*
        for(std::list<vpMbtDistanceLine*>::const_iterator
        it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){ l = *it; if
        (l->isVisible()){ l->initInteractionMatrixError();
          }
        }

        for(std::list<vpMbtDistanceCylinder*>::const_iterator
        it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){ cy = *it;
          if(cy->isVisible()) {
            cy->initInteractionMatrixError();
          }
        }

        for(std::list<vpMbtDistanceCircle*>::const_iterator
        it=circles[lvl].begin(); it!=circles[lvl].end(); ++it){ ci = *it; if
        (ci->isVisible()){ ci->initInteractionMatrixError();
          }
        }
        */

        try {
          computeVVS(*Ipyramid[lvl], lvl);
        } catch (...) {
          covarianceMatrix = -1;
          throw; // throw the original exception
        }

        testTracking();

        if (displayFeatures) {
          m_featuresToBeDisplayedEdge = getFeaturesForDisplayEdge();
        }

        // Looking for new visible face
        bool newvisibleface = false;
        visibleFace(I, m_cMo, newvisibleface);

        // cam.computeFov(I.getWidth(), I.getHeight());
        if (useScanLine) {
          faces.computeClippedPolygons(m_cMo, m_cam);
          faces.computeScanLineRender(m_cam, I.getWidth(), I.getHeight());
        }

        updateMovingEdge(I);

        initMovingEdge(I, m_cMo);
        // Reinit the moving edge for the lines which need it.
        reinitMovingEdge(I, m_cMo);

        if (computeProjError)
          computeProjectionError(I);

        upScale(lvl);
      } catch (const vpException &e) {
        if (lvl != 0) {
          m_cMo = cMo_1;
          reInitLevel(lvl);
          upScale(lvl);
        } else {
          upScale(lvl);
          throw(e);
        }
      }
    }
  } while (lvl != 0);

  cleanPyramid(Ipyramid);
}

void vpMbEdgeTracker::track(const vpImage<vpRGBa> &I)
{
  vpImageConvert::convert(I, m_I);
  track(m_I);
}

void vpMbEdgeTracker::init(const vpImage<unsigned char> &I)
{
  if (!modelInitialised) {
    throw vpException(vpException::fatalError, "model not initialized");
  }

  bool a = false;

#ifdef VISP_HAVE_OGRE
  if (useOgre) {
    if (!faces.isOgreInitialised()) {
      faces.setBackgroundSizeOgre(I.getHeight(), I.getWidth());
      faces.setOgreShowConfigDialog(ogreShowConfigDialog);
      faces.initOgre(m_cam);
      // Turn off Ogre config dialog display for the next call to this
      // function since settings are saved in the ogre.cfg file and used
      // during the next call
      ogreShowConfigDialog = false;
    }
  }
#endif

  if (clippingFlag > 2)
    m_cam.computeFov(I.getWidth(), I.getHeight());

  visibleFace(I, m_cMo, a);
  resetMovingEdge();

  if (useScanLine) {
    if (clippingFlag <= 2)
      m_cam.computeFov(I.getWidth(), I.getHeight());

    faces.computeClippedPolygons(m_cMo, m_cam);
    faces.computeScanLineRender(m_cam, I.getWidth(), I.getHeight());
  }

  initPyramid(I, Ipyramid);
  unsigned int i = (unsigned int)scales.size();
  do {
    i--;
    if (scales[i]) {
      downScale(i);
      initMovingEdge(*Ipyramid[i], m_cMo);
      upScale(i);
    }
  } while (i != 0);

  cleanPyramid(Ipyramid);
}

void vpMbEdgeTracker::setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cdMo)
{
  m_cMo = cdMo;

  init(I);
}

void vpMbEdgeTracker::setPose(const vpImage<vpRGBa> &I_color, const vpHomogeneousMatrix &cdMo)
{
  m_cMo = cdMo;

  vpImageConvert::convert(I_color, m_I);
  init(m_I);
}

void vpMbEdgeTracker::loadConfigFile(const std::string &configFile)
{
  // Load projection error config
  vpMbTracker::loadConfigFile(configFile);

#ifdef VISP_HAVE_PUGIXML
  vpMbtXmlGenericParser xmlp(vpMbtXmlGenericParser::EDGE_PARSER);

  xmlp.setCameraParameters(m_cam);
  xmlp.setAngleAppear(vpMath::deg(angleAppears));
  xmlp.setAngleDisappear(vpMath::deg(angleDisappears));
  xmlp.setEdgeMe(me);

  try {
    std::cout << " *********** Parsing XML for Mb Edge Tracker ************ " << std::endl;
    xmlp.parse(configFile);
  } catch (...) {
    throw vpException(vpException::ioError, "Cannot open XML file \"%s\"", configFile.c_str());
  }

  vpCameraParameters camera;
  vpMe meParser;
  xmlp.getCameraParameters(camera);
  xmlp.getEdgeMe(meParser);

  setCameraParameters(camera);
  setMovingEdge(meParser);
  angleAppears = vpMath::rad(xmlp.getAngleAppear());
  angleDisappears = vpMath::rad(xmlp.getAngleDisappear());

  if (xmlp.hasNearClippingDistance())
    setNearClippingDistance(xmlp.getNearClippingDistance());

  if (xmlp.hasFarClippingDistance())
    setFarClippingDistance(xmlp.getFarClippingDistance());

  if (xmlp.getFovClipping())
    setClipping(clippingFlag | vpPolygon3D::FOV_CLIPPING);

  useLodGeneral = xmlp.getLodState();
  minLineLengthThresholdGeneral = xmlp.getLodMinLineLengthThreshold();
  minPolygonAreaThresholdGeneral = xmlp.getLodMinPolygonAreaThreshold();

  applyLodSettingInConfig = false;
  if (this->getNbPolygon() > 0) {
    applyLodSettingInConfig = true;
    setLod(useLodGeneral);
    setMinLineLengthThresh(minLineLengthThresholdGeneral);
    setMinPolygonAreaThresh(minPolygonAreaThresholdGeneral);
  }

#else
  std::cerr << "pugixml third-party is not properly built to read config file: " << configFile << std::endl;
#endif
}

void vpMbEdgeTracker::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo,
                              const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
                              bool displayFullModel)
{
  //Display first the Moving-Edges
  if (displayFeatures) {
    displayFeaturesOnImage(I);
  }

  std::vector<std::vector<double> > models = vpMbEdgeTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);

  for (size_t i = 0; i < models.size(); i++) {
    if (vpMath::equal(models[i][0], 0)) {
      vpImagePoint ip1(models[i][1], models[i][2]);
      vpImagePoint ip2(models[i][3], models[i][4]);
      vpDisplay::displayLine(I, ip1, ip2, col, thickness);
    } else if (vpMath::equal(models[i][0], 1)) {
      vpImagePoint center(models[i][1], models[i][2]);
      double n20 = models[i][3];
      double n11 = models[i][4];
      double n02 = models[i][5];
      vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
    }
  }

#ifdef VISP_HAVE_OGRE
  if (useOgre)
    faces.displayOgre(cMo);
#endif
}

void vpMbEdgeTracker::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
                              const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
                              bool displayFullModel)
{
  //Display first the Moving-Edges
  if (displayFeatures) {
    displayFeaturesOnImage(I);
  }

  std::vector<std::vector<double> > models = vpMbEdgeTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);

  for (size_t i = 0; i < models.size(); i++) {
    if (vpMath::equal(models[i][0], 0)) {
      vpImagePoint ip1(models[i][1], models[i][2]);
      vpImagePoint ip2(models[i][3], models[i][4]);
      vpDisplay::displayLine(I, ip1, ip2, col, thickness);
    } else if (vpMath::equal(models[i][0], 1)) {
      vpImagePoint center(models[i][1], models[i][2]);
      double n20 = models[i][3];
      double n11 = models[i][4];
      double n02 = models[i][5];
      vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
    }
  }

#ifdef VISP_HAVE_OGRE
  if (useOgre)
    faces.displayOgre(cMo);
#endif
}

std::vector<std::vector<double> > vpMbEdgeTracker::getFeaturesForDisplayEdge()
{
  std::vector<std::vector<double> > features;

  const unsigned int lvl = 0;
  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
    vpMbtDistanceLine *l = *it;
    if (l->isVisible() && l->isTracked()) {
      std::vector<std::vector<double> > currentFeatures = l->getFeaturesForDisplay();
      features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
    }
  }

  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
       ++it) {
    vpMbtDistanceCylinder *cy = *it;
    if (cy->isVisible() && cy->isTracked()) {
      std::vector<std::vector<double> > currentFeatures = cy->getFeaturesForDisplay();
      features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
    }
  }

  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
    vpMbtDistanceCircle *ci = *it;
    if (ci->isVisible() && ci->isTracked()) {
      std::vector<std::vector<double> > currentFeatures = ci->getFeaturesForDisplay();
      features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
    }
  }

  return features;
}

std::vector<std::vector<double> > vpMbEdgeTracker::getModelForDisplay(unsigned int width, unsigned int height,
                                                                      const vpHomogeneousMatrix &cMo,
                                                                      const vpCameraParameters &cam,
                                                                      bool displayFullModel)
{
  std::vector<std::vector<double> > models;

  for (unsigned int i = 0; i < scales.size(); i += 1) {
    if (scales[i]) {
      for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
           ++it) {
        std::vector<std::vector<double> > currentModel =
          (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel);
        models.insert(models.end(), currentModel.begin(), currentModel.end());
      }

      for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
           it != cylinders[scaleLevel].end(); ++it) {
        std::vector<std::vector<double> > currentModel =
          (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel);
        models.insert(models.end(), currentModel.begin(), currentModel.end());
      }

      for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
           it != circles[scaleLevel].end(); ++it) {
        std::vector<double> paramsCircle = (*it)->getModelForDisplay(cMo, cam, displayFullModel);
        if (!paramsCircle.empty()) {
          models.push_back(paramsCircle);
        }
      }
      break; // displaying model on one scale only
    }
  }

  return models;
}

void vpMbEdgeTracker::displayFeaturesOnImage(const vpImage<unsigned char> &I)
{
  for (size_t i = 0; i < m_featuresToBeDisplayedEdge.size(); i++) {
    if (vpMath::equal(m_featuresToBeDisplayedEdge[i][0], 0)) {
      vpImagePoint ip(m_featuresToBeDisplayedEdge[i][1], m_featuresToBeDisplayedEdge[i][2]);
      int state = static_cast<int>(m_featuresToBeDisplayedEdge[i][3]);

      switch (state) {
      case vpMeSite::NO_SUPPRESSION:
        vpDisplay::displayCross(I, ip, 3, vpColor::green, 1);
        break;

      case vpMeSite::CONSTRAST:
        vpDisplay::displayCross(I, ip, 3, vpColor::blue, 1);
        break;

      case vpMeSite::THRESHOLD:
        vpDisplay::displayCross(I, ip, 3, vpColor::purple, 1);
        break;

      case vpMeSite::M_ESTIMATOR:
        vpDisplay::displayCross(I, ip, 3, vpColor::red, 1);
        break;

      case vpMeSite::TOO_NEAR:
        vpDisplay::displayCross(I, ip, 3, vpColor::cyan, 1);
        break;

      default:
        vpDisplay::displayCross(I, ip, 3, vpColor::yellow, 1);
      }
    }
  }
}

void vpMbEdgeTracker::displayFeaturesOnImage(const vpImage<vpRGBa> &I)
{
  for (size_t i = 0; i < m_featuresToBeDisplayedEdge.size(); i++) {
    if (vpMath::equal(m_featuresToBeDisplayedEdge[i][0], 0)) {
      vpImagePoint ip(m_featuresToBeDisplayedEdge[i][1], m_featuresToBeDisplayedEdge[i][2]);
      int state = static_cast<int>(m_featuresToBeDisplayedEdge[i][3]);

      switch (state) {
      case vpMeSite::NO_SUPPRESSION:
        vpDisplay::displayCross(I, ip, 3, vpColor::green, 1);
        break;

      case vpMeSite::CONSTRAST:
        vpDisplay::displayCross(I, ip, 3, vpColor::blue, 1);
        break;

      case vpMeSite::THRESHOLD:
        vpDisplay::displayCross(I, ip, 3, vpColor::purple, 1);
        break;

      case vpMeSite::M_ESTIMATOR:
        vpDisplay::displayCross(I, ip, 3, vpColor::red, 1);
        break;

      case vpMeSite::TOO_NEAR:
        vpDisplay::displayCross(I, ip, 3, vpColor::cyan, 1);
        break;

      default:
        vpDisplay::displayCross(I, ip, 3, vpColor::yellow, 1);
      }
    }
  }
}

void vpMbEdgeTracker::initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &_cMo)
{
  const bool doNotTrack = false;

  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
       ++it) {
    vpMbtDistanceLine *l = *it;
    bool isvisible = false;

    for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
         ++itindex) {
      int index = *itindex;
      if (index == -1)
        isvisible = true;
      else {
        if (l->hiddenface->isVisible((unsigned int)index))
          isvisible = true;
      }
    }

    // Si la ligne n'appartient a aucune face elle est tout le temps visible
    if (l->Lindex_polygon.empty())
      isvisible = true; // Not sure that this can occur

    if (isvisible) {
      l->setVisible(true);
      l->updateTracked();
      if (l->meline.empty() && l->isTracked())
        l->initMovingEdge(I, _cMo, doNotTrack, m_mask);
    } else {
      l->setVisible(false);
      for (size_t a = 0; a < l->meline.size(); a++) {
        if (l->meline[a] != NULL)
          delete l->meline[a];
        if (a < l->nbFeature.size())
          l->nbFeature[a] = 0;
      }
      l->nbFeatureTotal = 0;
      l->meline.clear();
      l->nbFeature.clear();
    }
  }

  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
       it != cylinders[scaleLevel].end(); ++it) {
    vpMbtDistanceCylinder *cy = *it;

    bool isvisible = false;

    int index = cy->index_polygon;
    if (index == -1)
      isvisible = true;
    else {
      if (cy->hiddenface->isVisible((unsigned int)index + 1) || cy->hiddenface->isVisible((unsigned int)index + 2) ||
          cy->hiddenface->isVisible((unsigned int)index + 3) || cy->hiddenface->isVisible((unsigned int)index + 4))
        isvisible = true;
    }
    //    vpTRACE("cyl with index %d is visible: %d", index, isvisible);

    if (isvisible) {
      cy->setVisible(true);
      if (cy->meline1 == NULL || cy->meline2 == NULL) {
        if (cy->isTracked())
          cy->initMovingEdge(I, _cMo, doNotTrack, m_mask);
      }
    } else {
      cy->setVisible(false);
      if (cy->meline1 != NULL)
        delete cy->meline1;
      if (cy->meline2 != NULL)
        delete cy->meline2;
      cy->meline1 = NULL;
      cy->meline2 = NULL;
      cy->nbFeature = 0;
      cy->nbFeaturel1 = 0;
      cy->nbFeaturel2 = 0;
    }
  }

  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
       it != circles[scaleLevel].end(); ++it) {
    vpMbtDistanceCircle *ci = *it;
    bool isvisible = false;

    int index = ci->index_polygon;
    if (index == -1)
      isvisible = true;
    else {
      if (ci->hiddenface->isVisible((unsigned int)index))
        isvisible = true;
    }

    if (isvisible) {
      ci->setVisible(true);
      if (ci->meEllipse == NULL) {
        if (ci->isTracked())
          ci->initMovingEdge(I, _cMo, doNotTrack, m_mask);
      }
    } else {
      ci->setVisible(false);
      if (ci->meEllipse != NULL)
        delete ci->meEllipse;
      ci->meEllipse = NULL;
      ci->nbFeature = 0;
    }
  }
}

void vpMbEdgeTracker::trackMovingEdge(const vpImage<unsigned char> &I)
{
  const bool doNotTrack = false;

  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
       ++it) {
    vpMbtDistanceLine *l = *it;
    if (l->isVisible() && l->isTracked()) {
      if (l->meline.empty()) {
        l->initMovingEdge(I, m_cMo, doNotTrack, m_mask);
      }
      l->trackMovingEdge(I);
    }
  }

  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
       it != cylinders[scaleLevel].end(); ++it) {
    vpMbtDistanceCylinder *cy = *it;
    if (cy->isVisible() && cy->isTracked()) {
      if (cy->meline1 == NULL || cy->meline2 == NULL) {
        cy->initMovingEdge(I, m_cMo, doNotTrack, m_mask);
      }
      cy->trackMovingEdge(I, m_cMo);
    }
  }

  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
       it != circles[scaleLevel].end(); ++it) {
    vpMbtDistanceCircle *ci = *it;
    if (ci->isVisible() && ci->isTracked()) {
      if (ci->meEllipse == NULL) {
        ci->initMovingEdge(I, m_cMo, doNotTrack, m_mask);
      }
      ci->trackMovingEdge(I, m_cMo);
    }
  }
}

void vpMbEdgeTracker::updateMovingEdge(const vpImage<unsigned char> &I)
{
  vpMbtDistanceLine *l;
  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
       ++it) {
    if ((*it)->isTracked()) {
      l = *it;
      l->updateMovingEdge(I, m_cMo);
      if (l->nbFeatureTotal == 0 && l->isVisible()) {
        l->Reinit = true;
      }
    }
  }

  vpMbtDistanceCylinder *cy;
  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
       it != cylinders[scaleLevel].end(); ++it) {
    if ((*it)->isTracked()) {
      cy = *it;
      cy->updateMovingEdge(I, m_cMo);
      if ((cy->nbFeaturel1 == 0 || cy->nbFeaturel2 == 0) && cy->isVisible()) {
        cy->Reinit = true;
      }
    }
  }

  vpMbtDistanceCircle *ci;
  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
       it != circles[scaleLevel].end(); ++it) {
    if ((*it)->isTracked()) {
      ci = *it;
      ci->updateMovingEdge(I, m_cMo);
      if (ci->nbFeature == 0 && ci->isVisible()) {
        ci->Reinit = true;
      }
    }
  }
}

void vpMbEdgeTracker::updateMovingEdgeWeights()
{
  unsigned int n = 0;

  vpMbtDistanceLine *l;
  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
       ++it) {
    if ((*it)->isTracked()) {
      l = *it;
      unsigned int indexLine = 0;
      double wmean = 0;
      for (size_t a = 0; a < l->meline.size(); a++) {
        if (l->nbFeature[a] > 0) {
          std::list<vpMeSite>::iterator itListLine;
          itListLine = l->meline[a]->getMeList().begin();

          for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
            wmean += m_w_edge[n + indexLine];
            vpMeSite p = *itListLine;
            if (m_w_edge[n + indexLine] < 0.5) {
              p.setState(vpMeSite::M_ESTIMATOR);

              *itListLine = p;
            }

            ++itListLine;
            indexLine++;
          }
        }
      }
      n += l->nbFeatureTotal;

      if (l->nbFeatureTotal != 0)
        wmean /= l->nbFeatureTotal;
      else
        wmean = 1;

      l->setMeanWeight(wmean);

      if (wmean < 0.8)
        l->Reinit = true;
    }
  }

  // Same thing with cylinders as with lines
  vpMbtDistanceCylinder *cy;
  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
       it != cylinders[scaleLevel].end(); ++it) {
    if ((*it)->isTracked()) {
      cy = *it;
      double wmean = 0;
      std::list<vpMeSite>::iterator itListCyl1;
      std::list<vpMeSite>::iterator itListCyl2;

      if (cy->nbFeature > 0) {
        itListCyl1 = cy->meline1->getMeList().begin();
        itListCyl2 = cy->meline2->getMeList().begin();

        for (unsigned int i = 0; i < cy->nbFeaturel1; i++) {
          wmean += m_w_edge[n + i];
          vpMeSite p = *itListCyl1;
          if (m_w_edge[n + i] < 0.5) {
            p.setState(vpMeSite::M_ESTIMATOR);

            *itListCyl1 = p;
          }

          ++itListCyl1;
        }
      }

      if (cy->nbFeaturel1 != 0)
        wmean /= cy->nbFeaturel1;
      else
        wmean = 1;

      cy->setMeanWeight1(wmean);

      if (wmean < 0.8) {
        cy->Reinit = true;
      }

      wmean = 0;
      for (unsigned int i = cy->nbFeaturel1; i < cy->nbFeature; i++) {
        wmean += m_w_edge[n + i];
        vpMeSite p = *itListCyl2;
        if (m_w_edge[n + i] < 0.5) {
          p.setState(vpMeSite::M_ESTIMATOR);

          *itListCyl2 = p;
        }

        ++itListCyl2;
      }

      if (cy->nbFeaturel2 != 0)
        wmean /= cy->nbFeaturel2;
      else
        wmean = 1;

      cy->setMeanWeight2(wmean);

      if (wmean < 0.8) {
        cy->Reinit = true;
      }

      n += cy->nbFeature;
    }
  }

  // Same thing with circles as with lines
  vpMbtDistanceCircle *ci;
  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
       it != circles[scaleLevel].end(); ++it) {
    if ((*it)->isTracked()) {
      ci = *it;
      double wmean = 0;
      std::list<vpMeSite>::iterator itListCir;

      if (ci->nbFeature > 0) {
        itListCir = ci->meEllipse->getMeList().begin();
      }

      wmean = 0;
      for (unsigned int i = 0; i < ci->nbFeature; i++) {
        wmean += m_w_edge[n + i];
        vpMeSite p = *itListCir;
        if (m_w_edge[n + i] < 0.5) {
          p.setState(vpMeSite::M_ESTIMATOR);

          *itListCir = p;
        }

        ++itListCir;
      }

      if (ci->nbFeature != 0)
        wmean /= ci->nbFeature;
      else
        wmean = 1;

      ci->setMeanWeight(wmean);

      if (wmean < 0.8) {
        ci->Reinit = true;
      }

      n += ci->nbFeature;
    }
  }
}

void vpMbEdgeTracker::reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &_cMo)
{
  vpMbtDistanceLine *l;
  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
       ++it) {
    if ((*it)->isTracked()) {
      l = *it;
      if (l->Reinit && l->isVisible())
        l->reinitMovingEdge(I, _cMo, m_mask);
    }
  }

  vpMbtDistanceCylinder *cy;
  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
       it != cylinders[scaleLevel].end(); ++it) {
    if ((*it)->isTracked()) {
      cy = *it;
      if (cy->Reinit && cy->isVisible())
        cy->reinitMovingEdge(I, _cMo, m_mask);
    }
  }

  vpMbtDistanceCircle *ci;
  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
       it != circles[scaleLevel].end(); ++it) {
    if ((*it)->isTracked()) {
      ci = *it;
      if (ci->Reinit && ci->isVisible())
        ci->reinitMovingEdge(I, _cMo, m_mask);
    }
  }
}

void vpMbEdgeTracker::resetMovingEdge()
{
  for (unsigned int i = 0; i < scales.size(); i += 1) {
    if (scales[i]) {
      for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
        for (size_t a = 0; a < (*it)->meline.size(); a++) {
          if ((*it)->meline[a] != NULL) {
            delete (*it)->meline[a];
            (*it)->meline[a] = NULL;
          }
        }

        (*it)->meline.clear();
        (*it)->nbFeature.clear();
        (*it)->nbFeatureTotal = 0;
      }

      for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
           ++it) {
        if ((*it)->meline1 != NULL) {
          delete (*it)->meline1;
          (*it)->meline1 = NULL;
        }
        if ((*it)->meline2 != NULL) {
          delete (*it)->meline2;
          (*it)->meline2 = NULL;
        }

        (*it)->nbFeature = 0;
        (*it)->nbFeaturel1 = 0;
        (*it)->nbFeaturel2 = 0;
      }

      for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
        if ((*it)->meEllipse != NULL) {
          delete (*it)->meEllipse;
          (*it)->meEllipse = NULL;
        }
        (*it)->nbFeature = 0;
      }
    }
  }
}

void vpMbEdgeTracker::addLine(vpPoint &P1, vpPoint &P2, int polygon, std::string name)
{
  {
    // suppress line already in the model
    bool already_here = false;
    vpMbtDistanceLine *l;

    for (unsigned int i = 0; i < scales.size(); i += 1) {
      if (scales[i]) {
        downScale(i);
        for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
          l = *it;
          if ((samePoint(*(l->p1), P1) && samePoint(*(l->p2), P2)) ||
              (samePoint(*(l->p1), P2) && samePoint(*(l->p2), P1))) {
            already_here = true;
            l->addPolygon(polygon);
            l->hiddenface = &faces;
          }
        }

        if (!already_here) {
          l = new vpMbtDistanceLine;

          l->setCameraParameters(m_cam);
          l->buildFrom(P1, P2);
          l->addPolygon(polygon);
          l->setMovingEdge(&me);
          l->hiddenface = &faces;
          l->useScanLine = useScanLine;

          l->setIndex(nline);
          l->setName(name);

          if (clippingFlag != vpPolygon3D::NO_CLIPPING)
            l->getPolygon().setClipping(clippingFlag);

          if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
            l->getPolygon().setNearClippingDistance(distNearClip);

          if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
            l->getPolygon().setFarClippingDistance(distFarClip);

          nline += 1;
          lines[i].push_back(l);
        }
        upScale(i);
      }
    }
  }
}

void vpMbEdgeTracker::removeLine(const std::string &name)
{
  vpMbtDistanceLine *l;

  for (unsigned int i = 0; i < scales.size(); i++) {
    if (scales[i]) {
      for (std::list<vpMbtDistanceLine *>::iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
        l = *it;
        if (name.compare(l->getName()) == 0) {
          lines[i].erase(it);
          break;
        }
      }
    }
  }
}

void vpMbEdgeTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, double r, int idFace,
                                const std::string &name)
{
  {
    bool already_here = false;
    vpMbtDistanceCircle *ci;

    for (unsigned int i = 0; i < scales.size(); i += 1) {
      if (scales[i]) {
        downScale(i);
        for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
          ci = *it;
          if ((samePoint(*(ci->p1), P1) && samePoint(*(ci->p2), P2) && samePoint(*(ci->p3), P3)) ||
              (samePoint(*(ci->p1), P1) && samePoint(*(ci->p2), P3) && samePoint(*(ci->p3), P2))) {
            already_here =
                (std::fabs(ci->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(ci->radius, r));
          }
        }

        if (!already_here) {
          ci = new vpMbtDistanceCircle;

          ci->setCameraParameters(m_cam);
          ci->buildFrom(P1, P2, P3, r);
          ci->setMovingEdge(&me);
          ci->setIndex(ncircle);
          ci->setName(name);
          ci->index_polygon = idFace;
          ci->hiddenface = &faces;

          //        if(clippingFlag != vpPolygon3D::NO_CLIPPING)
          //          ci->getPolygon().setClipping(clippingFlag);

          //        if((clippingFlag & vpPolygon3D::NEAR_CLIPPING) ==
          //        vpPolygon3D::NEAR_CLIPPING)
          //          ci->getPolygon().setNearClippingDistance(distNearClip);

          //        if((clippingFlag & vpPolygon3D::FAR_CLIPPING) ==
          //        vpPolygon3D::FAR_CLIPPING)
          //          ci->getPolygon().setFarClippingDistance(distFarClip);

          ncircle += 1;
          circles[i].push_back(ci);
        }
        upScale(i);
      }
    }
  }
}

void vpMbEdgeTracker::addCylinder(const vpPoint &P1, const vpPoint &P2, double r, int idFace,
                                  const std::string &name)
{
  {
    bool already_here = false;
    vpMbtDistanceCylinder *cy;

    for (unsigned int i = 0; i < scales.size(); i += 1) {
      if (scales[i]) {
        downScale(i);
        for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
             ++it) {
          cy = *it;
          if ((samePoint(*(cy->p1), P1) && samePoint(*(cy->p2), P2)) ||
              (samePoint(*(cy->p1), P2) && samePoint(*(cy->p2), P1))) {
            already_here =
                (std::fabs(cy->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(cy->radius, r));
          }
        }

        if (!already_here) {
          cy = new vpMbtDistanceCylinder;

          cy->setCameraParameters(m_cam);
          cy->buildFrom(P1, P2, r);
          cy->setMovingEdge(&me);
          cy->setIndex(ncylinder);
          cy->setName(name);
          cy->index_polygon = idFace;
          cy->hiddenface = &faces;
          ncylinder += 1;
          cylinders[i].push_back(cy);
        }
        upScale(i);
      }
    }
  }
}

void vpMbEdgeTracker::removeCylinder(const std::string &name)
{
  vpMbtDistanceCylinder *cy;

  for (unsigned int i = 0; i < scales.size(); i++) {
    if (scales[i]) {
      for (std::list<vpMbtDistanceCylinder *>::iterator it = cylinders[i].begin(); it != cylinders[i].end(); ++it) {
        cy = *it;
        if (name.compare(cy->getName()) == 0) {
          cylinders[i].erase(it);
          break;
        }
      }
    }
  }
}

void vpMbEdgeTracker::removeCircle(const std::string &name)
{
  vpMbtDistanceCircle *ci;

  for (unsigned int i = 0; i < scales.size(); i++) {
    if (scales[i]) {
      for (std::list<vpMbtDistanceCircle *>::iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
        ci = *it;
        if (name.compare(ci->getName()) == 0) {
          circles[i].erase(it);
          break;
        }
      }
    }
  }
}

void vpMbEdgeTracker::addPolygon(vpMbtPolygon &p)
{
  unsigned int nbpt = p.getNbPoint();
  if (nbpt > 0) {
    for (unsigned int i = 0; i < nbpt - 1; i++)
      addLine(p.p[i], p.p[i + 1], p.getIndex());
    addLine(p.p[nbpt - 1], p.p[0], p.getIndex());
  }
}

void vpMbEdgeTracker::visibleFace(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo,
                                  bool &newvisibleline)
{
  unsigned int n;
  bool changed = false;

  if (!useOgre) {
    // n = faces.setVisible(_I.getWidth(), I.getHeight(), m_cam, cMo, vpMath::rad(89), vpMath::rad(89),
    // changed);
    n = faces.setVisible(I.getWidth(), I.getHeight(), m_cam, cMo, angleAppears, angleDisappears, changed);
  } else {
#ifdef VISP_HAVE_OGRE
    n = faces.setVisibleOgre(I.getWidth(), I.getHeight(), m_cam, cMo, angleAppears, angleDisappears, changed);
#else
    n = faces.setVisible(I.getWidth(), I.getHeight(), m_cam, cMo, angleAppears, angleDisappears, changed);
#endif
  }

  if (n > nbvisiblepolygone) {
    // cout << "une nouvelle face est visible " << endl;
    newvisibleline = true;
  } else
    newvisibleline = false;

  nbvisiblepolygone = n;
}

void vpMbEdgeTracker::initFaceFromCorners(vpMbtPolygon &polygon)
{
  unsigned int nbpt = polygon.getNbPoint();
  if (nbpt > 0) {
    for (unsigned int i = 0; i < nbpt - 1; i++)
      vpMbEdgeTracker::addLine(polygon.p[i], polygon.p[i + 1], polygon.getIndex(), polygon.getName());
    vpMbEdgeTracker::addLine(polygon.p[nbpt - 1], polygon.p[0], polygon.getIndex(), polygon.getName());
  }
}
void vpMbEdgeTracker::initFaceFromLines(vpMbtPolygon &polygon)
{
  unsigned int nbpt = polygon.getNbPoint();
  if (nbpt > 0) {
    for (unsigned int i = 0; i < nbpt - 1; i++)
      vpMbEdgeTracker::addLine(polygon.p[i], polygon.p[i + 1], polygon.getIndex(), polygon.getName());
  }
}

unsigned int vpMbEdgeTracker::initMbtTracking(unsigned int &nberrors_lines, unsigned int &nberrors_cylinders,
                                              unsigned int &nberrors_circles)
{
  unsigned int nbrow = 0;
  nberrors_lines = 0;
  nberrors_cylinders = 0;
  nberrors_circles = 0;

  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
       ++it) {

    vpMbtDistanceLine *l = *it;

    if (l->isTracked()) {
      l->initInteractionMatrixError();
      nbrow += l->nbFeatureTotal;
      nberrors_lines += l->nbFeatureTotal;
    }
  }

  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
       it != cylinders[scaleLevel].end(); ++it) {
    vpMbtDistanceCylinder *cy = *it;

    if (cy->isTracked()) {
      cy->initInteractionMatrixError();
      nbrow += cy->nbFeature;
      nberrors_cylinders += cy->nbFeature;
    }
  }

  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
       it != circles[scaleLevel].end(); ++it) {
    vpMbtDistanceCircle *ci = *it;

    if (ci->isTracked()) {
      ci->initInteractionMatrixError();
      nbrow += ci->nbFeature;
      nberrors_circles += ci->nbFeature;
    }
  }

  return nbrow;
}

void vpMbEdgeTracker::initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius,
                                 int idFace, const std::string &name)
{
  addCircle(p1, p2, p3, radius, (int)idFace, name);
}

void vpMbEdgeTracker::initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace,
                                   const std::string &name)
{
  addCylinder(p1, p2, radius, (int)idFace, name);
}

void vpMbEdgeTracker::resetTracker()
{
  m_cMo.eye();
  vpMbtDistanceLine *l;
  vpMbtDistanceCylinder *cy;
  vpMbtDistanceCircle *ci;

  for (unsigned int i = 0; i < scales.size(); i += 1) {
    if (scales[i]) {
      for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
        l = *it;
        if (l != NULL)
          delete l;
        l = NULL;
      }

      for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
           ++it) {
        cy = *it;
        if (cy != NULL)
          delete cy;
        cy = NULL;
      }

      for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
        ci = *it;
        if (ci != NULL)
          delete ci;
        ci = NULL;
      }
      lines[i].clear();
      cylinders[i].clear();
      circles[i].clear();
    }
  }

  faces.reset();

  useScanLine = false;

#ifdef VISP_HAVE_OGRE
  useOgre = false;
#endif

  m_computeInteraction = true;
  nline = 0;
  ncylinder = 0;
  m_lambda = 1.0;
  nbvisiblepolygone = 0;
  percentageGdPt = 0.4;

  angleAppears = vpMath::rad(89);
  angleDisappears = vpMath::rad(89);
  clippingFlag = vpPolygon3D::NO_CLIPPING;

  m_optimizationMethod = vpMbTracker::GAUSS_NEWTON_OPT;

  // reinitialization of the scales.
  this->setScales(scales);
}

void vpMbEdgeTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
                                  const vpHomogeneousMatrix &cMo, bool verbose,
                                  const vpHomogeneousMatrix &T)
{
  m_cMo.eye();
  vpMbtDistanceLine *l;
  vpMbtDistanceCylinder *cy;
  vpMbtDistanceCircle *ci;

  for (unsigned int i = 0; i < scales.size(); i += 1) {
    if (scales[i]) {
      for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
        l = *it;
        if (l != NULL)
          delete l;
        l = NULL;
      }

      for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
           ++it) {
        cy = *it;
        if (cy != NULL)
          delete cy;
        cy = NULL;
      }

      for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
        ci = *it;
        if (ci != NULL)
          delete ci;
        ci = NULL;
      }

      lines[i].clear();
      cylinders[i].clear();
      circles[i].clear();
    }
  }

  faces.reset();

  // compute_interaction=1;
  nline = 0;
  ncylinder = 0;
  ncircle = 0;
  // lambda = 1;
  nbvisiblepolygone = 0;

  loadModel(cad_name, verbose, T);
  initFromPose(I, cMo);
}

unsigned int vpMbEdgeTracker::getNbPoints(unsigned int level) const
{
  if ((level > scales.size()) || !scales[level]) {
    throw vpException(vpException::dimensionError, "Cannot get the number of points for level %d: level is not used",
                      level);
  }

  unsigned int nbGoodPoints = 0;
  vpMbtDistanceLine *l;
  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[level].begin(); it != lines[level].end(); ++it) {
    l = *it;
    if (l->isVisible() && l->isTracked()) {
      for (size_t a = 0; a < l->meline.size(); a++) {
        if (l->nbFeature[a] != 0)
          for (std::list<vpMeSite>::const_iterator itme = l->meline[a]->getMeList().begin();
               itme != l->meline[a]->getMeList().end(); ++itme) {
            if (itme->getState() == vpMeSite::NO_SUPPRESSION)
              nbGoodPoints++;
          }
      }
    }
  }

  vpMbtDistanceCylinder *cy;
  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[level].begin(); it != cylinders[level].end();
       ++it) {
    cy = *it;
    if (cy->isVisible() && cy->isTracked() && (cy->meline1 != NULL || cy->meline2 != NULL)) {
      for (std::list<vpMeSite>::const_iterator itme1 = cy->meline1->getMeList().begin();
           itme1 != cy->meline1->getMeList().end(); ++itme1) {
        if (itme1->getState() == vpMeSite::NO_SUPPRESSION)
          nbGoodPoints++;
      }
      for (std::list<vpMeSite>::const_iterator itme2 = cy->meline2->getMeList().begin();
           itme2 != cy->meline2->getMeList().end(); ++itme2) {
        if (itme2->getState() == vpMeSite::NO_SUPPRESSION)
          nbGoodPoints++;
      }
    }
  }

  vpMbtDistanceCircle *ci;
  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[level].begin(); it != circles[level].end(); ++it) {
    ci = *it;
    if (ci->isVisible() && ci->isTracked() && ci->meEllipse != NULL) {
      for (std::list<vpMeSite>::const_iterator itme = ci->meEllipse->getMeList().begin();
           itme != ci->meEllipse->getMeList().end(); ++itme) {
        if (itme->getState() == vpMeSite::NO_SUPPRESSION)
          nbGoodPoints++;
      }
    }
  }

  return nbGoodPoints;
}

void vpMbEdgeTracker::setScales(const std::vector<bool> &scale)
{
  unsigned int nbActivatedLevels = 0;
  for (unsigned int i = 0; i < scale.size(); i++) {
    if (scale[i]) {
      nbActivatedLevels++;
    }
  }

  if (scale.empty() || (nbActivatedLevels == 0)) {
    vpERROR_TRACE(" !! WARNING : must use at least one level for the "
                  "tracking. Use the global one");
    this->scales.resize(0);
    this->scales.push_back(true);

    lines.resize(1);
    lines[0].clear();

    cylinders.resize(1);
    cylinders[0].clear();

    circles.resize(1);
    circles[0].clear();
  } else {
    this->scales = scale;

    lines.resize(scale.size());
    cylinders.resize(scale.size());
    circles.resize(scale.size());

    for (unsigned int i = 0; i < lines.size(); i++) {
      lines[i].clear();
      cylinders[i].clear();
      circles[i].clear();
    }
  }
}

void vpMbEdgeTracker::setFarClippingDistance(const double &dist)
{
  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING && dist <= distNearClip)
    std::cerr << "Far clipping value cannot be inferior than near clipping "
                 "value. Far clipping won't be considered."
              << std::endl;
  else if (dist < 0)
    std::cerr << "Far clipping value cannot be inferior than 0. Far clipping "
                 "won't be considered."
              << std::endl;
  else {
    vpMbTracker::setFarClippingDistance(dist);
    vpMbtDistanceLine *l;

    for (unsigned int i = 0; i < scales.size(); i += 1) {
      if (scales[i]) {
        for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
          l = *it;
          l->getPolygon().setFarClippingDistance(distFarClip);
        }
      }
    }
  }
}

void vpMbEdgeTracker::setNearClippingDistance(const double &dist)
{
  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING && dist >= distFarClip)
    std::cerr << "Near clipping value cannot be superior than far clipping "
                 "value. Near clipping won't be considered."
              << std::endl;
  else if (dist < 0)
    std::cerr << "Near clipping value cannot be inferior than 0. Near "
                 "clipping won't be considered."
              << std::endl;
  else {
    vpMbTracker::setNearClippingDistance(dist);
    vpMbtDistanceLine *l;

    for (unsigned int i = 0; i < scales.size(); i += 1) {
      if (scales[i]) {
        for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
          l = *it;
          l->getPolygon().setNearClippingDistance(distNearClip);
        }
      }
    }
  }
}

void vpMbEdgeTracker::setClipping(const unsigned int &flags)
{
  vpMbTracker::setClipping(flags);

  vpMbtDistanceLine *l;

  for (unsigned int i = 0; i < scales.size(); i += 1) {
    if (scales[i]) {
      for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
        l = *it;
        l->getPolygon().setClipping(clippingFlag);
      }
    }
  }
}

void vpMbEdgeTracker::initPyramid(const vpImage<unsigned char> &_I,
                                  std::vector<const vpImage<unsigned char> *> &_pyramid)
{
  _pyramid.resize(scales.size());

  if (scales[0]) {
    _pyramid[0] = &_I;
  } else {
    _pyramid[0] = NULL;
  }

  for (unsigned int i = 1; i < _pyramid.size(); i += 1) {
    if (scales[i]) {
      unsigned int cScale = static_cast<unsigned int>(pow(2., (int)i));
      vpImage<unsigned char> *I = new vpImage<unsigned char>(_I.getHeight() / cScale, _I.getWidth() / cScale);
#if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x020408))
      IplImage *vpI0 = cvCreateImageHeader(cvSize((int)_I.getWidth(), (int)_I.getHeight()), IPL_DEPTH_8U, 1);
      vpI0->imageData = (char *)(_I.bitmap);
      IplImage *vpI =
          cvCreateImage(cvSize((int)(_I.getWidth() / cScale), (int)(_I.getHeight() / cScale)), IPL_DEPTH_8U, 1);
      cvResize(vpI0, vpI, CV_INTER_NN);
      vpImageConvert::convert(vpI, *I);
      cvReleaseImage(&vpI);
      vpI0->imageData = NULL;
      cvReleaseImageHeader(&vpI0);
#else
      for (unsigned int k = 0, ii = 0; k < I->getHeight(); k += 1, ii += cScale) {
        for (unsigned int l = 0, jj = 0; l < I->getWidth(); l += 1, jj += cScale) {
          (*I)[k][l] = _I[ii][jj];
        }
      }
#endif
      _pyramid[i] = I;
    } else {
      _pyramid[i] = NULL;
    }
  }
}

void vpMbEdgeTracker::cleanPyramid(std::vector<const vpImage<unsigned char> *> &_pyramid)
{
  if (_pyramid.size() > 0) {
    _pyramid[0] = NULL;
    for (unsigned int i = 1; i < _pyramid.size(); i += 1) {
      if (_pyramid[i] != NULL) {
        delete _pyramid[i];
        _pyramid[i] = NULL;
      }
    }
    _pyramid.resize(0);
  }
}

void vpMbEdgeTracker::getLline(std::list<vpMbtDistanceLine *> &linesList, unsigned int level) const
{
  if (level > scales.size() || !scales[level]) {
    std::ostringstream oss;
    oss << level;
    std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
    throw vpException(vpException::dimensionError, errorMsg);
  }

  linesList = lines[level];
}

void vpMbEdgeTracker::getLcylinder(std::list<vpMbtDistanceCylinder *> &cylindersList, unsigned int level) const
{
  if (level > scales.size() || !scales[level]) {
    std::ostringstream oss;
    oss << level;
    std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
    throw vpException(vpException::dimensionError, errorMsg);
  }

  cylindersList = cylinders[level];
}

void vpMbEdgeTracker::getLcircle(std::list<vpMbtDistanceCircle *> &circlesList, unsigned int level) const
{
  if (level > scales.size() || !scales[level]) {
    std::ostringstream oss;
    oss << level;
    std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
    throw vpException(vpException::dimensionError, errorMsg);
  }

  circlesList = circles[level];
}

void vpMbEdgeTracker::downScale(const unsigned int _scale)
{
  const double ratio = pow(2., (int)_scale);
  scaleLevel = _scale;

  vpMatrix K = m_cam.get_K();

  K[0][0] /= ratio;
  K[1][1] /= ratio;
  K[0][2] /= ratio;
  K[1][2] /= ratio;

  m_cam.initFromCalibrationMatrix(K);
}

void vpMbEdgeTracker::upScale(const unsigned int _scale)
{
  const double ratio = pow(2., (int)_scale);
  scaleLevel = 0;

  vpMatrix K = m_cam.get_K();

  K[0][0] *= ratio;
  K[1][1] *= ratio;
  K[0][2] *= ratio;
  K[1][2] *= ratio;

  m_cam.initFromCalibrationMatrix(K);
}

void vpMbEdgeTracker::reInitLevel(const unsigned int _lvl)
{
  unsigned int scaleLevel_1 = scaleLevel;
  scaleLevel = _lvl;

  vpMbtDistanceLine *l;
  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
       ++it) {
    if ((*it)->isTracked()) {
      l = *it;
      l->reinitMovingEdge(*Ipyramid[_lvl], m_cMo, m_mask);
    }
  }

  vpMbtDistanceCylinder *cy;
  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
       it != cylinders[scaleLevel].end(); ++it) {
    if ((*it)->isTracked()) {
      cy = *it;
      cy->reinitMovingEdge(*Ipyramid[_lvl], m_cMo, m_mask);
    }
  }

  vpMbtDistanceCircle *ci;
  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
       it != circles[scaleLevel].end(); ++it) {
    if ((*it)->isTracked()) {
      ci = *it;
      ci->reinitMovingEdge(*Ipyramid[_lvl], m_cMo, m_mask);
    }
  }

  trackMovingEdge(*Ipyramid[_lvl]);
  updateMovingEdge(*Ipyramid[_lvl]);
  scaleLevel = scaleLevel_1;
}

void vpMbEdgeTracker::setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking)
{
  for (unsigned int i = 0; i < scales.size(); i += 1) {
    if (scales[i]) {
      for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
        /*(*it)->setTracked(useEdgeTracking);
        for(std::list<int>::const_iterator
        itpoly=(*it)->Lindex_polygon.begin();
        itpoly!=(*it)->Lindex_polygon.end(); ++itpoly){
          if(faces[(*itpoly)]->getName() != name){
            (*it)->setTracked(true);
            break;
          }
        }*/

        (*it)->setTracked(name, useEdgeTracking);
      }

      for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
           ++it) {
        if (faces[(unsigned)(*it)->index_polygon]->getName() == name) {
          (*it)->setTracked(useEdgeTracking);
        }
      }

      for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
        if (faces[(unsigned)(*it)->index_polygon]->getName() == name) {
          (*it)->setTracked(useEdgeTracking);
        }
      }
    }
  }
}