vpMbTracker.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:
 * Generic model based tracker
 *
 * Authors:
 * Romain Tallonneau
 * Aurelien Yol
 * Eric Marchand
 *
 *****************************************************************************/

#include <algorithm>
#include <iostream>
#include <limits>

#include <Simd/SimdLib.hpp>

#include <visp3/core/vpColVector.h>
#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpPoint.h>
#include <visp3/vision/vpPose.h>
#ifdef VISP_HAVE_MODULE_GUI
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#endif
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpColor.h>
#include <visp3/core/vpException.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpPixelMeterConversion.h>
#ifdef VISP_HAVE_MODULE_IO
#include <visp3/io/vpImageIo.h>
#endif
#include <visp3/core/vpCPUFeatures.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMatrixException.h>
#include <visp3/core/vpTrackingException.h>
#include <visp3/mbt/vpMbTracker.h>

#include <visp3/core/vpImageFilter.h>
#include <visp3/mbt/vpMbtXmlGenericParser.h>

#ifdef VISP_HAVE_COIN3D
// Inventor includes
#include <Inventor/VRMLnodes/SoVRMLCoordinate.h>
#include <Inventor/VRMLnodes/SoVRMLGroup.h>
#include <Inventor/VRMLnodes/SoVRMLIndexedFaceSet.h>
#include <Inventor/VRMLnodes/SoVRMLIndexedLineSet.h>
#include <Inventor/VRMLnodes/SoVRMLShape.h>
#include <Inventor/VRMLnodes/SoVRMLTransform.h>
#include <Inventor/actions/SoGetMatrixAction.h>
#include <Inventor/actions/SoGetPrimitiveCountAction.h>
#include <Inventor/actions/SoSearchAction.h>
#include <Inventor/actions/SoToVRML2Action.h>
#include <Inventor/actions/SoWriteAction.h>
#include <Inventor/misc/SoChildList.h>
#include <Inventor/nodes/SoSeparator.h>
#endif

#ifndef DOXYGEN_SHOULD_SKIP_THIS

namespace
{
struct SegmentInfo {
  SegmentInfo() : extremities(), name(), useLod(false), minLineLengthThresh(0.) {}

  std::vector<vpPoint> extremities;
  std::string name;
  bool useLod;
  double minLineLengthThresh;
};

struct PolygonFaceInfo {
  PolygonFaceInfo(double dist, const vpPolygon &poly, const std::vector<vpPoint> &corners)
    : distanceToCamera(dist), polygon(poly), faceCorners(corners)
  {
  }

  bool operator<(const PolygonFaceInfo &pfi) const { return distanceToCamera < pfi.distanceToCamera; }

  double distanceToCamera;
  vpPolygon polygon;
  std::vector<vpPoint> faceCorners;
};

std::istream& safeGetline(std::istream& is, std::string& t)
{
  t.clear();

  // The characters in the stream are read one-by-one using a std::streambuf.
  // That is faster than reading them one-by-one using the std::istream.
  // Code that uses streambuf this way must be guarded by a sentry object.
  // The sentry object performs various tasks,
  // such as thread synchronization and updating the stream state.

  std::istream::sentry se(is, true);
  std::streambuf* sb = is.rdbuf();

  for(;;) {
    int c = sb->sbumpc();
    if (c == '\n') {
      return is;
    }
    else if (c == '\r') {
      if(sb->sgetc() == '\n')
        sb->sbumpc();
      return is;
    }
    else if (c == std::streambuf::traits_type::eof()) {
      // Also handle the case when the last line has no line ending
      if(t.empty())
        is.setstate(std::ios::eofbit);
      return is;
    }
    else { // default case
      t += (char)c;
    }
  }
}
}
#endif // DOXYGEN_SHOULD_SKIP_THIS

vpMbTracker::vpMbTracker()
  : m_cam(), m_cMo(), oJo(6, 6), isoJoIdentity(true), modelFileName(), modelInitialised(false), poseSavingFilename(),
    computeCovariance(false), covarianceMatrix(), computeProjError(false), projectionError(90.0),
    displayFeatures(false), m_optimizationMethod(vpMbTracker::GAUSS_NEWTON_OPT), faces(), angleAppears(vpMath::rad(89)),
    angleDisappears(vpMath::rad(89)), distNearClip(0.001), distFarClip(100), clippingFlag(vpPolygon3D::NO_CLIPPING),
    useOgre(false), ogreShowConfigDialog(false), useScanLine(false), nbPoints(0), nbLines(0), nbPolygonLines(0),
    nbPolygonPoints(0), nbCylinders(0), nbCircles(0), useLodGeneral(false), applyLodSettingInConfig(false),
    minLineLengthThresholdGeneral(50.0), minPolygonAreaThresholdGeneral(2500.0), mapOfParameterNames(),
    m_computeInteraction(true), m_lambda(1.0), m_maxIter(30), m_stopCriteriaEpsilon(1e-8), m_initialMu(0.01),
    m_projectionErrorLines(), m_projectionErrorCylinders(), m_projectionErrorCircles(),
    m_projectionErrorFaces(), m_projectionErrorOgreShowConfigDialog(false),
    m_projectionErrorMe(), m_projectionErrorKernelSize(2), m_SobelX(5,5), m_SobelY(5,5),
    m_projectionErrorDisplay(false), m_projectionErrorDisplayLength(20), m_projectionErrorDisplayThickness(1),
    m_projectionErrorCam(), m_mask(NULL), m_I(), m_sodb_init_called(false), m_rand()
{
  oJo.eye();
  // Map used to parse additional information in CAO model files,
  // like name of faces or LOD setting
  mapOfParameterNames["name"] = "string";
  mapOfParameterNames["minPolygonAreaThreshold"] = "number";
  mapOfParameterNames["minLineLengthThreshold"] = "number";
  mapOfParameterNames["useLod"] = "boolean";

  vpImageFilter::getSobelKernelX(m_SobelX.data, m_projectionErrorKernelSize);
  vpImageFilter::getSobelKernelY(m_SobelY.data, m_projectionErrorKernelSize);
}

vpMbTracker::~vpMbTracker() {
  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin(); it != m_projectionErrorLines.end(); ++it) {
    vpMbtDistanceLine *l = *it;
    if (l != NULL)
      delete l;
    l = NULL;
  }

  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin(); it != m_projectionErrorCylinders.end(); ++it) {
    vpMbtDistanceCylinder *cy = *it;
    if (cy != NULL)
      delete cy;
    cy = NULL;
  }

  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin(); it != m_projectionErrorCircles.end(); ++it) {
    vpMbtDistanceCircle *ci = *it;
    if (ci != NULL)
      delete ci;
    ci = NULL;
  }
#if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION >= 2)
  if (m_sodb_init_called) {
    // Cleanup memory allocated by Coin library used to load a vrml model
    SoDB::finish();
  }
#endif
}

#ifdef VISP_HAVE_MODULE_GUI
void vpMbTracker::initClick(const vpImage<unsigned char> * const I, const vpImage<vpRGBa> * const I_color,
                            const std::string &initFile, bool displayHelp, const vpHomogeneousMatrix &T)
{
  vpHomogeneousMatrix last_cMo;
  vpPoseVector init_pos;
  vpImagePoint ip;
  vpMouseButton::vpMouseButtonType button = vpMouseButton::button1;

  std::string ext = ".init";
  std::string str_pose = "";
  size_t pos = initFile.rfind(ext);

  // Load the last poses from files
  std::fstream finitpos;
  std::ifstream finit;
  char s[FILENAME_MAX];
  if (poseSavingFilename.empty()) {
    if (pos != std::string::npos)
      str_pose = initFile.substr(0, pos) + ".0.pos";
    else
      str_pose = initFile + ".0.pos";

    finitpos.open(str_pose.c_str(), std::ios::in);
    sprintf(s, "%s", str_pose.c_str());
  } else {
    finitpos.open(poseSavingFilename.c_str(), std::ios::in);
    sprintf(s, "%s", poseSavingFilename.c_str());
  }
  if (finitpos.fail()) {
    std::cout << "cannot read " << s << std::endl << "cMo set to identity" << std::endl;
    last_cMo.eye();
  } else {
    for (unsigned int i = 0; i < 6; i += 1) {
      finitpos >> init_pos[i];
    }

    finitpos.close();
    last_cMo.buildFrom(init_pos);

    std::cout << "last_cMo : " << std::endl << last_cMo << std::endl;

    if (I) {
      vpDisplay::display(*I);
      display(*I, last_cMo, m_cam, vpColor::green, 1, true);
      vpDisplay::displayFrame(*I, last_cMo, m_cam, 0.05, vpColor::green);
      vpDisplay::flush(*I);
    } else {
      vpDisplay::display(*I_color);
      display(*I_color, last_cMo, m_cam, vpColor::green, 1, true);
      vpDisplay::displayFrame(*I_color, last_cMo, m_cam, 0.05, vpColor::green);
      vpDisplay::flush(*I_color);
    }

    std::cout << "No modification : left click " << std::endl;
    std::cout << "Modify initial pose : right click " << std::endl;

    if (I) {
      vpDisplay::displayText(*I, 15, 10, "left click to validate, right click to modify initial pose", vpColor::red);

      vpDisplay::flush(*I );

      while (!vpDisplay::getClick(*I, ip, button))
        ;
    } else {
      vpDisplay::displayText(*I_color, 15, 10, "left click to validate, right click to modify initial pose", vpColor::red);

      vpDisplay::flush(*I_color);

      while (!vpDisplay::getClick(*I_color, ip, button))
        ;
    }

  }

  if (!finitpos.fail() && button == vpMouseButton::button1) {
    m_cMo = last_cMo;
  } else {
    vpDisplay *d_help = NULL;

    if (I) {
      vpDisplay::display(*I);
      vpDisplay::flush(*I);
    }
    else {
      vpDisplay::display(*I_color);
      vpDisplay::flush(*I_color);
    }

    vpPose pose;

    pose.clearPoint();

    // file parser
    // number of points
    // X Y Z
    // X Y Z
    if (pos != std::string::npos)
      sprintf(s, "%s", initFile.c_str());
    else
      sprintf(s, "%s.init", initFile.c_str());

    std::cout << "Load 3D points from: " << s << std::endl;
    finit.open(s);
    if (finit.fail()) {
      std::cout << "cannot read " << s << std::endl;
      throw vpException(vpException::ioError, "Cannot open model-based tracker init file %s", s);
    }

#ifdef VISP_HAVE_MODULE_IO
    // Display window creation and initialisation
    try {
      if (displayHelp) {
        const std::string imgExtVec[] = {".ppm", ".pgm", ".jpg", ".jpeg", ".png"};
        std::string dispF;
        bool foundHelpImg = false;
        if (pos != std::string::npos) {
          for (size_t i = 0; i < 5 && !foundHelpImg; i++) {
            dispF = initFile.substr(0, pos) + imgExtVec[i];
            foundHelpImg = vpIoTools::checkFilename(dispF);
          }
        } else {
          for (size_t i = 0; i < 5 && !foundHelpImg; i++) {
            dispF = initFile + imgExtVec[i];
            foundHelpImg = vpIoTools::checkFilename(dispF);
          }
        }

        if (foundHelpImg) {
          std::cout << "Load image to help initialization: " << dispF << std::endl;
#if defined VISP_HAVE_X11
          d_help = new vpDisplayX;
#elif defined VISP_HAVE_GDI
          d_help = new vpDisplayGDI;
#elif defined VISP_HAVE_OPENCV
          d_help = new vpDisplayOpenCV;
#endif

          vpImage<vpRGBa> Iref;
          vpImageIo::read(Iref, dispF);
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
          const int winXPos = I != NULL ? I->display->getWindowXPosition() : I_color->display->getWindowXPosition();
          const int winYPos = I != NULL ? I->display->getWindowYPosition() : I_color->display->getWindowYPosition();
          unsigned int width = I != NULL ? I->getWidth() : I_color->getWidth();
          d_help->init(Iref, winXPos + (int)width + 80, winYPos,
                       "Where to initialize...");
          vpDisplay::display(Iref);
          vpDisplay::flush(Iref);
#endif
        }
      }
    } catch (...) {
      if (d_help != NULL) {
        delete d_help;
        d_help = NULL;
      }
    }
#else  //#ifdef VISP_HAVE_MODULE_IO
    (void)(displayHelp);
#endif //#ifdef VISP_HAVE_MODULE_IO
    // skip lines starting with # as comment
    removeComment(finit);

    unsigned int n3d;
    finit >> n3d;
    finit.ignore(256, '\n'); // skip the rest of the line
    std::cout << "Number of 3D points  " << n3d << std::endl;
    if (n3d > 100000) {
      throw vpException(vpException::badValue, "In %s file, the number of 3D points exceed the max allowed", s);
    }

    std::vector<vpPoint> P(n3d);
    for (unsigned int i = 0; i < n3d; i++) {
      // skip lines starting with # as comment
      removeComment(finit);

      vpColVector pt_3d(4, 1.0);
      finit >> pt_3d[0];
      finit >> pt_3d[1];
      finit >> pt_3d[2];
      finit.ignore(256, '\n'); // skip the rest of the line

      vpColVector pt_3d_tf = T*pt_3d;
      std::cout << "Point " << i + 1 << " with 3D coordinates: " << pt_3d_tf[0] << " " << pt_3d_tf[1] << " " << pt_3d_tf[2] << std::endl;

      P[i].setWorldCoordinates(pt_3d_tf[0], pt_3d_tf[1], pt_3d_tf[2]); // (X,Y,Z)
    }

    finit.close();

    bool isWellInit = false;
    while (!isWellInit) {
      std::vector<vpImagePoint> mem_ip;
      for (unsigned int i = 0; i < n3d; i++) {
        std::ostringstream text;
        text << "Click on point " << i + 1;
        if (I) {
          vpDisplay::display(*I);
          vpDisplay::displayText(*I, 15, 10, text.str(), vpColor::red);
          for (unsigned int k = 0; k < mem_ip.size(); k++) {
            vpDisplay::displayCross(*I, mem_ip[k], 10, vpColor::green, 2);
          }
          vpDisplay::flush(*I);
        } else {
          vpDisplay::display(*I_color);
          vpDisplay::displayText(*I_color, 15, 10, text.str(), vpColor::red);
          for (unsigned int k = 0; k < mem_ip.size(); k++) {
            vpDisplay::displayCross(*I_color, mem_ip[k], 10, vpColor::green, 2);
          }
          vpDisplay::flush(*I_color);
        }

        std::cout << "Click on point " << i + 1 << " ";
        double x = 0, y = 0;
        if (I) {
          vpDisplay::getClick(*I, ip);
          mem_ip.push_back(ip);
          vpDisplay::flush(*I);
        } else {
          vpDisplay::getClick(*I_color, ip);
          mem_ip.push_back(ip);
          vpDisplay::flush(*I_color);
        }
        vpPixelMeterConversion::convertPoint(m_cam, ip, x, y);
        P[i].set_x(x);
        P[i].set_y(y);

        std::cout << "with 2D coordinates: " << ip << std::endl;

        pose.addPoint(P[i]); // and added to the pose computation point list
      }
      if (I) {
        vpDisplay::flush(*I);
        vpDisplay::display(*I);
      } else {
        vpDisplay::flush(*I_color);
        vpDisplay::display(*I_color);
      }

      vpHomogeneousMatrix cMo1, cMo2;
      double d1, d2;
      d1 = d2 = std::numeric_limits<double>::max();
      try {
        pose.computePose(vpPose::LAGRANGE, cMo1);
        d1 = pose.computeResidual(cMo1);
      }
      catch(...) {
        // Lagrange non-planar cannot work with less than 6 points
      }
      try {
        pose.computePose(vpPose::DEMENTHON, cMo2);
        d2 = pose.computeResidual(cMo2);
      }
      catch(...) {
        // Should not occur
      }

      if (d1 < d2) {
        m_cMo = cMo1;
      } else {
        m_cMo = cMo2;
      }
      pose.computePose(vpPose::VIRTUAL_VS, m_cMo);

      if (I) {
        display(*I, m_cMo, m_cam, vpColor::green, 1, true);
        vpDisplay::displayText(*I, 15, 10, "left click to validate, right click to re initialize object", vpColor::red);

        vpDisplay::flush(*I);

        button = vpMouseButton::button1;
        while (!vpDisplay::getClick(*I, ip, button))
          ;

        if (button == vpMouseButton::button1) {
          isWellInit = true;
        } else {
          pose.clearPoint();
          vpDisplay::display(*I);
          vpDisplay::flush(*I);
        }
      } else {
        display(*I_color, m_cMo, m_cam, vpColor::green, 1, true);
        vpDisplay::displayText(*I_color, 15, 10, "left click to validate, right click to re initialize object", vpColor::red);

        vpDisplay::flush(*I_color);

        button = vpMouseButton::button1;
        while (!vpDisplay::getClick(*I_color, ip, button))
          ;

        if (button == vpMouseButton::button1) {
          isWellInit = true;
        } else {
          pose.clearPoint();
          vpDisplay::display(*I_color);
          vpDisplay::flush(*I_color);
        }
      }
    }
    if (I)
      vpDisplay::displayFrame(*I, m_cMo, m_cam, 0.05, vpColor::red);
    else
      vpDisplay::displayFrame(*I_color, m_cMo, m_cam, 0.05, vpColor::red);

    // save the pose into file
    if (poseSavingFilename.empty())
      savePose(str_pose);
    else
      savePose(poseSavingFilename);

    if (d_help != NULL) {
      delete d_help;
      d_help = NULL;
    }
  }

  std::cout << "cMo : " << std::endl << m_cMo << std::endl;

  if (I)
    init(*I);
  else {
    vpImageConvert::convert(*I_color, m_I);
    init(m_I);
  }
}

void vpMbTracker::initClick(const vpImage<unsigned char> &I, const std::string &initFile, bool displayHelp,
                            const vpHomogeneousMatrix &T)
{
  initClick(&I, NULL, initFile, displayHelp, T);
}

void vpMbTracker::initClick(const vpImage<vpRGBa> &I_color, const std::string &initFile, bool displayHelp,
                            const vpHomogeneousMatrix &T)
{
  initClick(NULL, &I_color, initFile, displayHelp, T);
}

void vpMbTracker::initClick(const vpImage<unsigned char> * const I, const vpImage<vpRGBa> * const I_color,
                            const std::vector<vpPoint> &points3D_list, const std::string &displayFile)
{
  if (I) {
    vpDisplay::display(*I);
    vpDisplay::flush(*I);
  } else {
    vpDisplay::display(*I_color);
    vpDisplay::flush(*I_color);
  }

  vpDisplay *d_help = NULL;

  vpPose pose;
  std::vector<vpPoint> P;
  for (unsigned int i = 0; i < points3D_list.size(); i++)
    P.push_back(vpPoint(points3D_list[i].get_oX(), points3D_list[i].get_oY(), points3D_list[i].get_oZ()));

#ifdef VISP_HAVE_MODULE_IO
  vpImage<vpRGBa> Iref;
  // Display window creation and initialisation
  if (vpIoTools::checkFilename(displayFile)) {
    try {
      std::cout << "Load image to help initialization: " << displayFile << std::endl;
#if defined VISP_HAVE_X11
      d_help = new vpDisplayX;
#elif defined VISP_HAVE_GDI
      d_help = new vpDisplayGDI;
#elif defined VISP_HAVE_OPENCV
      d_help = new vpDisplayOpenCV;
#endif

      vpImageIo::read(Iref, displayFile);
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
      if (I) {
        d_help->init(Iref, I->display->getWindowXPosition() + (int)I->getWidth() + 80, I->display->getWindowYPosition(),
                     "Where to initialize...");
      } else {
        d_help->init(Iref, I_color->display->getWindowXPosition() + (int)I_color->getWidth() + 80, I_color->display->getWindowYPosition(),
                     "Where to initialize...");
      }
      vpDisplay::display(Iref);
      vpDisplay::flush(Iref);
#endif
    } catch (...) {
      if (d_help != NULL) {
        delete d_help;
        d_help = NULL;
      }
    }
  }
#else  //#ifdef VISP_HAVE_MODULE_IO
  (void)(displayFile);
#endif //#ifdef VISP_HAVE_MODULE_IO

  vpImagePoint ip;
  bool isWellInit = false;
  while (!isWellInit) {
    for (unsigned int i = 0; i < points3D_list.size(); i++) {
      std::cout << "Click on point " << i + 1 << std::endl;
      double x = 0, y = 0;
      if (I) {
        vpDisplay::getClick(*I, ip);
        vpDisplay::displayCross(*I, ip, 5, vpColor::green);
        vpDisplay::flush(*I);
      } else {
        vpDisplay::getClick(*I_color, ip);
        vpDisplay::displayCross(*I_color, ip, 5, vpColor::green);
        vpDisplay::flush(*I_color);
      }
      vpPixelMeterConversion::convertPoint(m_cam, ip, x, y);
      P[i].set_x(x);
      P[i].set_y(y);

      std::cout << "Click on point " << ip << std::endl;

      if (I) {
        vpDisplay::displayPoint(*I, ip, vpColor::green); // display target point
      } else {
        vpDisplay::displayPoint(*I_color, ip, vpColor::green); // display target point
      }
      pose.addPoint(P[i]);                            // and added to the pose computation point list
    }
    if (I) {
      vpDisplay::flush(*I);
    } else {
      vpDisplay::flush(*I_color);
    }

    vpHomogeneousMatrix cMo1, cMo2;
    double d1, d2;
    d1 = d2 = std::numeric_limits<double>::max();
    try {
      pose.computePose(vpPose::LAGRANGE, cMo1);
      d1 = pose.computeResidual(cMo1);
    }
    catch(...) {
      // Lagrange non-planar cannot work with less than 6 points
    }
    try {
      pose.computePose(vpPose::DEMENTHON, cMo2);
      d2 = pose.computeResidual(cMo2);
    }
    catch(...) {
      // Should not occur
    }

    if (d1 < d2) {
      m_cMo = cMo1;
    } else {
      m_cMo = cMo2;
    }
    pose.computePose(vpPose::VIRTUAL_VS, m_cMo);

    if (I) {
      display(*I, m_cMo, m_cam, vpColor::green, 1, true);
      vpDisplay::displayText(*I, 15, 10, "left click to validate, right click to re initialize object", vpColor::red);

      vpDisplay::flush(*I);

      vpMouseButton::vpMouseButtonType button = vpMouseButton::button1;
      while (!vpDisplay::getClick(*I, ip, button)) {
      };

      if (button == vpMouseButton::button1) {
        isWellInit = true;
      } else {
        pose.clearPoint();
        vpDisplay::display(*I);
        vpDisplay::flush(*I);
      }
    } else {
      display(*I_color, m_cMo, m_cam, vpColor::green, 1, true);
      vpDisplay::displayText(*I_color, 15, 10, "left click to validate, right click to re initialize object", vpColor::red);

      vpDisplay::flush(*I_color);

      vpMouseButton::vpMouseButtonType button = vpMouseButton::button1;
      while (!vpDisplay::getClick(*I_color, ip, button)) {
      };

      if (button == vpMouseButton::button1) {
        isWellInit = true;
      } else {
        pose.clearPoint();
        vpDisplay::display(*I_color);
        vpDisplay::flush(*I_color);
      }
    }
  }

  if (I) {
    vpDisplay::displayFrame(*I, m_cMo, m_cam, 0.05, vpColor::red);
  } else {
    vpDisplay::displayFrame(*I_color, m_cMo, m_cam, 0.05, vpColor::red);
  }

  if (d_help != NULL) {
    delete d_help;
    d_help = NULL;
  }

  if (I)
    init(*I);
  else {
    vpImageConvert::convert(*I_color, m_I);
    init(m_I);
  }
}

void vpMbTracker::initClick(const vpImage<unsigned char> &I, const std::vector<vpPoint> &points3D_list,
                            const std::string &displayFile)
{
  initClick(&I, NULL, points3D_list, displayFile);
}

void vpMbTracker::initClick(const vpImage<vpRGBa> &I_color, const std::vector<vpPoint> &points3D_list,
                            const std::string &displayFile)
{
  initClick(NULL, &I_color, points3D_list, displayFile);
}
#endif //#ifdef VISP_HAVE_MODULE_GUI

void vpMbTracker::initFromPoints(const vpImage<unsigned char> * const I, const vpImage<vpRGBa> * const I_color,
                                 const std::string &initFile)
{
  char s[FILENAME_MAX];
  std::fstream finit;

  std::string ext = ".init";
  size_t pos = initFile.rfind(ext);

  if (pos == initFile.size() - ext.size() && pos != 0)
    sprintf(s, "%s", initFile.c_str());
  else
    sprintf(s, "%s.init", initFile.c_str());

  std::cout << "Load 2D/3D points from: " << s << std::endl;
  finit.open(s, std::ios::in);
  if (finit.fail()) {
    std::cout << "cannot read " << s << std::endl;
    throw vpException(vpException::ioError, "Cannot open model-based tracker init file %s", s);
  }

  //********
  // Read 3D points coordinates
  //********
  char c;
  // skip lines starting with # as comment
  finit.get(c);
  while (!finit.fail() && (c == '#')) {
    finit.ignore(256, '\n');
    finit.get(c);
  }
  finit.unget();

  unsigned int n3d;
  finit >> n3d;
  finit.ignore(256, '\n'); // skip the rest of the line
  std::cout << "Number of 3D points  " << n3d << std::endl;
  if (n3d > 100000) {
    throw vpException(vpException::badValue, "In %s file, the number of 3D points exceed the max allowed", s);
  }

  vpPoint *P = new vpPoint[n3d];
  for (unsigned int i = 0; i < n3d; i++) {
    // skip lines starting with # as comment
    finit.get(c);
    while (!finit.fail() && (c == '#')) {
      finit.ignore(256, '\n');
      finit.get(c);
    }
    finit.unget();
    double X, Y, Z;
    finit >> X;
    finit >> Y;
    finit >> Z;
    finit.ignore(256, '\n'); // skip the rest of the line

    std::cout << "Point " << i + 1 << " with 3D coordinates: " << X << " " << Y << " " << Z << std::endl;
    P[i].setWorldCoordinates(X, Y, Z); // (X,Y,Z)
  }

  //********
  // Read 3D points coordinates
  //********
  // skip lines starting with # as comment
  finit.get(c);
  while (!finit.fail() && (c == '#')) {
    finit.ignore(256, '\n');
    finit.get(c);
  }
  finit.unget();

  unsigned int n2d;
  finit >> n2d;
  finit.ignore(256, '\n'); // skip the rest of the line
  std::cout << "Number of 2D points  " << n2d << std::endl;
  if (n2d > 100000) {
    delete[] P;
    throw vpException(vpException::badValue, "In %s file, the number of 2D points exceed the max allowed", s);
  }

  if (n3d != n2d) {
    delete[] P;
    throw vpException(vpException::badValue,
                      "In %s file, number of 2D points %d and number of 3D "
                      "points %d are not equal",
                      s, n2d, n3d);
  }

  vpPose pose;
  for (unsigned int i = 0; i < n2d; i++) {
    // skip lines starting with # as comment
    finit.get(c);
    while (!finit.fail() && (c == '#')) {
      finit.ignore(256, '\n');
      finit.get(c);
    }
    finit.unget();
    double u, v, x = 0, y = 0;
    finit >> v;
    finit >> u;
    finit.ignore(256, '\n'); // skip the rest of the line

    vpImagePoint ip(v, u);
    std::cout << "Point " << i + 1 << " with 2D coordinates: " << ip << std::endl;
    vpPixelMeterConversion::convertPoint(m_cam, ip, x, y);
    P[i].set_x(x);
    P[i].set_y(y);
    pose.addPoint(P[i]);
  }

  finit.close();

  vpHomogeneousMatrix cMo1, cMo2;
  double d1, d2;
  d1 = d2 = std::numeric_limits<double>::max();
  try {
    pose.computePose(vpPose::LAGRANGE, cMo1);
    d1 = pose.computeResidual(cMo1);
  }
  catch(...) {
    // Lagrange non-planar cannot work with less than 6 points
  }
  try {
    pose.computePose(vpPose::DEMENTHON, cMo2);
    d2 = pose.computeResidual(cMo2);
  }
  catch(...) {
    // Should not occur
  }

  if (d1 < d2)
    m_cMo = cMo1;
  else
    m_cMo = cMo2;

  pose.computePose(vpPose::VIRTUAL_VS, m_cMo);

  delete[] P;

  if (I) {
    init(*I);
  } else {
    vpImageConvert::convert(*I_color, m_I);
    init(m_I);
  }
}

void vpMbTracker::initFromPoints(const vpImage<unsigned char> &I, const std::string &initFile)
{
  initFromPoints(&I, NULL, initFile);
}

void vpMbTracker::initFromPoints(const vpImage<vpRGBa> &I_color, const std::string &initFile)
{
  initFromPoints(NULL, &I_color, initFile);
}

void vpMbTracker::initFromPoints(const vpImage<unsigned char> * const I, const vpImage<vpRGBa> * const I_color,
                                 const std::vector<vpImagePoint> &points2D_list, const std::vector<vpPoint> &points3D_list)
{
  if (points2D_list.size() != points3D_list.size())
    vpERROR_TRACE("vpMbTracker::initFromPoints(), Number of 2D points "
                  "different to the number of 3D points.");

  size_t size = points3D_list.size();
  std::vector<vpPoint> P;
  vpPose pose;

  for (size_t i = 0; i < size; i++) {
    P.push_back(vpPoint(points3D_list[i].get_oX(), points3D_list[i].get_oY(), points3D_list[i].get_oZ()));
    double x = 0, y = 0;
    vpPixelMeterConversion::convertPoint(m_cam, points2D_list[i], x, y);
    P[i].set_x(x);
    P[i].set_y(y);
    pose.addPoint(P[i]);
  }

  vpHomogeneousMatrix cMo1, cMo2;
  double d1, d2;
  d1 = d2 = std::numeric_limits<double>::max();
  try {
    pose.computePose(vpPose::LAGRANGE, cMo1);
    d1 = pose.computeResidual(cMo1);
  }
  catch(...) {
    // Lagrange non-planar cannot work with less than 6 points
  }
  try {
    pose.computePose(vpPose::DEMENTHON, cMo2);
    d2 = pose.computeResidual(cMo2);
  }
  catch(...) {
    // Should not occur
  }

  if (d1 < d2)
    m_cMo = cMo1;
  else
    m_cMo = cMo2;

  pose.computePose(vpPose::VIRTUAL_VS, m_cMo);

  if (I) {
    init(*I);
  } else {
    vpImageConvert::convert(*I_color, m_I);
    init(m_I);
  }
}

void vpMbTracker::initFromPoints(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &points2D_list,
                                 const std::vector<vpPoint> &points3D_list)
{
  initFromPoints(&I, NULL, points2D_list, points3D_list);
}

void vpMbTracker::initFromPoints(const vpImage<vpRGBa> &I_color, const std::vector<vpImagePoint> &points2D_list,
                                 const std::vector<vpPoint> &points3D_list)
{
  initFromPoints(NULL, &I_color, points2D_list, points3D_list);
}

void vpMbTracker::initFromPose(const vpImage<unsigned char> * const I, const vpImage<vpRGBa> * const I_color,
                               const std::string &initFile)
{
  char s[FILENAME_MAX];
  std::fstream finit;
  vpPoseVector init_pos;

  std::string ext = ".pos";
  size_t pos = initFile.rfind(ext);

  if (pos == initFile.size() - ext.size() && pos != 0)
    sprintf(s, "%s", initFile.c_str());
  else
    sprintf(s, "%s.pos", initFile.c_str());

  finit.open(s, std::ios::in);
  if (finit.fail()) {
    std::cout << "cannot read " << s << std::endl;
    throw vpException(vpException::ioError, "cannot read init file");
  }

  for (unsigned int i = 0; i < 6; i += 1) {
    finit >> init_pos[i];
  }

  m_cMo.buildFrom(init_pos);

  if (I) {
    init(*I);
  } else {
    vpImageConvert::convert(*I_color, m_I);
    init(m_I);
  }
}

void vpMbTracker::initFromPose(const vpImage<unsigned char> &I, const std::string &initFile)
{
  initFromPose(&I, NULL, initFile);
}

void vpMbTracker::initFromPose(const vpImage<vpRGBa> &I_color, const std::string &initFile)
{
  initFromPose(NULL, &I_color, initFile);
}

void vpMbTracker::initFromPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
{
  m_cMo = cMo;
  init(I);
}

void vpMbTracker::initFromPose(const vpImage<vpRGBa> &I_color, const vpHomogeneousMatrix &cMo)
{
  m_cMo = cMo;
  vpImageConvert::convert(I_color, m_I);
  init(m_I);
}

void vpMbTracker::initFromPose(const vpImage<unsigned char> &I, const vpPoseVector &cPo)
{
  vpHomogeneousMatrix _cMo(cPo);
  initFromPose(I, _cMo);
}

void vpMbTracker::initFromPose(const vpImage<vpRGBa> &I_color, const vpPoseVector &cPo)
{
  vpHomogeneousMatrix _cMo(cPo);
  vpImageConvert::convert(I_color, m_I);
  initFromPose(m_I, _cMo);
}

void vpMbTracker::savePose(const std::string &filename) const
{
  vpPoseVector init_pos;
  std::fstream finitpos;
  char s[FILENAME_MAX];

  sprintf(s, "%s", filename.c_str());
  finitpos.open(s, std::ios::out);

  init_pos.buildFrom(m_cMo);
  finitpos << init_pos;
  finitpos.close();
}

void vpMbTracker::addPolygon(const std::vector<vpPoint> &corners, int idFace, const std::string &polygonName,
                             bool useLod, double minPolygonAreaThreshold,
                             double minLineLengthThreshold)
{
  std::vector<vpPoint> corners_without_duplicates;
  corners_without_duplicates.push_back(corners[0]);
  for (unsigned int i = 0; i < corners.size() - 1; i++) {
    if (std::fabs(corners[i].get_oX() - corners[i + 1].get_oX()) >
            std::fabs(corners[i].get_oX()) * std::numeric_limits<double>::epsilon() ||
        std::fabs(corners[i].get_oY() - corners[i + 1].get_oY()) >
            std::fabs(corners[i].get_oY()) * std::numeric_limits<double>::epsilon() ||
        std::fabs(corners[i].get_oZ() - corners[i + 1].get_oZ()) >
            std::fabs(corners[i].get_oZ()) * std::numeric_limits<double>::epsilon()) {
      corners_without_duplicates.push_back(corners[i + 1]);
    }
  }

  vpMbtPolygon polygon;
  polygon.setNbPoint((unsigned int)corners_without_duplicates.size());
  polygon.setIndex((int)idFace);
  polygon.setName(polygonName);
  polygon.setLod(useLod);

  //  //if(minPolygonAreaThreshold != -1.0) {
  //  if(std::fabs(minPolygonAreaThreshold + 1.0) >
  //  std::fabs(minPolygonAreaThreshold)*std::numeric_limits<double>::epsilon())
  //  {
  //    polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
  //  }
  //
  //  //if(minLineLengthThreshold != -1.0) {
  //  if(std::fabs(minLineLengthThreshold + 1.0) >
  //  std::fabs(minLineLengthThreshold)*std::numeric_limits<double>::epsilon())
  //  {
  //    polygon.setMinLineLengthThresh(minLineLengthThreshold);
  //  }

  polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
  polygon.setMinLineLengthThresh(minLineLengthThreshold);

  for (unsigned int j = 0; j < corners_without_duplicates.size(); j++) {
    polygon.addPoint(j, corners_without_duplicates[j]);
  }

  faces.addPolygon(&polygon);

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

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

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

void vpMbTracker::addPolygon(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius,
                             int idFace, const std::string &polygonName, bool useLod,
                             double minPolygonAreaThreshold)
{
  vpMbtPolygon polygon;
  polygon.setNbPoint(4);
  polygon.setName(polygonName);
  polygon.setLod(useLod);

  //    //if(minPolygonAreaThreshold != -1.0) {
  //    if(std::fabs(minPolygonAreaThreshold + 1.0) >
  //    std::fabs(minPolygonAreaThreshold)*std::numeric_limits<double>::epsilon())
  //    {
  //      polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
  //    }
  polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
  // Non sense to set minLineLengthThreshold for circle
  // but used to be coherent when applying LOD settings for all polygons
  polygon.setMinLineLengthThresh(minLineLengthThresholdGeneral);

  {
    // Create the 4 points of the circle bounding box
    vpPlane plane(p1, p2, p3, vpPlane::object_frame);

    // Matrice de passage entre world et circle frame
    double norm_X = sqrt(vpMath::sqr(p2.get_oX() - p1.get_oX()) + vpMath::sqr(p2.get_oY() - p1.get_oY()) +
                         vpMath::sqr(p2.get_oZ() - p1.get_oZ()));
    double norm_Y = sqrt(vpMath::sqr(plane.getA()) + vpMath::sqr(plane.getB()) + vpMath::sqr(plane.getC()));
    vpRotationMatrix wRc;
    vpColVector x(3), y(3), z(3);
    // X axis is P2-P1
    x[0] = (p2.get_oX() - p1.get_oX()) / norm_X;
    x[1] = (p2.get_oY() - p1.get_oY()) / norm_X;
    x[2] = (p2.get_oZ() - p1.get_oZ()) / norm_X;
    // Y axis is the normal of the plane
    y[0] = plane.getA() / norm_Y;
    y[1] = plane.getB() / norm_Y;
    y[2] = plane.getC() / norm_Y;
    // Z axis = X ^ Y
    z = vpColVector::crossProd(x, y);
    for (unsigned int i = 0; i < 3; i++) {
      wRc[i][0] = x[i];
      wRc[i][1] = y[i];
      wRc[i][2] = z[i];
    }

    vpTranslationVector wtc(p1.get_oX(), p1.get_oY(), p1.get_oZ());
    vpHomogeneousMatrix wMc(wtc, wRc);

    vpColVector c_p(4); // A point in the circle frame that is on the bbox
    c_p[0] = radius;
    c_p[1] = 0;
    c_p[2] = radius;
    c_p[3] = 1;

    // Matrix to rotate a point by 90 deg around Y in the circle frame
    for (unsigned int i = 0; i < 4; i++) {
      vpColVector w_p(4); // A point in the word frame
      vpHomogeneousMatrix cMc_90(vpTranslationVector(), vpRotationMatrix(0, vpMath::rad(90 * i), 0));
      w_p = wMc * cMc_90 * c_p;

      vpPoint w_P;
      w_P.setWorldCoordinates(w_p[0], w_p[1], w_p[2]);

      polygon.addPoint(i, w_P);
    }
  }

  polygon.setIndex(idFace);
  faces.addPolygon(&polygon);

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

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

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

void vpMbTracker::addPolygon(const vpPoint &p1, const vpPoint &p2, int idFace, const std::string &polygonName,
                             bool useLod, double minLineLengthThreshold)
{
  // A polygon as a single line that corresponds to the revolution axis of the
  // cylinder
  vpMbtPolygon polygon;
  polygon.setNbPoint(2);

  polygon.addPoint(0, p1);
  polygon.addPoint(1, p2);

  polygon.setIndex(idFace);
  polygon.setName(polygonName);
  polygon.setLod(useLod);

  //  //if(minLineLengthThreshold != -1.0) {
  //  if(std::fabs(minLineLengthThreshold + 1.0) >
  //  std::fabs(minLineLengthThreshold)*std::numeric_limits<double>::epsilon())
  //  {
  //    polygon.setMinLineLengthThresh(minLineLengthThreshold);
  //  }
  polygon.setMinLineLengthThresh(minLineLengthThreshold);
  // Non sense to set minPolygonAreaThreshold for cylinder
  // but used to be coherent when applying LOD settings for all polygons
  polygon.setMinPolygonAreaThresh(minPolygonAreaThresholdGeneral);

  faces.addPolygon(&polygon);

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

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

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

void vpMbTracker::addPolygon(const std::vector<std::vector<vpPoint> > &listFaces, int idFace,
                             const std::string &polygonName, bool useLod, double minLineLengthThreshold)
{
  int id = idFace;
  for (unsigned int i = 0; i < listFaces.size(); i++) {
    vpMbtPolygon polygon;
    polygon.setNbPoint((unsigned int)listFaces[i].size());
    for (unsigned int j = 0; j < listFaces[i].size(); j++)
      polygon.addPoint(j, listFaces[i][j]);

    polygon.setIndex(id);
    polygon.setName(polygonName);
    polygon.setIsPolygonOriented(false);
    polygon.setLod(useLod);
    polygon.setMinLineLengthThresh(minLineLengthThreshold);
    polygon.setMinPolygonAreaThresh(minPolygonAreaThresholdGeneral);

    faces.addPolygon(&polygon);

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

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

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

    id++;
  }
}

void vpMbTracker::loadModel(const std::string &modelFile, bool verbose, const vpHomogeneousMatrix &odTo)
{
  std::string::const_iterator it;

  if (vpIoTools::checkFilename(modelFile)) {
    it = modelFile.end();
    if ((*(it - 1) == 'o' && *(it - 2) == 'a' && *(it - 3) == 'c' && *(it - 4) == '.') ||
        (*(it - 1) == 'O' && *(it - 2) == 'A' && *(it - 3) == 'C' && *(it - 4) == '.')) {
      std::vector<std::string> vectorOfModelFilename;
      int startIdFace = (int)faces.size();
      nbPoints = 0;
      nbLines = 0;
      nbPolygonLines = 0;
      nbPolygonPoints = 0;
      nbCylinders = 0;
      nbCircles = 0;
      loadCAOModel(modelFile, vectorOfModelFilename, startIdFace, verbose, true, odTo);
    } else if ((*(it - 1) == 'l' && *(it - 2) == 'r' && *(it - 3) == 'w' && *(it - 4) == '.') ||
               (*(it - 1) == 'L' && *(it - 2) == 'R' && *(it - 3) == 'W' && *(it - 4) == '.')) {
      loadVRMLModel(modelFile);
    } else {
      throw vpException(vpException::ioError, "Error: File %s doesn't contain a cao or wrl model", modelFile.c_str());
    }
  } else {
    throw vpException(vpException::ioError, "Error: File %s doesn't exist", modelFile.c_str());
  }

  this->modelInitialised = true;
  this->modelFileName = modelFile;
}

void vpMbTracker::loadVRMLModel(const std::string &modelFile)
{
#ifdef VISP_HAVE_COIN3D
  m_sodb_init_called = true;
  SoDB::init(); // Call SoDB::finish() before ending the program.

  SoInput in;
  SbBool ok = in.openFile(modelFile.c_str());
  SoVRMLGroup *sceneGraphVRML2;

  if (!ok) {
    vpERROR_TRACE("can't open file to load model");
    throw vpException(vpException::fatalError, "can't open file to load model");
  }

  if (!in.isFileVRML2()) {
    SoSeparator *sceneGraph = SoDB::readAll(&in);
    if (sceneGraph == NULL) { /*return -1;*/
    }
    sceneGraph->ref();

    SoToVRML2Action tovrml2;
    tovrml2.apply(sceneGraph);

    sceneGraphVRML2 = tovrml2.getVRML2SceneGraph();
    sceneGraphVRML2->ref();
    sceneGraph->unref();
  } else {
    sceneGraphVRML2 = SoDB::readAllVRML(&in);
    if (sceneGraphVRML2 == NULL) { /*return -1;*/
    }
    sceneGraphVRML2->ref();
  }

  in.closeFile();

  vpHomogeneousMatrix transform;
  int indexFace = (int)faces.size();
  extractGroup(sceneGraphVRML2, transform, indexFace);

  sceneGraphVRML2->unref();
#else
  vpERROR_TRACE("coin not detected with ViSP, cannot load model : %s", modelFile.c_str());
  throw vpException(vpException::fatalError, "coin not detected with ViSP, cannot load model");
#endif
}

void vpMbTracker::removeComment(std::ifstream &fileId)
{
  char c;

  fileId.get(c);
  while (!fileId.fail() && (c == '#')) {
    fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n'));
    fileId.get(c);
  }
  if (fileId.fail()) {
    throw(vpException(vpException::ioError, "Reached end of file"));
  }
  fileId.unget();
}

std::map<std::string, std::string> vpMbTracker::parseParameters(std::string &endLine)
{
  std::map<std::string, std::string> mapOfParams;

  bool exit = false;
  while (!endLine.empty() && !exit) {
    exit = true;

    for (std::map<std::string, std::string>::const_iterator it = mapOfParameterNames.begin();
         it != mapOfParameterNames.end(); ++it) {
      endLine = vpIoTools::trim(endLine);
      std::string param(it->first + "=");

      // Compare with a potential parameter
      if (endLine.compare(0, param.size(), param) == 0) {
        exit = false;
        endLine = endLine.substr(param.size());

        bool parseQuote = false;
        if (it->second == "string") {
          // Check if the string is between quotes
          if (endLine.size() > 2 && endLine[0] == '"') {
            parseQuote = true;
            endLine = endLine.substr(1);
            size_t pos = endLine.find_first_of('"');

            if (pos != std::string::npos) {
              mapOfParams[it->first] = endLine.substr(0, pos);
              endLine = endLine.substr(pos + 1);
            } else {
              parseQuote = false;
            }
          }
        }

        if (!parseQuote) {
          // Deal with space or tabulation after parameter value to substring
          // to the next sequence
          size_t pos1 = endLine.find_first_of(' ');
          size_t pos2 = endLine.find_first_of('\t');
          size_t pos = pos1 < pos2 ? pos1 : pos2;

          mapOfParams[it->first] = endLine.substr(0, pos);
          endLine = endLine.substr(pos + 1);
        }
      }
    }
  }

  return mapOfParams;
}

void vpMbTracker::loadCAOModel(const std::string &modelFile, std::vector<std::string> &vectorOfModelFilename,
                               int &startIdFace, bool verbose, bool parent,
                               const vpHomogeneousMatrix &odTo)
{
  std::ifstream fileId;
  fileId.exceptions(std::ifstream::failbit | std::ifstream::eofbit);
  fileId.open(modelFile.c_str(), std::ifstream::in);
  if (fileId.fail()) {
    std::cout << "cannot read CAO model file: " << modelFile << std::endl;
    throw vpException(vpException::ioError, "cannot read CAO model file");
  }

  if (verbose) {
    std::cout << "Model file : " << modelFile << std::endl;
  }
  vectorOfModelFilename.push_back(modelFile);

  try {
    char c;
    // Extraction of the version (remove empty line and commented ones
    // (comment line begin with the #)).
    // while ((fileId.get(c) != NULL) && (c == '#')) fileId.ignore(256, '\n');
    removeComment(fileId);

    int caoVersion;
    fileId.get(c);
    if (c == 'V') {
      fileId >> caoVersion;
      fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n'));  // skip the rest of the line
    } else {
      std::cout << "in vpMbTracker::loadCAOModel() -> Bad parameter header "
                   "file : use V0, V1, ...";
      throw vpException(vpException::badValue, "in vpMbTracker::loadCAOModel() -> Bad parameter "
                                               "header file : use V0, V1, ...");
    }

    removeComment(fileId);

    std::string line;
    const std::string prefix_load = "load";

    fileId.get(c);
    fileId.unget();
    bool header = false;
    while (c == 'l' || c == 'L') {
      getline(fileId, line);

      if (!line.compare(0, prefix_load.size(), prefix_load)) {
        //remove "load("
        std::string paramsStr = line.substr(5);
        //get parameters inside load()
        paramsStr = paramsStr.substr(0, paramsStr.find_first_of(")"));
        //split by comma
        std::vector<std::string> params = vpIoTools::splitChain(paramsStr, ",");
        //remove whitespaces
        for (size_t i = 0; i < params.size(); i++) {
          params[i] = vpIoTools::trim(params[i]);
        }

        if (!params.empty()) {
          // Get the loaded model pathname
          std::string headerPathRead = params[0];
          headerPathRead = headerPathRead.substr(1);
          headerPathRead = headerPathRead.substr(0, headerPathRead.find_first_of("\""));

          std::string headerPath = headerPathRead;
          if (!vpIoTools::isAbsolutePathname(headerPathRead)) {
            std::string parentDirectory = vpIoTools::getParent(modelFile);
            headerPath = vpIoTools::createFilePath(parentDirectory, headerPathRead);
          }

          // Normalize path
          headerPath = vpIoTools::path(headerPath);

          // Get real path
          headerPath = vpIoTools::getAbsolutePathname(headerPath);

          vpHomogeneousMatrix oTo_local;
          vpTranslationVector t;
          vpThetaUVector tu;
          for (size_t i = 1; i < params.size(); i++) {
            std::string param = params[i];
            {
              const std::string prefix = "t=[";
              if (!param.compare(0, prefix.size(), prefix)) {
                param = param.substr(prefix.size());
                param = param.substr(0, param.find_first_of("]"));

                std::vector<std::string> values = vpIoTools::splitChain(param, ";");
                if (values.size() == 3) {
                  t[0] = atof(values[0].c_str());
                  t[1] = atof(values[1].c_str());
                  t[2] = atof(values[2].c_str());
                }
              }
            }
            {
              const std::string prefix = "tu=[";
              if (!param.compare(0, prefix.size(), prefix)) {
                param = param.substr(prefix.size());
                param = param.substr(0, param.find_first_of("]"));

                std::vector<std::string> values = vpIoTools::splitChain(param, ";");
                if (values.size() == 3) {
                  for (size_t j = 0; j < values.size(); j++) {
                    std::string value = values[j];
                    bool radian = true;
                    size_t unitPos = value.find("deg");
                    if (unitPos != std::string::npos) {
                      value = value.substr(0, unitPos);
                      radian = false;
                    }

                    unitPos = value.find("rad");
                    if (unitPos != std::string::npos) {
                      value = value.substr(0, unitPos);
                    }
                    tu[static_cast<unsigned int>(j)] = !radian ? vpMath::rad(atof(value.c_str())) : atof(value.c_str());
                  }
                }
              }
            }
          }
          oTo_local.buildFrom(t, tu);

          bool cyclic = false;
          for (std::vector<std::string>::const_iterator it = vectorOfModelFilename.begin();
               it != vectorOfModelFilename.end() && !cyclic; ++it) {
            if (headerPath == *it) {
              cyclic = true;
            }
          }

          if (!cyclic) {
            if (vpIoTools::checkFilename(headerPath)) {
              header = true;
              loadCAOModel(headerPath, vectorOfModelFilename, startIdFace, verbose, false, odTo*oTo_local);
            } else {
              throw vpException(vpException::ioError, "file cannot be open");
            }
          } else {
            std::cout << "WARNING Cyclic dependency detected with file " << headerPath << " declared in " << modelFile
                      << std::endl;
          }
        }
      }

      removeComment(fileId);
      fileId.get(c);
      fileId.unget();
    }

    unsigned int caoNbrPoint;
    fileId >> caoNbrPoint;
    fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line

    nbPoints += caoNbrPoint;
    if (verbose || (parent && !header)) {
      std::cout << "> " << caoNbrPoint << " points" << std::endl;
    }

    if (caoNbrPoint > 100000) {
      throw vpException(vpException::badValue, "Exceed the max number of points in the CAO model.");
    }

    if (caoNbrPoint == 0 && !header) {
      throw vpException(vpException::badValue, "in vpMbTracker::loadCAOModel() -> no points are defined");
    }
    vpPoint *caoPoints = new vpPoint[caoNbrPoint];

    int i; // image coordinate (used for matching)
    int j;

    for (unsigned int k = 0; k < caoNbrPoint; k++) {
      removeComment(fileId);

      vpColVector pt_3d(4, 1.0);
      fileId >> pt_3d[0];
      fileId >> pt_3d[1];
      fileId >> pt_3d[2];

      if (caoVersion == 2) {
        fileId >> i;
        fileId >> j;
      }

      fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line

      vpColVector pt_3d_tf = odTo*pt_3d;
      caoPoints[k].setWorldCoordinates(pt_3d_tf[0], pt_3d_tf[1], pt_3d_tf[2]);
    }

    removeComment(fileId);

    // Store in a map the potential segments to add
    std::map<std::pair<unsigned int, unsigned int>, SegmentInfo> segmentTemporaryMap;
    unsigned int caoNbrLine;
    fileId >> caoNbrLine;
    fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line

    nbLines += caoNbrLine;
    unsigned int *caoLinePoints = NULL;
    if (verbose || (parent && !header)) {
      std::cout << "> " << caoNbrLine << " lines" << std::endl;
    }

    if (caoNbrLine > 100000) {
      delete[] caoPoints;
      throw vpException(vpException::badValue, "Exceed the max number of lines in the CAO model.");
    }

    if (caoNbrLine > 0)
      caoLinePoints = new unsigned int[2 * caoNbrLine];

    unsigned int index1, index2;
    // Initialization of idFace with startIdFace for dealing with recursive
    // load in header
    int idFace = startIdFace;

    for (unsigned int k = 0; k < caoNbrLine; k++) {
      removeComment(fileId);

      fileId >> index1;
      fileId >> index2;

      // Get the end of the line
      std::string endLine = "";
      if (safeGetline(fileId, endLine).good()) {
        std::map<std::string, std::string> mapOfParams = parseParameters(endLine);

        std::string segmentName = "";
        double minLineLengthThresh = !applyLodSettingInConfig ? minLineLengthThresholdGeneral : 50.0;
        bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
        if (mapOfParams.find("name") != mapOfParams.end()) {
          segmentName = mapOfParams["name"];
        }
        if (mapOfParams.find("minLineLengthThreshold") != mapOfParams.end()) {
          minLineLengthThresh = std::atof(mapOfParams["minLineLengthThreshold"].c_str());
        }
        if (mapOfParams.find("useLod") != mapOfParams.end()) {
          useLod = vpIoTools::parseBoolean(mapOfParams["useLod"]);
        }

        SegmentInfo segmentInfo;
        segmentInfo.name = segmentName;
        segmentInfo.useLod = useLod;
        segmentInfo.minLineLengthThresh = minLineLengthThresh;

        caoLinePoints[2 * k] = index1;
        caoLinePoints[2 * k + 1] = index2;

        if (index1 < caoNbrPoint && index2 < caoNbrPoint) {
          std::vector<vpPoint> extremities;
          extremities.push_back(caoPoints[index1]);
          extremities.push_back(caoPoints[index2]);
          segmentInfo.extremities = extremities;

          std::pair<unsigned int, unsigned int> key(index1, index2);

          segmentTemporaryMap[key] = segmentInfo;
        } else {
          vpTRACE(" line %d has wrong coordinates.", k);
        }
      }
    }

    removeComment(fileId);

    /* Load polygon from the lines extracted earlier (the first point of the
     * line is used)*/
    // Store in a vector the indexes of the segments added in the face segment
    // case
    std::vector<std::pair<unsigned int, unsigned int> > faceSegmentKeyVector;
    unsigned int caoNbrPolygonLine;
    fileId >> caoNbrPolygonLine;
    fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line

    nbPolygonLines += caoNbrPolygonLine;
    if (verbose || (parent && !header)) {
      std::cout << "> " << caoNbrPolygonLine << " polygon lines" << std::endl;
    }

    if (caoNbrPolygonLine > 100000) {
      delete[] caoPoints;
      delete[] caoLinePoints;
      throw vpException(vpException::badValue, "Exceed the max number of polygon lines.");
    }

    unsigned int index;
    for (unsigned int k = 0; k < caoNbrPolygonLine; k++) {
      removeComment(fileId);

      unsigned int nbLinePol;
      fileId >> nbLinePol;
      std::vector<vpPoint> corners;
      if (nbLinePol > 100000) {
        throw vpException(vpException::badValue, "Exceed the max number of lines.");
      }

      for (unsigned int n = 0; n < nbLinePol; n++) {
        fileId >> index;

        if (index >= caoNbrLine) {
          throw vpException(vpException::badValue, "Exceed the max number of lines.");
        }
        corners.push_back(caoPoints[caoLinePoints[2 * index]]);
        corners.push_back(caoPoints[caoLinePoints[2 * index + 1]]);

        std::pair<unsigned int, unsigned int> key(caoLinePoints[2 * index], caoLinePoints[2 * index + 1]);
        faceSegmentKeyVector.push_back(key);
      }

      // Get the end of the line
      std::string endLine = "";
      if (safeGetline(fileId, endLine).good()) {
        std::map<std::string, std::string> mapOfParams = parseParameters(endLine);

        std::string polygonName = "";
        bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
        double minPolygonAreaThreshold = !applyLodSettingInConfig ? minPolygonAreaThresholdGeneral : 2500.0;
        if (mapOfParams.find("name") != mapOfParams.end()) {
          polygonName = mapOfParams["name"];
        }
        if (mapOfParams.find("minPolygonAreaThreshold") != mapOfParams.end()) {
          minPolygonAreaThreshold = std::atof(mapOfParams["minPolygonAreaThreshold"].c_str());
        }
        if (mapOfParams.find("useLod") != mapOfParams.end()) {
          useLod = vpIoTools::parseBoolean(mapOfParams["useLod"]);
        }

        addPolygon(corners, idFace, polygonName, useLod, minPolygonAreaThreshold, minLineLengthThresholdGeneral);
        initFaceFromLines(*(faces.getPolygon().back())); // Init from the last polygon that was added

        addProjectionErrorPolygon(corners, idFace++, polygonName, useLod, minPolygonAreaThreshold, minLineLengthThresholdGeneral);
        initProjectionErrorFaceFromLines(*(m_projectionErrorFaces.getPolygon().back()));
      }
    }

    // Add the segments which were not already added in the face segment case
    for (std::map<std::pair<unsigned int, unsigned int>, SegmentInfo>::const_iterator it = segmentTemporaryMap.begin();
         it != segmentTemporaryMap.end(); ++it) {
      if (std::find(faceSegmentKeyVector.begin(), faceSegmentKeyVector.end(), it->first) ==
          faceSegmentKeyVector.end()) {
        addPolygon(it->second.extremities, idFace, it->second.name, it->second.useLod, minPolygonAreaThresholdGeneral,
                   it->second.minLineLengthThresh);
        initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added

        addProjectionErrorPolygon(it->second.extremities, idFace++, it->second.name, it->second.useLod, minPolygonAreaThresholdGeneral,
                                  it->second.minLineLengthThresh);
        initProjectionErrorFaceFromCorners(*(m_projectionErrorFaces.getPolygon().back()));
      }
    }

    removeComment(fileId);

    /* Extract the polygon using the point coordinates (top of the file) */
    unsigned int caoNbrPolygonPoint;
    fileId >> caoNbrPolygonPoint;
    fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line

    nbPolygonPoints += caoNbrPolygonPoint;
    if (verbose || (parent && !header)) {
      std::cout << "> " << caoNbrPolygonPoint << " polygon points" << std::endl;
    }

    if (caoNbrPolygonPoint > 100000) {
      throw vpException(vpException::badValue, "Exceed the max number of polygon point.");
    }

    for (unsigned int k = 0; k < caoNbrPolygonPoint; k++) {
      removeComment(fileId);

      unsigned int nbPointPol;
      fileId >> nbPointPol;
      if (nbPointPol > 100000) {
        throw vpException(vpException::badValue, "Exceed the max number of points.");
      }
      std::vector<vpPoint> corners;
      for (unsigned int n = 0; n < nbPointPol; n++) {
        fileId >> index;
        if (index > caoNbrPoint - 1) {
          throw vpException(vpException::badValue, "Exceed the max number of points.");
        }
        corners.push_back(caoPoints[index]);
      }

      // Get the end of the line
      std::string endLine = "";
      if (safeGetline(fileId, endLine).good()) {
        std::map<std::string, std::string> mapOfParams = parseParameters(endLine);

        std::string polygonName = "";
        bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
        double minPolygonAreaThreshold = !applyLodSettingInConfig ? minPolygonAreaThresholdGeneral : 2500.0;
        if (mapOfParams.find("name") != mapOfParams.end()) {
          polygonName = mapOfParams["name"];
        }
        if (mapOfParams.find("minPolygonAreaThreshold") != mapOfParams.end()) {
          minPolygonAreaThreshold = std::atof(mapOfParams["minPolygonAreaThreshold"].c_str());
        }
        if (mapOfParams.find("useLod") != mapOfParams.end()) {
          useLod = vpIoTools::parseBoolean(mapOfParams["useLod"]);
        }

        addPolygon(corners, idFace, polygonName, useLod, minPolygonAreaThreshold, minLineLengthThresholdGeneral);
        initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added

        addProjectionErrorPolygon(corners, idFace++, polygonName, useLod, minPolygonAreaThreshold, minLineLengthThresholdGeneral);
        initProjectionErrorFaceFromCorners(*(m_projectionErrorFaces.getPolygon().back()));
      }
    }

    unsigned int caoNbCylinder;
    try {
      removeComment(fileId);

      if (fileId.eof()) { // check if not at the end of the file (for old
                          // style files)
        delete[] caoPoints;
        delete[] caoLinePoints;
        return;
      }

      /* Extract the cylinders */
      fileId >> caoNbCylinder;
      fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n')); // skip the rest of the line

      nbCylinders += caoNbCylinder;
      if (verbose || (parent && !header)) {
        std::cout << "> " << caoNbCylinder << " cylinders" << std::endl;
      }

      if (caoNbCylinder > 100000) {
        throw vpException(vpException::badValue, "Exceed the max number of cylinders.");
      }

      for (unsigned int k = 0; k < caoNbCylinder; ++k) {
        removeComment(fileId);

        double radius;
        unsigned int indexP1, indexP2;
        fileId >> indexP1;
        fileId >> indexP2;
        fileId >> radius;

        // Get the end of the line
        std::string endLine = "";
        if (safeGetline(fileId, endLine).good()) {
          std::map<std::string, std::string> mapOfParams = parseParameters(endLine);

          std::string polygonName = "";
          bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
          double minLineLengthThreshold = !applyLodSettingInConfig ? minLineLengthThresholdGeneral : 50.0;
          if (mapOfParams.find("name") != mapOfParams.end()) {
            polygonName = mapOfParams["name"];
          }
          if (mapOfParams.find("minLineLengthThreshold") != mapOfParams.end()) {
            minLineLengthThreshold = std::atof(mapOfParams["minLineLengthThreshold"].c_str());
          }
          if (mapOfParams.find("useLod") != mapOfParams.end()) {
            useLod = vpIoTools::parseBoolean(mapOfParams["useLod"]);
          }

          int idRevolutionAxis = idFace;
          addPolygon(caoPoints[indexP1], caoPoints[indexP2], idFace, polygonName, useLod, minLineLengthThreshold);

          addProjectionErrorPolygon(caoPoints[indexP1], caoPoints[indexP2], idFace++, polygonName, useLod, minLineLengthThreshold);

          std::vector<std::vector<vpPoint> > listFaces;
          createCylinderBBox(caoPoints[indexP1], caoPoints[indexP2], radius, listFaces);
          addPolygon(listFaces, idFace, polygonName, useLod, minLineLengthThreshold);

          initCylinder(caoPoints[indexP1], caoPoints[indexP2], radius, idRevolutionAxis, polygonName);

          addProjectionErrorPolygon(listFaces, idFace, polygonName, useLod, minLineLengthThreshold);
          initProjectionErrorCylinder(caoPoints[indexP1], caoPoints[indexP2], radius, idRevolutionAxis, polygonName);

          idFace += 4;
        }
      }

    } catch (const std::exception& e) {
      std::cerr << "Cannot get the number of cylinders. Defaulting to zero." << std::endl;
      std::cerr << "Exception: " << e.what() << std::endl;
      caoNbCylinder = 0;
    }

    unsigned int caoNbCircle;
    try {
      removeComment(fileId);

      if (fileId.eof()) { // check if not at the end of the file (for old
                          // style files)
        delete[] caoPoints;
        delete[] caoLinePoints;
        return;
      }

      /* Extract the circles */
      fileId >> caoNbCircle;
      fileId.ignore(std::numeric_limits<std::streamsize>::max(), fileId.widen('\n'));  // skip the rest of the line

      nbCircles += caoNbCircle;
      if (verbose || (parent && !header)) {
        std::cout << "> " << caoNbCircle << " circles" << std::endl;
      }

      if (caoNbCircle > 100000) {
        throw vpException(vpException::badValue, "Exceed the max number of cicles.");
      }

      for (unsigned int k = 0; k < caoNbCircle; ++k) {
        removeComment(fileId);

        double radius;
        unsigned int indexP1, indexP2, indexP3;
        fileId >> radius;
        fileId >> indexP1;
        fileId >> indexP2;
        fileId >> indexP3;

        // Get the end of the line
        std::string endLine = "";
        if (safeGetline(fileId, endLine).good()) {
          std::map<std::string, std::string> mapOfParams = parseParameters(endLine);

          std::string polygonName = "";
          bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
          double minPolygonAreaThreshold = !applyLodSettingInConfig ? minPolygonAreaThresholdGeneral : 2500.0;
          if (mapOfParams.find("name") != mapOfParams.end()) {
            polygonName = mapOfParams["name"];
          }
          if (mapOfParams.find("minPolygonAreaThreshold") != mapOfParams.end()) {
            minPolygonAreaThreshold = std::atof(mapOfParams["minPolygonAreaThreshold"].c_str());
          }
          if (mapOfParams.find("useLod") != mapOfParams.end()) {
            useLod = vpIoTools::parseBoolean(mapOfParams["useLod"]);
          }

          addPolygon(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName, useLod,
                     minPolygonAreaThreshold);

          initCircle(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName);

          addProjectionErrorPolygon(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace, polygonName, useLod,
                                    minPolygonAreaThreshold);
          initProjectionErrorCircle(caoPoints[indexP1], caoPoints[indexP2], caoPoints[indexP3], radius, idFace++, polygonName);
        }
      }

    } catch (const std::exception& e) {
      std::cerr << "Cannot get the number of circles. Defaulting to zero." << std::endl;
      std::cerr << "Exception: " << e.what() << std::endl;
      caoNbCircle = 0;
    }

    startIdFace = idFace;

    delete[] caoPoints;
    delete[] caoLinePoints;

    if (header && parent) {
      if (verbose) {
        std::cout << "Global information for " << vpIoTools::getName(modelFile) << " :" << std::endl;
        std::cout << "Total nb of points : " << nbPoints << std::endl;
        std::cout << "Total nb of lines : " << nbLines << std::endl;
        std::cout << "Total nb of polygon lines : " << nbPolygonLines << std::endl;
        std::cout << "Total nb of polygon points : " << nbPolygonPoints << std::endl;
        std::cout << "Total nb of cylinders : " << nbCylinders << std::endl;
        std::cout << "Total nb of circles : " << nbCircles << std::endl;
      } else {
        std::cout << "> " << nbPoints << " points" << std::endl;
        std::cout << "> " << nbLines << " lines" << std::endl;
        std::cout << "> " << nbPolygonLines << " polygon lines" << std::endl;
        std::cout << "> " << nbPolygonPoints << " polygon points" << std::endl;
        std::cout << "> " << nbCylinders << " cylinders" << std::endl;
        std::cout << "> " << nbCircles << " circles" << std::endl;
      }
    }

    //Go up: remove current model
    vectorOfModelFilename.pop_back();
  } catch (const std::exception& e) {
    std::cerr << "Cannot read line!" << std::endl;
    std::cerr << "Exception: " << e.what() << std::endl;
    throw vpException(vpException::ioError, "cannot read line");
  }
}

#ifdef VISP_HAVE_COIN3D

void vpMbTracker::extractGroup(SoVRMLGroup *sceneGraphVRML2, vpHomogeneousMatrix &transform, int &idFace)
{
  vpHomogeneousMatrix transformCur;
  SoVRMLTransform *sceneGraphVRML2Trasnform = dynamic_cast<SoVRMLTransform *>(sceneGraphVRML2);
  if (sceneGraphVRML2Trasnform) {
    float rx, ry, rz, rw;
    sceneGraphVRML2Trasnform->rotation.getValue().getValue(rx, ry, rz, rw);
    vpRotationMatrix rotMat(vpQuaternionVector(rx, ry, rz, rw));
    //     std::cout << "Rotation: " << rx << " " << ry << " " << rz << " " <<
    //     rw << std::endl;

    float tx, ty, tz;
    tx = sceneGraphVRML2Trasnform->translation.getValue()[0];
    ty = sceneGraphVRML2Trasnform->translation.getValue()[1];
    tz = sceneGraphVRML2Trasnform->translation.getValue()[2];
    vpTranslationVector transVec(tx, ty, tz);
    //     std::cout << "Translation: " << tx << " " << ty << " " << tz <<
    //     std::endl;

    float sx, sy, sz;
    sx = sceneGraphVRML2Trasnform->scale.getValue()[0];
    sy = sceneGraphVRML2Trasnform->scale.getValue()[1];
    sz = sceneGraphVRML2Trasnform->scale.getValue()[2];
    //     std::cout << "Scale: " << sx << " " << sy << " " << sz <<
    //     std::endl;

    for (unsigned int i = 0; i < 3; i++)
      rotMat[0][i] *= sx;
    for (unsigned int i = 0; i < 3; i++)
      rotMat[1][i] *= sy;
    for (unsigned int i = 0; i < 3; i++)
      rotMat[2][i] *= sz;

    transformCur = vpHomogeneousMatrix(transVec, rotMat);
    transform = transform * transformCur;
  }

  int nbShapes = sceneGraphVRML2->getNumChildren();
  //   std::cout << sceneGraphVRML2->getTypeId().getName().getString() <<
  //   std::endl; std::cout << "Nb object in VRML : " << nbShapes <<
  //   std::endl;

  SoNode *child;

  for (int i = 0; i < nbShapes; i++) {
    vpHomogeneousMatrix transform_recursive(transform);
    child = sceneGraphVRML2->getChild(i);

    if (child->getTypeId() == SoVRMLGroup::getClassTypeId()) {
      extractGroup((SoVRMLGroup *)child, transform_recursive, idFace);
    }

    if (child->getTypeId() == SoVRMLTransform::getClassTypeId()) {
      extractGroup((SoVRMLTransform *)child, transform_recursive, idFace);
    }

    if (child->getTypeId() == SoVRMLShape::getClassTypeId()) {
      SoChildList *child2list = child->getChildren();
      std::string name = child->getName().getString();

      for (int j = 0; j < child2list->getLength(); j++) {
        if (((SoNode *)child2list->get(j))->getTypeId() == SoVRMLIndexedFaceSet::getClassTypeId()) {
          SoVRMLIndexedFaceSet *face_set;
          face_set = (SoVRMLIndexedFaceSet *)child2list->get(j);
          if (!strncmp(face_set->getName().getString(), "cyl", 3)) {
            extractCylinders(face_set, transform, idFace, name);
          } else {
            extractFaces(face_set, transform, idFace, name);
          }
        }
        if (((SoNode *)child2list->get(j))->getTypeId() == SoVRMLIndexedLineSet::getClassTypeId()) {
          SoVRMLIndexedLineSet *line_set;
          line_set = (SoVRMLIndexedLineSet *)child2list->get(j);
          extractLines(line_set, idFace, name);
        }
      }
    }
  }
}

void vpMbTracker::extractFaces(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace,
                               const std::string &polygonName)
{
  std::vector<vpPoint> corners;

  //  SoMFInt32 indexList = _face_set->coordIndex;
  //  int indexListSize = indexList.getNum();
  int indexListSize = face_set->coordIndex.getNum();

  vpColVector pointTransformed(4);
  vpPoint pt;
  SoVRMLCoordinate *coord;

  for (int i = 0; i < indexListSize; i++) {
    if (face_set->coordIndex[i] == -1) {
      if (corners.size() > 1) {
        addPolygon(corners, idFace, polygonName);
        initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added

        addProjectionErrorPolygon(corners, idFace++, polygonName);
        initProjectionErrorFaceFromCorners(*(m_projectionErrorFaces.getPolygon().back()));
        corners.resize(0);
      }
    } else {
      coord = (SoVRMLCoordinate *)(face_set->coord.getValue());
      int index = face_set->coordIndex[i];
      pointTransformed[0] = coord->point[index].getValue()[0];
      pointTransformed[1] = coord->point[index].getValue()[1];
      pointTransformed[2] = coord->point[index].getValue()[2];
      pointTransformed[3] = 1.0;

      pointTransformed = transform * pointTransformed;

      pt.setWorldCoordinates(pointTransformed[0], pointTransformed[1], pointTransformed[2]);
      corners.push_back(pt);
    }
  }
}

void vpMbTracker::extractCylinders(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace,
                                   const std::string &polygonName)
{
  std::vector<vpPoint> corners_c1, corners_c2; // points belonging to the
                                               // first circle and to the
                                               // second one.
  SoVRMLCoordinate *coords = (SoVRMLCoordinate *)face_set->coord.getValue();

  unsigned int indexListSize = (unsigned int)coords->point.getNum();

  if (indexListSize % 2 == 1) {
    std::cout << "Not an even number of points when extracting a cylinder." << std::endl;
    throw vpException(vpException::dimensionError, "Not an even number of points when extracting a cylinder.");
  }
  corners_c1.resize(indexListSize / 2);
  corners_c2.resize(indexListSize / 2);
  vpColVector pointTransformed(4);
  vpPoint pt;

  // extract all points and fill the two sets.

  for (int i = 0; i < coords->point.getNum(); ++i) {
    pointTransformed[0] = coords->point[i].getValue()[0];
    pointTransformed[1] = coords->point[i].getValue()[1];
    pointTransformed[2] = coords->point[i].getValue()[2];
    pointTransformed[3] = 1.0;

    pointTransformed = transform * pointTransformed;

    pt.setWorldCoordinates(pointTransformed[0], pointTransformed[1], pointTransformed[2]);

    if (i < (int)corners_c1.size()) {
      corners_c1[(unsigned int)i] = pt;
    } else {
      corners_c2[(unsigned int)i - corners_c1.size()] = pt;
    }
  }

  vpPoint p1 = getGravityCenter(corners_c1);
  vpPoint p2 = getGravityCenter(corners_c2);

  vpColVector dist(3);
  dist[0] = p1.get_oX() - corners_c1[0].get_oX();
  dist[1] = p1.get_oY() - corners_c1[0].get_oY();
  dist[2] = p1.get_oZ() - corners_c1[0].get_oZ();
  double radius_c1 = sqrt(dist.sumSquare());
  dist[0] = p2.get_oX() - corners_c2[0].get_oX();
  dist[1] = p2.get_oY() - corners_c2[0].get_oY();
  dist[2] = p2.get_oZ() - corners_c2[0].get_oZ();
  double radius_c2 = sqrt(dist.sumSquare());

  if (std::fabs(radius_c1 - radius_c2) >
      (std::numeric_limits<double>::epsilon() * vpMath::maximum(radius_c1, radius_c2))) {
    std::cout << "Radius from the two circles of the cylinders are different." << std::endl;
    throw vpException(vpException::badValue, "Radius from the two circles of the cylinders are different.");
  }

  // addPolygon(p1, p2, idFace, polygonName);
  // initCylinder(p1, p2, radius_c1, idFace++);

  int idRevolutionAxis = idFace;
  addPolygon(p1, p2, idFace, polygonName);

  addProjectionErrorPolygon(p1, p2, idFace++, polygonName);

  std::vector<std::vector<vpPoint> > listFaces;
  createCylinderBBox(p1, p2, radius_c1, listFaces);
  addPolygon(listFaces, idFace, polygonName);

  initCylinder(p1, p2, radius_c1, idRevolutionAxis, polygonName);

  addProjectionErrorPolygon(listFaces, idFace, polygonName);
  initProjectionErrorCylinder(p1, p2, radius_c1, idRevolutionAxis, polygonName);

  idFace += 4;
}

void vpMbTracker::extractLines(SoVRMLIndexedLineSet *line_set, int &idFace, const std::string &polygonName)
{
  std::vector<vpPoint> corners;
  corners.resize(0);

  int indexListSize = line_set->coordIndex.getNum();

  SbVec3f point(0, 0, 0);
  vpPoint pt;
  SoVRMLCoordinate *coord;

  for (int i = 0; i < indexListSize; i++) {
    if (line_set->coordIndex[i] == -1) {
      if (corners.size() > 1) {
        addPolygon(corners, idFace, polygonName);
        initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added

        addProjectionErrorPolygon(corners, idFace++, polygonName);
        initProjectionErrorFaceFromCorners(*(m_projectionErrorFaces.getPolygon().back()));
        corners.resize(0);
      }
    } else {
      coord = (SoVRMLCoordinate *)(line_set->coord.getValue());
      int index = line_set->coordIndex[i];
      point[0] = coord->point[index].getValue()[0];
      point[1] = coord->point[index].getValue()[1];
      point[2] = coord->point[index].getValue()[2];

      pt.setWorldCoordinates(point[0], point[1], point[2]);
      corners.push_back(pt);
    }
  }
}

#endif // VISP_HAVE_COIN3D

vpPoint vpMbTracker::getGravityCenter(const std::vector<vpPoint> &pts) const
{
  if (pts.empty()) {
    std::cout << "Cannot extract center of gravity of empty set." << std::endl;
    throw vpException(vpException::dimensionError, "Cannot extract center of gravity of empty set.");
  }
  double oX = 0;
  double oY = 0;
  double oZ = 0;
  vpPoint G;

  for (unsigned int i = 0; i < pts.size(); ++i) {
    oX += pts[i].get_oX();
    oY += pts[i].get_oY();
    oZ += pts[i].get_oZ();
  }

  G.setWorldCoordinates(oX / pts.size(), oY / pts.size(), oZ / pts.size());
  return G;
}

std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > >
vpMbTracker::getPolygonFaces(bool orderPolygons, bool useVisibility, bool clipPolygon)
{
  // Temporary variable to permit to order polygons by distance
  std::vector<vpPolygon> polygonsTmp;
  std::vector<std::vector<vpPoint> > roisPtTmp;

  // Pair containing the list of vpPolygon and the list of face corners
  std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > pairOfPolygonFaces;

  for (unsigned int i = 0; i < faces.getPolygon().size(); i++) {
    // A face has at least 3 points
    if (faces.getPolygon()[i]->nbpt > 2) {
      if ((useVisibility && faces.getPolygon()[i]->isvisible) || !useVisibility) {
        std::vector<vpImagePoint> roiPts;

        if (clipPolygon) {
          faces.getPolygon()[i]->getRoiClipped(m_cam, roiPts, m_cMo);
        } else {
          roiPts = faces.getPolygon()[i]->getRoi(m_cam, m_cMo);
        }

        if (roiPts.size() <= 2) {
          continue;
        }

        polygonsTmp.push_back(vpPolygon(roiPts));

        std::vector<vpPoint> polyPts;
        if (clipPolygon) {
          faces.getPolygon()[i]->getPolygonClipped(polyPts);
        } else {
          for (unsigned int j = 0; j < faces.getPolygon()[i]->nbpt; j++) {
            polyPts.push_back(faces.getPolygon()[i]->p[j]);
          }
        }
        roisPtTmp.push_back(polyPts);
      }
    }
  }

  if (orderPolygons) {
    // Order polygons by distance (near to far)
    std::vector<PolygonFaceInfo> listOfPolygonFaces;
    for (unsigned int i = 0; i < polygonsTmp.size(); i++) {
      double x_centroid = 0.0, y_centroid = 0.0, z_centroid = 0.0;
      for (unsigned int j = 0; j < roisPtTmp[i].size(); j++) {
        x_centroid += roisPtTmp[i][j].get_X();
        y_centroid += roisPtTmp[i][j].get_Y();
        z_centroid += roisPtTmp[i][j].get_Z();
      }

      x_centroid /= roisPtTmp[i].size();
      y_centroid /= roisPtTmp[i].size();
      z_centroid /= roisPtTmp[i].size();

      double squared_dist = x_centroid * x_centroid + y_centroid * y_centroid + z_centroid * z_centroid;
      listOfPolygonFaces.push_back(PolygonFaceInfo(squared_dist, polygonsTmp[i], roisPtTmp[i]));
    }

    // Sort the list of polygon faces
    std::sort(listOfPolygonFaces.begin(), listOfPolygonFaces.end());

    polygonsTmp.resize(listOfPolygonFaces.size());
    roisPtTmp.resize(listOfPolygonFaces.size());

    size_t cpt = 0;
    for (std::vector<PolygonFaceInfo>::const_iterator it = listOfPolygonFaces.begin(); it != listOfPolygonFaces.end();
         ++it, cpt++) {
      polygonsTmp[cpt] = it->polygon;
      roisPtTmp[cpt] = it->faceCorners;
    }

    pairOfPolygonFaces.first = polygonsTmp;
    pairOfPolygonFaces.second = roisPtTmp;
  } else {
    pairOfPolygonFaces.first = polygonsTmp;
    pairOfPolygonFaces.second = roisPtTmp;
  }

  return pairOfPolygonFaces;
}

void vpMbTracker::setOgreVisibilityTest(const bool &v)
{
  useOgre = v;
  if (useOgre) {
#ifndef VISP_HAVE_OGRE
    useOgre = false;
    std::cout << "WARNING: ViSP doesn't have Ogre3D, basic visibility test "
                 "will be used. setOgreVisibilityTest() set to false."
              << std::endl;
#endif
  }
}

void vpMbTracker::setFarClippingDistance(const double &dist)
{
  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING && dist <= distNearClip)
    vpTRACE("Far clipping value cannot be inferior than near clipping value. "
            "Far clipping won't be considered.");
  else if (dist < 0)
    vpTRACE("Far clipping value cannot be inferior than 0. Far clipping "
            "won't be considered.");
  else {
    clippingFlag = (clippingFlag | vpPolygon3D::FAR_CLIPPING);
    distFarClip = dist;
    for (unsigned int i = 0; i < faces.size(); i++) {
      faces[i]->setFarClippingDistance(distFarClip);
    }
#ifdef VISP_HAVE_OGRE
    faces.getOgreContext()->setFarClippingDistance(distFarClip);
#endif
  }
}

void vpMbTracker::setLod(bool useLod, const std::string &name)
{
  for (unsigned int i = 0; i < faces.size(); i++) {
    if (name.empty() || faces[i]->name == name) {
      faces[i]->setLod(useLod);
    }
  }
}

void vpMbTracker::setMinLineLengthThresh(double minLineLengthThresh, const std::string &name)
{
  for (unsigned int i = 0; i < faces.size(); i++) {
    if (name.empty() || faces[i]->name == name) {
      faces[i]->setMinLineLengthThresh(minLineLengthThresh);
    }
  }
}

void vpMbTracker::setMinPolygonAreaThresh(double minPolygonAreaThresh, const std::string &name)
{
  for (unsigned int i = 0; i < faces.size(); i++) {
    if (name.empty() || faces[i]->name == name) {
      faces[i]->setMinPolygonAreaThresh(minPolygonAreaThresh);
    }
  }
}

void vpMbTracker::setNearClippingDistance(const double &dist)
{
  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING && dist >= distFarClip)
    vpTRACE("Near clipping value cannot be superior than far clipping value. "
            "Near clipping won't be considered.");
  else if (dist < 0)
    vpTRACE("Near clipping value cannot be inferior than 0. Near clipping "
            "won't be considered.");
  else {
    clippingFlag = (clippingFlag | vpPolygon3D::NEAR_CLIPPING);
    distNearClip = dist;
    for (unsigned int i = 0; i < faces.size(); i++) {
      faces[i]->setNearClippingDistance(distNearClip);
    }
#ifdef VISP_HAVE_OGRE
    faces.getOgreContext()->setNearClippingDistance(distNearClip);
#endif
  }
}

void vpMbTracker::setClipping(const unsigned int &flags)
{
  clippingFlag = flags;
  for (unsigned int i = 0; i < faces.size(); i++)
    faces[i]->setClipping(clippingFlag);
}

void vpMbTracker::computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true,
                                             const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true,
                                             const vpMatrix &LVJ_true, const vpColVector &error)
{
  if (computeCovariance) {
    vpMatrix D;
    D.diag(w_true);

    // Note that here the covariance is computed on cMoPrev for time
    // computation efficiency
    if (isoJoIdentity_) {
      covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev, error, L_true, D);
    } else {
      covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev, error, LVJ_true, D);
    }
  }
}

void vpMbTracker::computeJTR(const vpMatrix &interaction, const vpColVector &error, vpColVector &JTR) const
{
  if (interaction.getRows() != error.getRows() || interaction.getCols() != 6) {
    throw vpMatrixException(vpMatrixException::incorrectMatrixSizeError, "Incorrect matrices size in computeJTR.");
  }

  JTR.resize(6, false);

  SimdComputeJtR(interaction.data, interaction.getRows(), error.data, JTR.data);
}

void vpMbTracker::computeVVSCheckLevenbergMarquardt(unsigned int iter, vpColVector &error,
                                                    const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev,
                                                    double &mu, bool &reStartFromLastIncrement, vpColVector *const w,
                                                    const vpColVector *const m_w_prev)
{
  if (iter != 0 && m_optimizationMethod == vpMbTracker::LEVENBERG_MARQUARDT_OPT) {
    if (error.sumSquare() / (double)error.getRows() > m_error_prev.sumSquare() / (double)m_error_prev.getRows()) {
      mu *= 10.0;

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

      m_cMo = cMoPrev;
      error = m_error_prev;
      if (w != NULL && m_w_prev != NULL) {
        *w = *m_w_prev;
      }
      reStartFromLastIncrement = true;
    }
  }
}

void vpMbTracker::computeVVSPoseEstimation(const bool isoJoIdentity_, unsigned int iter, vpMatrix &L,
                                           vpMatrix &LTL, vpColVector &R, const vpColVector &error,
                                           vpColVector &error_prev, vpColVector &LTR, double &mu, vpColVector &v,
                                           const vpColVector *const w, vpColVector *const m_w_prev)
{
  if (isoJoIdentity_) {
    LTL = L.AtA();
    computeJTR(L, R, LTR);

    switch (m_optimizationMethod) {
    case vpMbTracker::LEVENBERG_MARQUARDT_OPT: {
      vpMatrix LMA(LTL.getRows(), LTL.getCols());
      LMA.eye();
      vpMatrix LTLmuI = LTL + (LMA * mu);
      v = -m_lambda * LTLmuI.pseudoInverse(LTLmuI.getRows() * std::numeric_limits<double>::epsilon()) * LTR;

      if (iter != 0)
        mu /= 10.0;

      error_prev = error;
      if (w != NULL && m_w_prev != NULL)
        *m_w_prev = *w;
      break;
    }

    case vpMbTracker::GAUSS_NEWTON_OPT:
    default:
      v = -m_lambda * LTL.pseudoInverse(LTL.getRows() * std::numeric_limits<double>::epsilon()) * LTR;
      break;
    }
  } else {
    vpVelocityTwistMatrix cVo;
    cVo.buildFrom(m_cMo);
    vpMatrix LVJ = (L * (cVo * oJo));
    vpMatrix LVJTLVJ = (LVJ).AtA();
    vpColVector LVJTR;
    computeJTR(LVJ, R, LVJTR);

    switch (m_optimizationMethod) {
    case vpMbTracker::LEVENBERG_MARQUARDT_OPT: {
      vpMatrix LMA(LVJTLVJ.getRows(), LVJTLVJ.getCols());
      LMA.eye();
      vpMatrix LTLmuI = LVJTLVJ + (LMA * mu);
      v = -m_lambda * LTLmuI.pseudoInverse(LTLmuI.getRows() * std::numeric_limits<double>::epsilon()) * LVJTR;
      v = cVo * v;

      if (iter != 0)
        mu /= 10.0;

      error_prev = error;
      if (w != NULL && m_w_prev != NULL)
        *m_w_prev = *w;
      break;
    }
    case vpMbTracker::GAUSS_NEWTON_OPT:
    default:
      v = -m_lambda * LVJTLVJ.pseudoInverse(LVJTLVJ.getRows() * std::numeric_limits<double>::epsilon()) * LVJTR;
      v = cVo * v;
      break;
    }
  }
}

void vpMbTracker::computeVVSWeights(vpRobust &robust, const vpColVector &error, vpColVector &w)
{
  if (error.getRows() > 0)
    robust.MEstimator(vpRobust::TUKEY, error, w);
}

vpColVector vpMbTracker::getEstimatedDoF() const
{
  vpColVector v(6);
  for (unsigned int i = 0; i < 6; i++)
    v[i] = oJo[i][i];
  return v;
}

void vpMbTracker::setEstimatedDoF(const vpColVector &v)
{
  if (v.getRows() == 6) {
    isoJoIdentity = true;
    for (unsigned int i = 0; i < 6; i++) {
      // if(v[i] != 0){
      if (std::fabs(v[i]) > std::numeric_limits<double>::epsilon()) {
        oJo[i][i] = 1.0;
      } else {
        oJo[i][i] = 0.0;
        isoJoIdentity = false;
      }
    }
  }
}

void vpMbTracker::createCylinderBBox(const vpPoint &p1, const vpPoint &p2, const double &radius,
                                     std::vector<std::vector<vpPoint> > &listFaces)
{
  listFaces.clear();

  //    std::vector<vpPoint> revolutionAxis;
  //    revolutionAxis.push_back(p1);
  //    revolutionAxis.push_back(p2);
  //    listFaces.push_back(revolutionAxis);

  vpColVector axis(3);
  axis[0] = p1.get_oX() - p2.get_oX();
  axis[1] = p1.get_oY() - p2.get_oY();
  axis[2] = p1.get_oZ() - p2.get_oZ();

  vpColVector randomVec(3);
  randomVec = 0;

  vpColVector axisOrtho(3);

  randomVec[0] = 1.0;
  axisOrtho = vpColVector::crossProd(axis, randomVec);

  if (axisOrtho.frobeniusNorm() < std::numeric_limits<double>::epsilon()) {
    randomVec = 0;
    randomVec[1] = 1.0;
    axisOrtho = vpColVector::crossProd(axis, randomVec);
    if (axisOrtho.frobeniusNorm() < std::numeric_limits<double>::epsilon()) {
      randomVec = 0;
      randomVec[2] = 1.0;
      axisOrtho = vpColVector::crossProd(axis, randomVec);
      if (axisOrtho.frobeniusNorm() < std::numeric_limits<double>::epsilon())
        throw vpMatrixException(vpMatrixException::badValue, "Problem in the cylinder definition");
    }
  }

  axisOrtho.normalize();

  vpColVector axisOrthoBis(3);
  axisOrthoBis = vpColVector::crossProd(axis, axisOrtho);
  axisOrthoBis.normalize();

  // First circle
  vpColVector p1Vec(3);
  p1Vec[0] = p1.get_oX();
  p1Vec[1] = p1.get_oY();
  p1Vec[2] = p1.get_oZ();
  vpColVector fc1 = p1Vec + axisOrtho * radius;
  vpColVector fc2 = p1Vec + axisOrthoBis * radius;
  vpColVector fc3 = p1Vec - axisOrtho * radius;
  vpColVector fc4 = p1Vec - axisOrthoBis * radius;

  vpColVector p2Vec(3);
  p2Vec[0] = p2.get_oX();
  p2Vec[1] = p2.get_oY();
  p2Vec[2] = p2.get_oZ();
  vpColVector sc1 = p2Vec + axisOrtho * radius;
  vpColVector sc2 = p2Vec + axisOrthoBis * radius;
  vpColVector sc3 = p2Vec - axisOrtho * radius;
  vpColVector sc4 = p2Vec - axisOrthoBis * radius;

  std::vector<vpPoint> pointsFace;
  pointsFace.push_back(vpPoint(fc1[0], fc1[1], fc1[2]));
  pointsFace.push_back(vpPoint(sc1[0], sc1[1], sc1[2]));
  pointsFace.push_back(vpPoint(sc2[0], sc2[1], sc2[2]));
  pointsFace.push_back(vpPoint(fc2[0], fc2[1], fc2[2]));
  listFaces.push_back(pointsFace);

  pointsFace.clear();
  pointsFace.push_back(vpPoint(fc2[0], fc2[1], fc2[2]));
  pointsFace.push_back(vpPoint(sc2[0], sc2[1], sc2[2]));
  pointsFace.push_back(vpPoint(sc3[0], sc3[1], sc3[2]));
  pointsFace.push_back(vpPoint(fc3[0], fc3[1], fc3[2]));
  listFaces.push_back(pointsFace);

  pointsFace.clear();
  pointsFace.push_back(vpPoint(fc3[0], fc3[1], fc3[2]));
  pointsFace.push_back(vpPoint(sc3[0], sc3[1], sc3[2]));
  pointsFace.push_back(vpPoint(sc4[0], sc4[1], sc4[2]));
  pointsFace.push_back(vpPoint(fc4[0], fc4[1], fc4[2]));
  listFaces.push_back(pointsFace);

  pointsFace.clear();
  pointsFace.push_back(vpPoint(fc4[0], fc4[1], fc4[2]));
  pointsFace.push_back(vpPoint(sc4[0], sc4[1], sc4[2]));
  pointsFace.push_back(vpPoint(sc1[0], sc1[1], sc1[2]));
  pointsFace.push_back(vpPoint(fc1[0], fc1[1], fc1[2]));
  listFaces.push_back(pointsFace);
}

bool vpMbTracker::samePoint(const vpPoint &P1, const vpPoint &P2) const
{
  double dx = fabs(P1.get_oX() - P2.get_oX());
  double dy = fabs(P1.get_oY() - P2.get_oY());
  double dz = fabs(P1.get_oZ() - P2.get_oZ());

  if (dx <= std::numeric_limits<double>::epsilon() && dy <= std::numeric_limits<double>::epsilon() &&
      dz <= std::numeric_limits<double>::epsilon())
    return true;
  else
    return false;
}

void vpMbTracker::addProjectionErrorPolygon(const std::vector<vpPoint> &corners, int idFace, const std::string &polygonName,
                                            bool useLod, double minPolygonAreaThreshold,
                                            double minLineLengthThreshold)
{
  std::vector<vpPoint> corners_without_duplicates;
  corners_without_duplicates.push_back(corners[0]);
  for (unsigned int i = 0; i < corners.size() - 1; i++) {
    if (std::fabs(corners[i].get_oX() - corners[i + 1].get_oX()) >
            std::fabs(corners[i].get_oX()) * std::numeric_limits<double>::epsilon() ||
        std::fabs(corners[i].get_oY() - corners[i + 1].get_oY()) >
            std::fabs(corners[i].get_oY()) * std::numeric_limits<double>::epsilon() ||
        std::fabs(corners[i].get_oZ() - corners[i + 1].get_oZ()) >
            std::fabs(corners[i].get_oZ()) * std::numeric_limits<double>::epsilon()) {
      corners_without_duplicates.push_back(corners[i + 1]);
    }
  }

  vpMbtPolygon polygon;
  polygon.setNbPoint((unsigned int)corners_without_duplicates.size());
  polygon.setIndex((int)idFace);
  polygon.setName(polygonName);
  polygon.setLod(useLod);

  polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
  polygon.setMinLineLengthThresh(minLineLengthThreshold);

  for (unsigned int j = 0; j < corners_without_duplicates.size(); j++) {
    polygon.addPoint(j, corners_without_duplicates[j]);
  }

  m_projectionErrorFaces.addPolygon(&polygon);

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

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

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

void vpMbTracker::addProjectionErrorPolygon(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius,
                                            int idFace, const std::string &polygonName, bool useLod,
                                            double minPolygonAreaThreshold)
{
  vpMbtPolygon polygon;
  polygon.setNbPoint(4);
  polygon.setName(polygonName);
  polygon.setLod(useLod);

  polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
  // Non sense to set minLineLengthThreshold for circle
  // but used to be coherent when applying LOD settings for all polygons
  polygon.setMinLineLengthThresh(minLineLengthThresholdGeneral);

  {
    // Create the 4 points of the circle bounding box
    vpPlane plane(p1, p2, p3, vpPlane::object_frame);

    // Matrice de passage entre world et circle frame
    double norm_X = sqrt(vpMath::sqr(p2.get_oX() - p1.get_oX()) + vpMath::sqr(p2.get_oY() - p1.get_oY()) +
                         vpMath::sqr(p2.get_oZ() - p1.get_oZ()));
    double norm_Y = sqrt(vpMath::sqr(plane.getA()) + vpMath::sqr(plane.getB()) + vpMath::sqr(plane.getC()));
    vpRotationMatrix wRc;
    vpColVector x(3), y(3), z(3);
    // X axis is P2-P1
    x[0] = (p2.get_oX() - p1.get_oX()) / norm_X;
    x[1] = (p2.get_oY() - p1.get_oY()) / norm_X;
    x[2] = (p2.get_oZ() - p1.get_oZ()) / norm_X;
    // Y axis is the normal of the plane
    y[0] = plane.getA() / norm_Y;
    y[1] = plane.getB() / norm_Y;
    y[2] = plane.getC() / norm_Y;
    // Z axis = X ^ Y
    z = vpColVector::crossProd(x, y);
    for (unsigned int i = 0; i < 3; i++) {
      wRc[i][0] = x[i];
      wRc[i][1] = y[i];
      wRc[i][2] = z[i];
    }

    vpTranslationVector wtc(p1.get_oX(), p1.get_oY(), p1.get_oZ());
    vpHomogeneousMatrix wMc(wtc, wRc);

    vpColVector c_p(4); // A point in the circle frame that is on the bbox
    c_p[0] = radius;
    c_p[1] = 0;
    c_p[2] = radius;
    c_p[3] = 1;

    // Matrix to rotate a point by 90 deg around Y in the circle frame
    for (unsigned int i = 0; i < 4; i++) {
      vpColVector w_p(4); // A point in the word frame
      vpHomogeneousMatrix cMc_90(vpTranslationVector(), vpRotationMatrix(0, vpMath::rad(90 * i), 0));
      w_p = wMc * cMc_90 * c_p;

      vpPoint w_P;
      w_P.setWorldCoordinates(w_p[0], w_p[1], w_p[2]);

      polygon.addPoint(i, w_P);
    }
  }

  polygon.setIndex(idFace);
  m_projectionErrorFaces.addPolygon(&polygon);

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

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

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

void vpMbTracker::addProjectionErrorPolygon(const vpPoint &p1, const vpPoint &p2, int idFace, const std::string &polygonName,
                                            bool useLod, double minLineLengthThreshold)
{
  // A polygon as a single line that corresponds to the revolution axis of the
  // cylinder
  vpMbtPolygon polygon;
  polygon.setNbPoint(2);

  polygon.addPoint(0, p1);
  polygon.addPoint(1, p2);

  polygon.setIndex(idFace);
  polygon.setName(polygonName);
  polygon.setLod(useLod);

  polygon.setMinLineLengthThresh(minLineLengthThreshold);
  // Non sense to set minPolygonAreaThreshold for cylinder
  // but used to be coherent when applying LOD settings for all polygons
  polygon.setMinPolygonAreaThresh(minPolygonAreaThresholdGeneral);

  m_projectionErrorFaces.addPolygon(&polygon);

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

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

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

void vpMbTracker::addProjectionErrorPolygon(const std::vector<std::vector<vpPoint> > &listFaces, int idFace,
                                            const std::string &polygonName, bool useLod, double minLineLengthThreshold)
{
  int id = idFace;
  for (unsigned int i = 0; i < listFaces.size(); i++) {
    vpMbtPolygon polygon;
    polygon.setNbPoint((unsigned int)listFaces[i].size());
    for (unsigned int j = 0; j < listFaces[i].size(); j++)
      polygon.addPoint(j, listFaces[i][j]);

    polygon.setIndex(id);
    polygon.setName(polygonName);
    polygon.setIsPolygonOriented(false);
    polygon.setLod(useLod);
    polygon.setMinLineLengthThresh(minLineLengthThreshold);
    polygon.setMinPolygonAreaThresh(minPolygonAreaThresholdGeneral);

    m_projectionErrorFaces.addPolygon(&polygon);

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

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

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

    id++;
  }
}

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

  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin(); it != m_projectionErrorLines.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 = &m_projectionErrorFaces;
    }
  }

  if (!already_here) {
    l = new vpMbtDistanceLine;

    l->setCameraParameters(m_cam);
    l->buildFrom(P1, P2, m_rand);
    l->addPolygon(polygon);
    l->setMovingEdge(&m_projectionErrorMe);
    l->hiddenface = &m_projectionErrorFaces;
    l->useScanLine = useScanLine;

    l->setIndex((unsigned int)m_projectionErrorLines.size());
    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);

    m_projectionErrorLines.push_back(l);
  }
}

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

  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin(); it != m_projectionErrorCircles.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(&m_projectionErrorMe);
    ci->setIndex((unsigned int)m_projectionErrorCircles.size());
    ci->setName(name);
    ci->index_polygon = idFace;
    ci->hiddenface = &m_projectionErrorFaces;

    m_projectionErrorCircles.push_back(ci);
  }
}

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

  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin(); it != m_projectionErrorCylinders.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(&m_projectionErrorMe);
    cy->setIndex((unsigned int)m_projectionErrorCylinders.size());
    cy->setName(name);
    cy->index_polygon = idFace;
    cy->hiddenface = &m_projectionErrorFaces;
    m_projectionErrorCylinders.push_back(cy);
  }
}

void vpMbTracker::initProjectionErrorCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3,
                                            double radius, int idFace, const std::string &name)
{
  addProjectionErrorCircle(p1, p2, p3, radius, idFace, name);
}

void vpMbTracker::initProjectionErrorCylinder(const vpPoint &p1, const vpPoint &p2, double radius,
                                              int idFace, const std::string &name)
{
  addProjectionErrorCylinder(p1, p2, radius, idFace, name);
}

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

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

double vpMbTracker::computeCurrentProjectionError(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &_cMo,
                                                  const vpCameraParameters &_cam)
{
  if (!modelInitialised) {
    throw vpException(vpException::fatalError, "model not initialized");
  }

  unsigned int nbFeatures = 0;
  double totalProjectionError = computeProjectionErrorImpl(I, _cMo, _cam, nbFeatures);

  if (nbFeatures > 0) {
    return vpMath::deg(totalProjectionError / (double)nbFeatures);
  }

  return 90.0;
}

double vpMbTracker::computeProjectionErrorImpl(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &_cMo,
                                               const vpCameraParameters &_cam, unsigned int &nbFeatures)
{
  bool update_cam = m_projectionErrorCam != _cam;
  if (update_cam) {
    m_projectionErrorCam = _cam;

    for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin();
         it != m_projectionErrorLines.end(); ++it) {
      vpMbtDistanceLine *l = *it;
      l->setCameraParameters(m_projectionErrorCam);
    }

    for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin();
         it != m_projectionErrorCylinders.end(); ++it) {
      vpMbtDistanceCylinder *cy = *it;
      cy->setCameraParameters(m_projectionErrorCam);
    }

    for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin();
         it != m_projectionErrorCircles.end(); ++it) {
      vpMbtDistanceCircle *ci = *it;
      ci->setCameraParameters(m_projectionErrorCam);
    }
  }

#ifdef VISP_HAVE_OGRE
  if (useOgre) {
    if (update_cam || !m_projectionErrorFaces.isOgreInitialised()) {
      m_projectionErrorFaces.setBackgroundSizeOgre(I.getHeight(), I.getWidth());
      m_projectionErrorFaces.setOgreShowConfigDialog(m_projectionErrorOgreShowConfigDialog);
      m_projectionErrorFaces.initOgre(m_projectionErrorCam);
      // 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
      m_projectionErrorOgreShowConfigDialog = false;
    }
  }
#endif

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

  projectionErrorVisibleFace(I.getWidth(), I.getHeight(), _cMo);

  projectionErrorResetMovingEdges();

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

    m_projectionErrorFaces.computeClippedPolygons(_cMo, m_projectionErrorCam);
    m_projectionErrorFaces.computeScanLineRender(m_projectionErrorCam, I.getWidth(), I.getHeight());
  }

  projectionErrorInitMovingEdge(I, _cMo);

  double totalProjectionError = 0.0;
  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin(); it != m_projectionErrorLines.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);
          totalProjectionError += lineNormGradient;
          nbFeatures += lineNbFeatures;
        }
      }
    }
  }

  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin();
       it != m_projectionErrorCylinders.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);
        totalProjectionError += 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);
        totalProjectionError += cylinderNormGradient;
        nbFeatures += cylinderNbFeatures;
      }
    }
  }

  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin();
       it != m_projectionErrorCircles.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);
      totalProjectionError += circleNormGradient;
      nbFeatures += circleNbFeatures;
    }
  }

  return totalProjectionError;
}

void vpMbTracker::projectionErrorVisibleFace(unsigned int width, unsigned int height, const vpHomogeneousMatrix &_cMo)
{
  bool changed = false;

  if (!useOgre) {
    m_projectionErrorFaces.setVisible(width, height, m_projectionErrorCam, _cMo, angleAppears, angleDisappears, changed);
  } else {
#ifdef VISP_HAVE_OGRE
     m_projectionErrorFaces.setVisibleOgre(width, height, m_projectionErrorCam, _cMo, angleAppears, angleDisappears, changed);
#else
    m_projectionErrorFaces.setVisible(width, height, m_projectionErrorCam, _cMo, angleAppears, angleDisappears, changed);
#endif
  }
}

void vpMbTracker::projectionErrorResetMovingEdges()
{
  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin(); it != m_projectionErrorLines.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::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin(); it != m_projectionErrorCylinders.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::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin(); it != m_projectionErrorCircles.end(); ++it) {
    if ((*it)->meEllipse != NULL) {
      delete (*it)->meEllipse;
      (*it)->meEllipse = NULL;
    }
    (*it)->nbFeature = 0;
  }
}

void vpMbTracker::projectionErrorInitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &_cMo)
{
  const bool doNotTrack = true;

  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin();
       it != m_projectionErrorLines.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::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin();
       it != m_projectionErrorCylinders.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;
    }

    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::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin();
       it != m_projectionErrorCircles.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 vpMbTracker::loadConfigFile(const std::string &configFile, bool verbose)
{
  vpMbtXmlGenericParser xmlp(vpMbtXmlGenericParser::PROJECTION_ERROR_PARSER);
  xmlp.setVerbose(verbose);
  xmlp.setProjectionErrorMe(m_projectionErrorMe);
  xmlp.setProjectionErrorKernelSize(m_projectionErrorKernelSize);

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

  vpMe meParser;
  xmlp.getProjectionErrorMe(meParser);

  setProjectionErrorMovingEdge(meParser);
  setProjectionErrorKernelSize(xmlp.getProjectionErrorKernelSize());
}

void vpMbTracker::setProjectionErrorMovingEdge(const vpMe &me)
{
  m_projectionErrorMe = me;

  for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_projectionErrorLines.begin(); it != m_projectionErrorLines.end(); ++it) {
    vpMbtDistanceLine *l = *it;
    l->setMovingEdge(&m_projectionErrorMe);
  }

  for (std::vector<vpMbtDistanceCylinder *>::const_iterator it = m_projectionErrorCylinders.begin(); it != m_projectionErrorCylinders.end();
       ++it) {
    vpMbtDistanceCylinder *cy = *it;
    cy->setMovingEdge(&m_projectionErrorMe);
  }

  for (std::vector<vpMbtDistanceCircle *>::const_iterator it = m_projectionErrorCircles.begin(); it != m_projectionErrorCircles.end(); ++it) {
    vpMbtDistanceCircle *ci = *it;
    ci->setMovingEdge(&m_projectionErrorMe);
  }
}

void vpMbTracker::setProjectionErrorKernelSize(const unsigned int &size)
{
  m_projectionErrorKernelSize = size;

  m_SobelX.resize(size*2+1, size*2+1, false, false);
  vpImageFilter::getSobelKernelX(m_SobelX.data, size);

  m_SobelY.resize(size*2+1, size*2+1, false, false);
  vpImageFilter::getSobelKernelY(m_SobelY.data, size);
}