vpMbGenericTracker.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.
 *
 *****************************************************************************/

#include <visp3/mbt/vpMbGenericTracker.h>

#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpExponentialMap.h>
#include <visp3/core/vpTrackingException.h>
#include <visp3/mbt/vpMbtXmlGenericParser.h>

vpMbGenericTracker::vpMbGenericTracker()
  : m_error(), m_L(), m_mapOfCameraTransformationMatrix(), m_mapOfFeatureFactors(), m_mapOfTrackers(),
    m_percentageGdPt(0.4), m_referenceCameraName("Camera"), m_thresholdOutlier(0.5), m_w(), m_weightedError()
{
  m_mapOfTrackers["Camera"] = new TrackerWrapper(EDGE_TRACKER);

  // Add default camera transformation matrix
  m_mapOfCameraTransformationMatrix["Camera"] = vpHomogeneousMatrix();

  // Add default ponderation between each feature type
  m_mapOfFeatureFactors[EDGE_TRACKER] = 1.0;

#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
  m_mapOfFeatureFactors[KLT_TRACKER] = 1.0;
#endif

  m_mapOfFeatureFactors[DEPTH_NORMAL_TRACKER] = 1.0;
  m_mapOfFeatureFactors[DEPTH_DENSE_TRACKER] = 1.0;
}

vpMbGenericTracker::vpMbGenericTracker(const unsigned int nbCameras, const int trackerType)
  : m_error(), m_L(), m_mapOfCameraTransformationMatrix(), m_mapOfFeatureFactors(), m_mapOfTrackers(),
    m_percentageGdPt(0.4), m_referenceCameraName("Camera"), m_thresholdOutlier(0.5), m_w(), m_weightedError()
{
  if (nbCameras == 0) {
    throw vpException(vpTrackingException::fatalError, "Cannot use no camera!");
  } else if (nbCameras == 1) {
    m_mapOfTrackers["Camera"] = new TrackerWrapper(trackerType);

    // Add default camera transformation matrix
    m_mapOfCameraTransformationMatrix["Camera"] = vpHomogeneousMatrix();
  } else {
    for (unsigned int i = 1; i <= nbCameras; i++) {
      std::stringstream ss;
      ss << "Camera" << i;
      m_mapOfTrackers[ss.str()] = new TrackerWrapper(trackerType);

      // Add default camera transformation matrix
      m_mapOfCameraTransformationMatrix[ss.str()] = vpHomogeneousMatrix();
    }

    // Set by default the reference camera to the first one
    m_referenceCameraName = m_mapOfTrackers.begin()->first;
  }

  // Add default ponderation between each feature type
  m_mapOfFeatureFactors[EDGE_TRACKER] = 1.0;

#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
  m_mapOfFeatureFactors[KLT_TRACKER] = 1.0;
#endif

  m_mapOfFeatureFactors[DEPTH_NORMAL_TRACKER] = 1.0;
  m_mapOfFeatureFactors[DEPTH_DENSE_TRACKER] = 1.0;
}

vpMbGenericTracker::vpMbGenericTracker(const std::vector<int> &trackerTypes)
  : m_error(), m_L(), m_mapOfCameraTransformationMatrix(), m_mapOfFeatureFactors(), m_mapOfTrackers(),
    m_percentageGdPt(0.4), m_referenceCameraName("Camera"), m_thresholdOutlier(0.5), m_w(), m_weightedError()
{
  if (trackerTypes.empty()) {
    throw vpException(vpException::badValue, "There is no camera!");
  }

  if (trackerTypes.size() == 1) {
    m_mapOfTrackers["Camera"] = new TrackerWrapper(trackerTypes[0]);

    // Add default camera transformation matrix
    m_mapOfCameraTransformationMatrix["Camera"] = vpHomogeneousMatrix();
  } else {
    for (size_t i = 1; i <= trackerTypes.size(); i++) {
      std::stringstream ss;
      ss << "Camera" << i;
      m_mapOfTrackers[ss.str()] = new TrackerWrapper(trackerTypes[i - 1]);

      // Add default camera transformation matrix
      m_mapOfCameraTransformationMatrix[ss.str()] = vpHomogeneousMatrix();
    }

    // Set by default the reference camera to the first one
    m_referenceCameraName = m_mapOfTrackers.begin()->first;
  }

  // Add default ponderation between each feature type
  m_mapOfFeatureFactors[EDGE_TRACKER] = 1.0;

#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
  m_mapOfFeatureFactors[KLT_TRACKER] = 1.0;
#endif

  m_mapOfFeatureFactors[DEPTH_NORMAL_TRACKER] = 1.0;
  m_mapOfFeatureFactors[DEPTH_DENSE_TRACKER] = 1.0;
}

vpMbGenericTracker::vpMbGenericTracker(const std::vector<std::string> &cameraNames,
                                       const std::vector<int> &trackerTypes)
  : m_error(), m_L(), m_mapOfCameraTransformationMatrix(), m_mapOfFeatureFactors(), m_mapOfTrackers(),
    m_percentageGdPt(0.4), m_referenceCameraName("Camera"), m_thresholdOutlier(0.5), m_w(), m_weightedError()
{
  if (cameraNames.size() != trackerTypes.size() || cameraNames.empty()) {
    throw vpException(vpTrackingException::badValue,
                      "cameraNames.size() != trackerTypes.size() || cameraNames.empty()");
  }

  for (size_t i = 0; i < cameraNames.size(); i++) {
    m_mapOfTrackers[cameraNames[i]] = new TrackerWrapper(trackerTypes[i]);

    // Add default camera transformation matrix
    m_mapOfCameraTransformationMatrix[cameraNames[i]] = vpHomogeneousMatrix();
  }

  // Set by default the reference camera to the first one
  m_referenceCameraName = m_mapOfTrackers.begin()->first;

  // Add default ponderation between each feature type
  m_mapOfFeatureFactors[EDGE_TRACKER] = 1.0;

#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
  m_mapOfFeatureFactors[KLT_TRACKER] = 1.0;
#endif

  m_mapOfFeatureFactors[DEPTH_NORMAL_TRACKER] = 1.0;
  m_mapOfFeatureFactors[DEPTH_DENSE_TRACKER] = 1.0;
}

vpMbGenericTracker::~vpMbGenericTracker()
{
  for (std::map<std::string, TrackerWrapper *>::iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end();
       ++it) {
    delete it->second;
    it->second = NULL;
  }
}

double vpMbGenericTracker::computeCurrentProjectionError(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &_cMo,
                                                         const vpCameraParameters &_cam)
{
  double rawTotalProjectionError = 0.0;
  unsigned int nbTotalFeaturesUsed = 0;

  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;

    unsigned int nbFeaturesUsed = 0;
    double curProjError = tracker->computeProjectionErrorImpl(I, _cMo, _cam, nbFeaturesUsed);

    if (nbFeaturesUsed > 0) {
      nbTotalFeaturesUsed += nbFeaturesUsed;
      rawTotalProjectionError += curProjError;
    }
  }

  if (nbTotalFeaturesUsed > 0) {
    return vpMath::deg(rawTotalProjectionError / (double)nbTotalFeaturesUsed);
  }

  return 90.0;
}

void vpMbGenericTracker::computeProjectionError()
{
  if (computeProjError) {
    double rawTotalProjectionError = 0.0;
    unsigned int nbTotalFeaturesUsed = 0;

    for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
         it != m_mapOfTrackers.end(); ++it) {
      TrackerWrapper *tracker = it->second;

      double curProjError = tracker->getProjectionError();
      unsigned int nbFeaturesUsed = tracker->nbFeaturesForProjErrorComputation;

      if (nbFeaturesUsed > 0) {
        nbTotalFeaturesUsed += nbFeaturesUsed;
        rawTotalProjectionError += (vpMath::rad(curProjError) * nbFeaturesUsed);
      }
    }

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

void vpMbGenericTracker::computeVVS(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages)
{
  computeVVSInit(mapOfImages);

  if (m_error.getRows() < 4) {
    throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
  }

  double normRes = 0;
  double normRes_1 = -1;
  unsigned int iter = 0;

  vpMatrix LTL;
  vpColVector LTR, v;
  vpColVector error_prev;

  double mu = m_initialMu;
  vpHomogeneousMatrix cMo_prev;

  bool isoJoIdentity_ = true;

  // Covariance
  vpColVector W_true(m_error.getRows());
  vpMatrix L_true, LVJ_true;

  // Create the map of VelocityTwistMatrices
  std::map<std::string, vpVelocityTwistMatrix> mapOfVelocityTwist;
  for (std::map<std::string, vpHomogeneousMatrix>::const_iterator it = m_mapOfCameraTransformationMatrix.begin();
       it != m_mapOfCameraTransformationMatrix.end(); ++it) {
    vpVelocityTwistMatrix cVo;
    cVo.buildFrom(it->second);
    mapOfVelocityTwist[it->first] = cVo;
  }

  double factorEdge = m_mapOfFeatureFactors[EDGE_TRACKER];
#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
  double factorKlt = m_mapOfFeatureFactors[KLT_TRACKER];
#endif
  double factorDepth = m_mapOfFeatureFactors[DEPTH_NORMAL_TRACKER];
  double factorDepthDense = m_mapOfFeatureFactors[DEPTH_DENSE_TRACKER];

  while (std::fabs(normRes_1 - normRes) > m_stopCriteriaEpsilon && (iter < m_maxIter)) {
    computeVVSInteractionMatrixAndResidu(mapOfImages, mapOfVelocityTwist);

    bool reStartFromLastIncrement = false;
    computeVVSCheckLevenbergMarquardt(iter, m_error, error_prev, cMo_prev, mu, reStartFromLastIncrement);
    if (reStartFromLastIncrement) {
      for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
           it != m_mapOfTrackers.end(); ++it) {
        TrackerWrapper *tracker = it->second;

        tracker->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo_prev;

#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
        vpHomogeneousMatrix c_curr_tTc_curr0 =
            m_mapOfCameraTransformationMatrix[it->first] * cMo_prev * tracker->c0Mo.inverse();
        tracker->ctTc0 = c_curr_tTc_curr0;
#endif
      }
    }

    if (!reStartFromLastIncrement) {
      computeVVSWeights();

      if (computeCovariance) {
        L_true = m_L;
        if (!isoJoIdentity_) {
          vpVelocityTwistMatrix cVo;
          cVo.buildFrom(cMo);
          LVJ_true = (m_L * (cVo * oJo));
        }
      }

      vpVelocityTwistMatrix cVo;
      if (iter == 0) {
        isoJoIdentity_ = true;
        oJo.eye();

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

          vpMatrix K; // kernel
          unsigned int rank = (m_L * cVo).kernel(K);
          if (rank == 0) {
            throw vpException(vpException::fatalError, "Rank=0, cannot estimate the pose !");
          }

          if (rank != 6) {
            vpMatrix I; // Identity
            I.eye(6);
            oJo = I - K.AtA();

            isoJoIdentity_ = false;
          }
        }
      }

      // Weighting
      double num = 0;
      double den = 0;

      unsigned int start_index = 0;
      for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
           it != m_mapOfTrackers.end(); ++it) {
        TrackerWrapper *tracker = it->second;

        if (tracker->m_trackerType & EDGE_TRACKER) {
          for (unsigned int i = 0; i < tracker->m_error_edge.getRows(); i++) {
            double wi = tracker->m_w_edge[i] * tracker->m_factor[i] * factorEdge;
            W_true[start_index + i] = wi;
            m_weightedError[start_index + i] = wi * m_error[start_index + i];

            num += wi * vpMath::sqr(m_error[start_index + i]);
            den += wi;

            for (unsigned int j = 0; j < m_L.getCols(); j++) {
              m_L[start_index + i][j] *= wi;
            }
          }

          start_index += tracker->m_error_edge.getRows();
        }

#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
        if (tracker->m_trackerType & KLT_TRACKER) {
          for (unsigned int i = 0; i < tracker->m_error_klt.getRows(); i++) {
            double wi = tracker->m_w_klt[i] * factorKlt;
            W_true[start_index + i] = wi;
            m_weightedError[start_index + i] = wi * m_error[start_index + i];

            num += wi * vpMath::sqr(m_error[start_index + i]);
            den += wi;

            for (unsigned int j = 0; j < m_L.getCols(); j++) {
              m_L[start_index + i][j] *= wi;
            }
          }

          start_index += tracker->m_error_klt.getRows();
        }
#endif

        if (tracker->m_trackerType & DEPTH_NORMAL_TRACKER) {
          for (unsigned int i = 0; i < tracker->m_error_depthNormal.getRows(); i++) {
            double wi = tracker->m_w_depthNormal[i] * factorDepth;
            W_true[start_index + i] = wi;
            m_weightedError[start_index + i] = wi * m_error[start_index + i];

            num += wi * vpMath::sqr(m_error[start_index + i]);
            den += wi;

            for (unsigned int j = 0; j < m_L.getCols(); j++) {
              m_L[start_index + i][j] *= wi;
            }
          }

          start_index += tracker->m_error_depthNormal.getRows();
        }

        if (tracker->m_trackerType & DEPTH_DENSE_TRACKER) {
          for (unsigned int i = 0; i < tracker->m_error_depthDense.getRows(); i++) {
            double wi = tracker->m_w_depthDense[i] * factorDepthDense;
            W_true[start_index + i] = wi;
            m_weightedError[start_index + i] = wi * m_error[start_index + i];

            num += wi * vpMath::sqr(m_error[start_index + i]);
            den += wi;

            for (unsigned int j = 0; j < m_L.getCols(); j++) {
              m_L[start_index + i][j] *= wi;
            }
          }

          start_index += tracker->m_error_depthDense.getRows();
        }
      }

      normRes_1 = normRes;
      normRes = sqrt(num / den);

      computeVVSPoseEstimation(isoJoIdentity_, iter, m_L, LTL, m_weightedError, m_error, error_prev, LTR, mu, v);

      cMo_prev = cMo;

      cMo = vpExponentialMap::direct(v).inverse() * cMo;

#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
      for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
           it != m_mapOfTrackers.end(); ++it) {
        TrackerWrapper *tracker = it->second;

        vpHomogeneousMatrix c_curr_tTc_curr0 =
            m_mapOfCameraTransformationMatrix[it->first] * cMo * tracker->c0Mo.inverse();
        tracker->ctTc0 = c_curr_tTc_curr0;
      }
#endif

      // Update cMo
      for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
           it != m_mapOfTrackers.end(); ++it) {
        TrackerWrapper *tracker = it->second;
        tracker->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo;
      }
    }

    iter++;
  }

  computeCovarianceMatrixVVS(isoJoIdentity_, W_true, cMo_prev, L_true, LVJ_true, m_error);

  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;

    if (tracker->m_trackerType & EDGE_TRACKER) {
      tracker->updateMovingEdgeWeights();
    }
  }
}

void vpMbGenericTracker::computeVVSInit()
{
  throw vpException(vpException::fatalError, "vpMbGenericTracker::computeVVSInit() should not be called!");
}

void vpMbGenericTracker::computeVVSInit(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages)
{
  unsigned int nbFeatures = 0;

  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->computeVVSInit(mapOfImages[it->first]);

    nbFeatures += tracker->m_error.getRows();
  }

  m_L.resize(nbFeatures, 6, false, false);
  m_error.resize(nbFeatures, false);

  m_weightedError.resize(nbFeatures, false);
  m_w.resize(nbFeatures, false);
  m_w = 1;
}

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

void vpMbGenericTracker::computeVVSInteractionMatrixAndResidu(
    std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
    std::map<std::string, vpVelocityTwistMatrix> &mapOfVelocityTwist)
{
  unsigned int start_index = 0;

  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;

    tracker->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo;
#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
    vpHomogeneousMatrix c_curr_tTc_curr0 = m_mapOfCameraTransformationMatrix[it->first] * cMo * tracker->c0Mo.inverse();
    tracker->ctTc0 = c_curr_tTc_curr0;
#endif

    tracker->computeVVSInteractionMatrixAndResidu(mapOfImages[it->first]);

    m_L.insert(tracker->m_L * mapOfVelocityTwist[it->first], start_index, 0);
    m_error.insert(start_index, tracker->m_error);

    start_index += tracker->m_error.getRows();
  }
}

void vpMbGenericTracker::computeVVSWeights()
{
  unsigned int start_index = 0;

  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->computeVVSWeights();

    m_w.insert(start_index, tracker->m_w);
    start_index += tracker->m_w.getRows();
  }
}

void vpMbGenericTracker::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo_,
                                 const vpCameraParameters &cam_, const vpColor &col, const unsigned int thickness,
                                 const bool displayFullModel)
{
  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
  if (it != m_mapOfTrackers.end()) {
    TrackerWrapper *tracker = it->second;
    tracker->display(I, cMo_, cam_, col, thickness, displayFullModel);
  } else {
    std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
  }
}

void vpMbGenericTracker::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo_,
                                 const vpCameraParameters &cam_, const vpColor &col, const unsigned int thickness,
                                 const bool displayFullModel)
{
  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
  if (it != m_mapOfTrackers.end()) {
    TrackerWrapper *tracker = it->second;
    tracker->display(I, cMo_, cam_, col, thickness, displayFullModel);
  } else {
    std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
  }
}

void vpMbGenericTracker::display(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
                                 const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
                                 const vpCameraParameters &cam1, const vpCameraParameters &cam2, const vpColor &color,
                                 const unsigned int thickness, const bool displayFullModel)
{
  if (m_mapOfTrackers.size() == 2) {
    std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
    it->second->display(I1, c1Mo, cam1, color, thickness, displayFullModel);
    ++it;

    it->second->display(I2, c2Mo, cam2, color, thickness, displayFullModel);
  } else {
    std::cerr << "The tracker is not set as a stereo configuration! There are " << m_mapOfTrackers.size() << " cameras!"
              << std::endl;
  }
}

void vpMbGenericTracker::display(const vpImage<vpRGBa> &I1, const vpImage<vpRGBa> &I2, const vpHomogeneousMatrix &c1Mo,
                                 const vpHomogeneousMatrix &c2Mo, const vpCameraParameters &cam1,
                                 const vpCameraParameters &cam2, const vpColor &color, const unsigned int thickness,
                                 const bool displayFullModel)
{
  if (m_mapOfTrackers.size() == 2) {
    std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
    it->second->display(I1, c1Mo, cam1, color, thickness, displayFullModel);
    ++it;

    it->second->display(I2, c2Mo, cam2, color, thickness, displayFullModel);
  } else {
    std::cerr << "The tracker is not set as a stereo configuration! There are " << m_mapOfTrackers.size() << " cameras!"
              << std::endl;
  }
}

void vpMbGenericTracker::display(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                                 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
                                 const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
                                 const vpColor &col, const unsigned int thickness, const bool displayFullModel)
{
  // Display only for the given images
  for (std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.begin();
       it_img != mapOfImages.end(); ++it_img) {
    std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it_img->first);
    std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(it_img->first);
    std::map<std::string, vpCameraParameters>::const_iterator it_cam = mapOfCameraParameters.find(it_img->first);

    if (it_tracker != m_mapOfTrackers.end() && it_camPose != mapOfCameraPoses.end() &&
        it_cam != mapOfCameraParameters.end()) {
      TrackerWrapper *tracker = it_tracker->second;
      tracker->display(*it_img->second, it_camPose->second, it_cam->second, col, thickness, displayFullModel);
    } else {
      std::cerr << "Missing elements for image:" << it_img->first << "!" << std::endl;
    }
  }
}

void vpMbGenericTracker::display(const std::map<std::string, const vpImage<vpRGBa> *> &mapOfImages,
                                 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
                                 const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
                                 const vpColor &col, const unsigned int thickness, const bool displayFullModel)
{
  // Display only for the given images
  for (std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img = mapOfImages.begin();
       it_img != mapOfImages.end(); ++it_img) {
    std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it_img->first);
    std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(it_img->first);
    std::map<std::string, vpCameraParameters>::const_iterator it_cam = mapOfCameraParameters.find(it_img->first);

    if (it_tracker != m_mapOfTrackers.end() && it_camPose != mapOfCameraPoses.end() &&
        it_cam != mapOfCameraParameters.end()) {
      TrackerWrapper *tracker = it_tracker->second;
      tracker->display(*it_img->second, it_camPose->second, it_cam->second, col, thickness, displayFullModel);
    } else {
      std::cerr << "Missing elements for image:" << it_img->first << "!" << std::endl;
    }
  }
}

std::vector<std::string> vpMbGenericTracker::getCameraNames() const
{
  std::vector<std::string> cameraNames;

  for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.begin();
       it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
    cameraNames.push_back(it_tracker->first);
  }

  return cameraNames;
}

void vpMbGenericTracker::getCameraParameters(vpCameraParameters &cam1, vpCameraParameters &cam2) const
{
  if (m_mapOfTrackers.size() == 2) {
    std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
    it->second->getCameraParameters(cam1);
    ++it;

    it->second->getCameraParameters(cam2);
  } else {
    std::cerr << "The tracker is not set as a stereo configuration! There are " << m_mapOfTrackers.size() << " cameras!"
              << std::endl;
  }
}

void vpMbGenericTracker::getCameraParameters(std::map<std::string, vpCameraParameters> &mapOfCameraParameters) const
{
  // Clear the input map
  mapOfCameraParameters.clear();

  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    vpCameraParameters cam_;
    it->second->getCameraParameters(cam_);
    mapOfCameraParameters[it->first] = cam_;
  }
}

std::map<std::string, int> vpMbGenericTracker::getCameraTrackerTypes() const
{
  std::map<std::string, int> trackingTypes;

  TrackerWrapper *traker;
  for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.begin();
       it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
    traker = it_tracker->second;
    trackingTypes[it_tracker->first] = traker->getTrackerType();
  }

  return trackingTypes;
}

void vpMbGenericTracker::getClipping(unsigned int &clippingFlag1, unsigned int &clippingFlag2) const
{
  if (m_mapOfTrackers.size() == 2) {
    std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
    clippingFlag1 = it->second->getClipping();
    ++it;

    clippingFlag2 = it->second->getClipping();
  } else {
    std::cerr << "The tracker is not set as a stereo configuration! There are " << m_mapOfTrackers.size() << " cameras!"
              << std::endl;
  }
}

void vpMbGenericTracker::getClipping(std::map<std::string, unsigned int> &mapOfClippingFlags) const
{
  mapOfClippingFlags.clear();

  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    mapOfClippingFlags[it->first] = tracker->getClipping();
  }
}

vpMbHiddenFaces<vpMbtPolygon> &vpMbGenericTracker::getFaces()
{
  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
  if (it != m_mapOfTrackers.end()) {
    return it->second->getFaces();
  }

  std::cerr << "The reference camera: " << m_referenceCameraName << " cannot be found!" << std::endl;
  return faces;
}

vpMbHiddenFaces<vpMbtPolygon> &vpMbGenericTracker::getFaces(const std::string &cameraName)
{
  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(cameraName);
  if (it != m_mapOfTrackers.end()) {
    return it->second->getFaces();
  }

  std::cerr << "The camera: " << cameraName << " cannot be found!" << std::endl;
  return faces;
}

#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))

std::list<vpMbtDistanceCircle *> &vpMbGenericTracker::getFeaturesCircle()
{
  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
  if (it != m_mapOfTrackers.end()) {
    TrackerWrapper *tracker = it->second;
    return tracker->getFeaturesCircle();
  } else {
    throw vpException(vpTrackingException::badValue, "Cannot find the reference camera:  %s!",
                      m_referenceCameraName.c_str());
  }
}

std::list<vpMbtDistanceKltCylinder *> &vpMbGenericTracker::getFeaturesKltCylinder()
{
  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
  if (it != m_mapOfTrackers.end()) {
    TrackerWrapper *tracker = it->second;
    return tracker->getFeaturesKltCylinder();
  } else {
    throw vpException(vpTrackingException::badValue, "Cannot find the reference camera:  %s!",
                      m_referenceCameraName.c_str());
  }
}

std::list<vpMbtDistanceKltPoints *> &vpMbGenericTracker::getFeaturesKlt()
{
  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
  if (it != m_mapOfTrackers.end()) {
    TrackerWrapper *tracker = it->second;
    return tracker->getFeaturesKlt();
  } else {
    throw vpException(vpTrackingException::badValue, "Cannot find the reference camera:  %s!",
                      m_referenceCameraName.c_str());
  }
}
#endif

double vpMbGenericTracker::getGoodMovingEdgesRatioThreshold() const { return m_percentageGdPt; }

#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))

std::vector<vpImagePoint> vpMbGenericTracker::getKltImagePoints() const
{
  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
  if (it != m_mapOfTrackers.end()) {
    TrackerWrapper *tracker = it->second;
    return tracker->getKltImagePoints();
  } else {
    std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
  }

  return std::vector<vpImagePoint>();
}

std::map<int, vpImagePoint> vpMbGenericTracker::getKltImagePointsWithId() const
{
  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
  if (it != m_mapOfTrackers.end()) {
    TrackerWrapper *tracker = it->second;
    return tracker->getKltImagePointsWithId();
  } else {
    std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
  }

  return std::map<int, vpImagePoint>();
}

unsigned int vpMbGenericTracker::getKltMaskBorder() const
{
  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
  if (it != m_mapOfTrackers.end()) {
    TrackerWrapper *tracker = it->second;
    return tracker->getKltMaskBorder();
  } else {
    std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
  }

  return 0;
}

int vpMbGenericTracker::getKltNbPoints() const
{
  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
  if (it != m_mapOfTrackers.end()) {
    TrackerWrapper *tracker = it->second;
    return tracker->getKltNbPoints();
  } else {
    std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
  }

  return 0;
}

vpKltOpencv vpMbGenericTracker::getKltOpencv() const
{
  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);

  if (it_tracker != m_mapOfTrackers.end()) {
    TrackerWrapper *tracker;
    tracker = it_tracker->second;
    return tracker->getKltOpencv();
  } else {
    std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
  }

  return vpKltOpencv();
}

void vpMbGenericTracker::getKltOpencv(vpKltOpencv &klt1, vpKltOpencv &klt2) const
{
  if (m_mapOfTrackers.size() == 2) {
    std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
    klt1 = it->second->getKltOpencv();
    ++it;

    klt2 = it->second->getKltOpencv();
  } else {
    std::cerr << "The tracker is not set as a stereo configuration! There are " << m_mapOfTrackers.size() << " cameras!"
              << std::endl;
  }
}

void vpMbGenericTracker::getKltOpencv(std::map<std::string, vpKltOpencv> &mapOfKlts) const
{
  mapOfKlts.clear();

  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    mapOfKlts[it->first] = tracker->getKltOpencv();
  }
}

#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)

std::vector<cv::Point2f> vpMbGenericTracker::getKltPoints() const
{
  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
  if (it != m_mapOfTrackers.end()) {
    TrackerWrapper *tracker = it->second;
    return tracker->getKltPoints();
  } else {
    std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
  }

  return std::vector<cv::Point2f>();
}
#endif

double vpMbGenericTracker::getKltThresholdAcceptation() const { return m_thresholdOutlier; }
#endif

void vpMbGenericTracker::getLcircle(std::list<vpMbtDistanceCircle *> &circlesList,
                                    const unsigned int level) const
{
  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
  if (it != m_mapOfTrackers.end()) {
    it->second->getLcircle(circlesList, level);
  } else {
    std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist!" << std::endl;
  }
}

void vpMbGenericTracker::getLcircle(const std::string &cameraName, std::list<vpMbtDistanceCircle *> &circlesList,
                                    const unsigned int level) const
{
  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(cameraName);
  if (it != m_mapOfTrackers.end()) {
    it->second->getLcircle(circlesList, level);
  } else {
    std::cerr << "The camera: " << cameraName << " does not exist!" << std::endl;
  }
}

void vpMbGenericTracker::getLcylinder(std::list<vpMbtDistanceCylinder *> &cylindersList,
                                      const unsigned int level) const
{
  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
  if (it != m_mapOfTrackers.end()) {
    it->second->getLcylinder(cylindersList, level);
  } else {
    std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist!" << std::endl;
  }
}

void vpMbGenericTracker::getLcylinder(const std::string &cameraName, std::list<vpMbtDistanceCylinder *> &cylindersList,
                                      const unsigned int level) const
{
  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(cameraName);
  if (it != m_mapOfTrackers.end()) {
    it->second->getLcylinder(cylindersList, level);
  } else {
    std::cerr << "The camera: " << cameraName << " does not exist!" << std::endl;
  }
}

void vpMbGenericTracker::getLline(std::list<vpMbtDistanceLine *> &linesList,
                                  const unsigned int level) const
{
  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);

  if (it != m_mapOfTrackers.end()) {
    it->second->getLline(linesList, level);
  } else {
    std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist!" << std::endl;
  }
}

void vpMbGenericTracker::getLline(const std::string &cameraName, std::list<vpMbtDistanceLine *> &linesList,
                                  const unsigned int level) const
{
  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(cameraName);
  if (it != m_mapOfTrackers.end()) {
    it->second->getLline(linesList, level);
  } else {
    std::cerr << "The camera: " << cameraName << " does not exist!" << std::endl;
  }
}

vpMe vpMbGenericTracker::getMovingEdge() const
{
  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);

  if (it != m_mapOfTrackers.end()) {
    return it->second->getMovingEdge();
  } else {
    std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist!" << std::endl;
  }

  return vpMe();
}

void vpMbGenericTracker::getMovingEdge(vpMe &me1, vpMe &me2) const
{
  if (m_mapOfTrackers.size() == 2) {
    std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
    it->second->getMovingEdge(me1);
    ++it;

    it->second->getMovingEdge(me2);
  } else {
    std::cerr << "The tracker is not set as a stereo configuration! There are " << m_mapOfTrackers.size() << " cameras!"
              << std::endl;
  }
}

void vpMbGenericTracker::getMovingEdge(std::map<std::string, vpMe> &mapOfMovingEdges) const
{
  mapOfMovingEdges.clear();

  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    mapOfMovingEdges[it->first] = tracker->getMovingEdge();
  }
}

unsigned int vpMbGenericTracker::getNbPoints(const unsigned int level) const
{
  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);

  unsigned int nbGoodPoints = 0;
  if (it != m_mapOfTrackers.end()) {

    nbGoodPoints = it->second->getNbPoints(level);
  } else {
    std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist!" << std::endl;
  }

  return nbGoodPoints;
}

void vpMbGenericTracker::getNbPoints(std::map<std::string, unsigned int> &mapOfNbPoints, const unsigned int level) const
{
  mapOfNbPoints.clear();

  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    mapOfNbPoints[it->first] = tracker->getNbPoints(level);
  }
}

unsigned int vpMbGenericTracker::getNbPolygon() const
{
  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
  if (it != m_mapOfTrackers.end()) {
    return it->second->getNbPolygon();
  }

  std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist!" << std::endl;
  return 0;
}

void vpMbGenericTracker::getNbPolygon(std::map<std::string, unsigned int> &mapOfNbPolygons) const
{
  mapOfNbPolygons.clear();

  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    mapOfNbPolygons[it->first] = tracker->getNbPolygon();
  }
}

vpMbtPolygon *vpMbGenericTracker::getPolygon(const unsigned int index)
{
  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
  if (it != m_mapOfTrackers.end()) {
    return it->second->getPolygon(index);
  }

  std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist!" << std::endl;
  return NULL;
}

vpMbtPolygon *vpMbGenericTracker::getPolygon(const std::string &cameraName, const unsigned int index)
{
  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(cameraName);
  if (it != m_mapOfTrackers.end()) {
    return it->second->getPolygon(index);
  }

  std::cerr << "The camera: " << cameraName << " does not exist!" << std::endl;
  return NULL;
}

std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > >
vpMbGenericTracker::getPolygonFaces(const bool orderPolygons, const bool useVisibility, const bool clipPolygon)
{
  std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > polygonFaces;

  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
  if (it != m_mapOfTrackers.end()) {
    TrackerWrapper *tracker = it->second;
    polygonFaces = tracker->getPolygonFaces(orderPolygons, useVisibility, clipPolygon);
  } else {
    std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
  }

  return polygonFaces;
}

void vpMbGenericTracker::getPolygonFaces(std::map<std::string, std::vector<vpPolygon> > &mapOfPolygons,
                                         std::map<std::string, std::vector<std::vector<vpPoint> > > &mapOfPoints,
                                         const bool orderPolygons, const bool useVisibility, const bool clipPolygon)
{
  mapOfPolygons.clear();
  mapOfPoints.clear();

  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > polygonFaces =
        tracker->getPolygonFaces(orderPolygons, useVisibility, clipPolygon);

    mapOfPolygons[it->first] = polygonFaces.first;
    mapOfPoints[it->first] = polygonFaces.second;
  }
}

void vpMbGenericTracker::getPose(vpHomogeneousMatrix &c1Mo, vpHomogeneousMatrix &c2Mo) const
{
  if (m_mapOfTrackers.size() == 2) {
    std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
    it->second->getPose(c1Mo);
    ++it;

    it->second->getPose(c2Mo);
  } else {
    std::cerr << "The tracker is not set as a stereo configuration! There are " << m_mapOfTrackers.size() << " cameras!"
              << std::endl;
  }
}

void vpMbGenericTracker::getPose(std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses) const
{
  // Clear the map
  mapOfCameraPoses.clear();

  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->getPose(mapOfCameraPoses[it->first]);
  }
}

int vpMbGenericTracker::getTrackerType() const
{
  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
  if (it != m_mapOfTrackers.end()) {
    TrackerWrapper *tracker = it->second;
    return tracker->getTrackerType();
  } else {
    throw vpException(vpTrackingException::badValue, "Cannot find the reference camera: %s!",
                      m_referenceCameraName.c_str());
  }
}

void vpMbGenericTracker::init(const vpImage<unsigned char> &I)
{
  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo;
    tracker->init(I);
  }
}

void vpMbGenericTracker::initCircle(const vpPoint & /*p1*/, const vpPoint & /*p2*/, const vpPoint & /*p3*/,
                                    const double /*radius*/, const int /*idFace*/, const std::string & /*name*/)
{
  throw vpException(vpException::fatalError, "vpMbGenericTracker::initCircle() should not be called!");
}

#ifdef VISP_HAVE_MODULE_GUI

void vpMbGenericTracker::initClick(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
                                   const std::string &initFile1, const std::string &initFile2, const bool displayHelp,
                                   const vpHomogeneousMatrix &T1, const vpHomogeneousMatrix &T2)
{
  if (m_mapOfTrackers.size() == 2) {
    std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
    TrackerWrapper *tracker = it->second;
    tracker->initClick(I1, initFile1, displayHelp, T1);

    ++it;

    tracker = it->second;
    tracker->initClick(I2, initFile2, displayHelp, T2);

    it = m_mapOfTrackers.find(m_referenceCameraName);
    if (it != m_mapOfTrackers.end()) {
      tracker = it->second;

      // Set the reference cMo
      tracker->getPose(cMo);
    }
  } else {
    throw vpException(vpTrackingException::initializationError,
                      "Cannot initClick()! Require two cameras but there are %d cameras!", m_mapOfTrackers.size());
  }
}

void vpMbGenericTracker::initClick(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                                   const std::map<std::string, std::string> &mapOfInitFiles, const bool displayHelp,
                                   const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
{
  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
      mapOfImages.find(m_referenceCameraName);
  std::map<std::string, std::string>::const_iterator it_initFile = mapOfInitFiles.find(m_referenceCameraName);

  if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_initFile != mapOfInitFiles.end()) {
    TrackerWrapper *tracker = it_tracker->second;
    std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
    if (it_T != mapOfT.end())
      tracker->initClick(*it_img->second, it_initFile->second, displayHelp, it_T->second);
    else
      tracker->initClick(*it_img->second, it_initFile->second, displayHelp);
    tracker->getPose(cMo);
  } else {
    throw vpException(vpTrackingException::initializationError, "Cannot initClick for the reference camera!");
  }

  // Vector of missing initFile for cameras
  std::vector<std::string> vectorOfMissingCameraPoses;

  // Set pose for the specified cameras
  for (it_tracker = m_mapOfTrackers.begin(); it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
    if (it_tracker->first != m_referenceCameraName) {
      it_img = mapOfImages.find(it_tracker->first);
      it_initFile = mapOfInitFiles.find(it_tracker->first);

      if (it_img != mapOfImages.end() && it_initFile != mapOfInitFiles.end()) {
        // InitClick for the current camera
        TrackerWrapper *tracker = it_tracker->second;
        tracker->initClick(*it_img->second, it_initFile->second, displayHelp);
      } else {
        vectorOfMissingCameraPoses.push_back(it_tracker->first);
      }
    }
  }

  // Init for cameras that do not have an initFile
  for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
       it != vectorOfMissingCameraPoses.end(); ++it) {
    it_img = mapOfImages.find(*it);
    std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
        m_mapOfCameraTransformationMatrix.find(*it);

    if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
      vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo;
      m_mapOfTrackers[*it]->cMo = cCurrentMo;
      m_mapOfTrackers[*it]->init(*it_img->second);
    } else {
      throw vpException(vpTrackingException::initializationError,
                        "Missing image or missing camera transformation "
                        "matrix! Cannot set the pose for camera: %s!",
                        it->c_str());
    }
  }
}
#endif

void vpMbGenericTracker::initCylinder(const vpPoint & /*p1*/, const vpPoint & /*p2*/, const double /*radius*/,
                                      const int /*idFace*/, const std::string & /*name*/)
{
  throw vpException(vpException::fatalError, "vpMbGenericTracker::initCylinder() should not be called!");
}

void vpMbGenericTracker::initFaceFromCorners(vpMbtPolygon & /*polygon*/)
{
  throw vpException(vpException::fatalError, "vpMbGenericTracker::initFaceFromCorners() should not be called!");
}

void vpMbGenericTracker::initFaceFromLines(vpMbtPolygon & /*polygon*/)
{
  throw vpException(vpException::fatalError, "vpMbGenericTracker::initFaceFromLines() should not be called!");
}

void vpMbGenericTracker::initFromPoints(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
                                        const std::string &initFile1, const std::string &initFile2)
{
  if (m_mapOfTrackers.size() == 2) {
    std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
    TrackerWrapper *tracker = it->second;
    tracker->initFromPoints(I1, initFile1);

    ++it;

    tracker = it->second;
    tracker->initFromPoints(I2, initFile2);

    it = m_mapOfTrackers.find(m_referenceCameraName);
    if (it != m_mapOfTrackers.end()) {
      tracker = it->second;

      // Set the reference cMo
      tracker->getPose(cMo);

      // Set the reference camera parameters
      tracker->getCameraParameters(cam);
    }
  } else {
    throw vpException(vpTrackingException::initializationError,
                      "Cannot initFromPoints()! Require two cameras but "
                      "there are %d cameras!",
                      m_mapOfTrackers.size());
  }
}

void vpMbGenericTracker::initFromPoints(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                                        const std::map<std::string, std::string> &mapOfInitPoints)
{
  // Set the reference cMo
  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
      mapOfImages.find(m_referenceCameraName);
  std::map<std::string, std::string>::const_iterator it_initPoints = mapOfInitPoints.find(m_referenceCameraName);

  if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_initPoints != mapOfInitPoints.end()) {
    TrackerWrapper *tracker = it_tracker->second;
    tracker->initFromPoints(*it_img->second, it_initPoints->second);
    tracker->getPose(cMo);
  } else {
    throw vpException(vpTrackingException::initializationError, "Cannot initFromPoints() for the reference camera!");
  }

  // Vector of missing initPoints for cameras
  std::vector<std::string> vectorOfMissingCameraPoints;

  // Set pose for the specified cameras
  for (it_tracker = m_mapOfTrackers.begin(); it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
    it_img = mapOfImages.find(it_tracker->first);
    it_initPoints = mapOfInitPoints.find(it_tracker->first);

    if (it_img != mapOfImages.end() && it_initPoints != mapOfInitPoints.end()) {
      // Set pose
      it_tracker->second->initFromPoints(*it_img->second, it_initPoints->second);
    } else {
      vectorOfMissingCameraPoints.push_back(it_tracker->first);
    }
  }

  for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoints.begin();
       it != vectorOfMissingCameraPoints.end(); ++it) {
    it_img = mapOfImages.find(*it);
    std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
        m_mapOfCameraTransformationMatrix.find(*it);

    if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
      vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo;
      m_mapOfTrackers[*it]->initFromPose(*it_img->second, cCurrentMo);
    } else {
      throw vpException(vpTrackingException::initializationError,
                        "Missing image or missing camera transformation "
                        "matrix! Cannot init the pose for camera: %s!",
                        it->c_str());
    }
  }
}

void vpMbGenericTracker::initFromPose(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
                                      const std::string &initFile1, const std::string &initFile2)
{
  if (m_mapOfTrackers.size() == 2) {
    std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
    TrackerWrapper *tracker = it->second;
    tracker->initFromPose(I1, initFile1);

    ++it;

    tracker = it->second;
    tracker->initFromPose(I2, initFile2);

    it = m_mapOfTrackers.find(m_referenceCameraName);
    if (it != m_mapOfTrackers.end()) {
      tracker = it->second;

      // Set the reference cMo
      tracker->getPose(cMo);

      // Set the reference camera parameters
      tracker->getCameraParameters(cam);
    }
  } else {
    throw vpException(vpTrackingException::initializationError,
                      "Cannot initFromPose()! Require two cameras but there "
                      "are %d cameras!",
                      m_mapOfTrackers.size());
  }
}

void vpMbGenericTracker::initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                                      const std::map<std::string, std::string> &mapOfInitPoses)
{
  // Set the reference cMo
  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
      mapOfImages.find(m_referenceCameraName);
  std::map<std::string, std::string>::const_iterator it_initPose = mapOfInitPoses.find(m_referenceCameraName);

  if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_initPose != mapOfInitPoses.end()) {
    TrackerWrapper *tracker = it_tracker->second;
    tracker->initFromPose(*it_img->second, it_initPose->second);
    tracker->getPose(cMo);
  } else {
    throw vpException(vpTrackingException::initializationError, "Cannot initFromPose() for the reference camera!");
  }

  // Vector of missing pose matrices for cameras
  std::vector<std::string> vectorOfMissingCameraPoses;

  // Set pose for the specified cameras
  for (it_tracker = m_mapOfTrackers.begin(); it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
    it_img = mapOfImages.find(it_tracker->first);
    it_initPose = mapOfInitPoses.find(it_tracker->first);

    if (it_img != mapOfImages.end() && it_initPose != mapOfInitPoses.end()) {
      // Set pose
      it_tracker->second->initFromPose(*it_img->second, it_initPose->second);
    } else {
      vectorOfMissingCameraPoses.push_back(it_tracker->first);
    }
  }

  for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
       it != vectorOfMissingCameraPoses.end(); ++it) {
    it_img = mapOfImages.find(*it);
    std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
        m_mapOfCameraTransformationMatrix.find(*it);

    if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
      vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo;
      m_mapOfTrackers[*it]->initFromPose(*it_img->second, cCurrentMo);
    } else {
      throw vpException(vpTrackingException::initializationError,
                        "Missing image or missing camera transformation "
                        "matrix! Cannot init the pose for camera: %s!",
                        it->c_str());
    }
  }
}

void vpMbGenericTracker::initFromPose(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
                                      const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo)
{
  if (m_mapOfTrackers.size() == 2) {
    std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
    it->second->initFromPose(I1, c1Mo);

    ++it;

    it->second->initFromPose(I2, c2Mo);

    this->cMo = c1Mo;
  } else {
    throw vpException(vpTrackingException::initializationError,
                      "This method requires 2 cameras but there are %d cameras!", m_mapOfTrackers.size());
  }
}

void vpMbGenericTracker::initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                                      const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
{
  // Set the reference cMo
  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
      mapOfImages.find(m_referenceCameraName);
  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(m_referenceCameraName);

  if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
    TrackerWrapper *tracker = it_tracker->second;
    tracker->initFromPose(*it_img->second, it_camPose->second);
    tracker->getPose(cMo);
  } else {
    throw vpException(vpTrackingException::initializationError, "Cannot set pose for the reference camera!");
  }

  // Vector of missing pose matrices for cameras
  std::vector<std::string> vectorOfMissingCameraPoses;

  // Set pose for the specified cameras
  for (it_tracker = m_mapOfTrackers.begin(); it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
    it_img = mapOfImages.find(it_tracker->first);
    it_camPose = mapOfCameraPoses.find(it_tracker->first);

    if (it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
      // Set pose
      it_tracker->second->initFromPose(*it_img->second, it_camPose->second);
    } else {
      vectorOfMissingCameraPoses.push_back(it_tracker->first);
    }
  }

  for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
       it != vectorOfMissingCameraPoses.end(); ++it) {
    it_img = mapOfImages.find(*it);
    std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
        m_mapOfCameraTransformationMatrix.find(*it);

    if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
      vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo;
      m_mapOfTrackers[*it]->initFromPose(*it_img->second, cCurrentMo);
    } else {
      throw vpException(vpTrackingException::initializationError,
                        "Missing image or missing camera transformation "
                        "matrix! Cannot set the pose for camera: %s!",
                        it->c_str());
    }
  }
}

void vpMbGenericTracker::loadConfigFile(const std::string &configFile)
{
  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->loadConfigFile(configFile);
  }

  if (m_mapOfTrackers.find(m_referenceCameraName) == m_mapOfTrackers.end()) {
    throw vpException(vpException::fatalError, "Cannot find the reference camera:  %s!", m_referenceCameraName.c_str());
  }

  m_mapOfTrackers[m_referenceCameraName]->getCameraParameters(this->cam);
  this->angleAppears = m_mapOfTrackers[m_referenceCameraName]->getAngleAppear();
  this->angleDisappears = m_mapOfTrackers[m_referenceCameraName]->getAngleDisappear();
  this->clippingFlag = m_mapOfTrackers[m_referenceCameraName]->getClipping();
}

void vpMbGenericTracker::loadConfigFile(const std::string &configFile1, const std::string &configFile2)
{
  if (m_mapOfTrackers.size() != 2) {
    throw vpException(vpException::fatalError, "The tracker is not set in a stereo configuration!");
  }

  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.begin();
  TrackerWrapper *tracker = it_tracker->second;
  tracker->loadConfigFile(configFile1);

  ++it_tracker;
  tracker = it_tracker->second;
  tracker->loadConfigFile(configFile2);

  if (m_mapOfTrackers.find(m_referenceCameraName) == m_mapOfTrackers.end()) {
    throw vpException(vpException::fatalError, "Cannot find the reference camera:  %s!", m_referenceCameraName.c_str());
  }

  m_mapOfTrackers[m_referenceCameraName]->getCameraParameters(this->cam);
  this->angleAppears = m_mapOfTrackers[m_referenceCameraName]->getAngleAppear();
  this->angleDisappears = m_mapOfTrackers[m_referenceCameraName]->getAngleDisappear();
  this->clippingFlag = m_mapOfTrackers[m_referenceCameraName]->getClipping();
}

void vpMbGenericTracker::loadConfigFile(const std::map<std::string, std::string> &mapOfConfigFiles)
{
  for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.begin();
       it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
    TrackerWrapper *tracker = it_tracker->second;

    std::map<std::string, std::string>::const_iterator it_config = mapOfConfigFiles.find(it_tracker->first);
    if (it_config != mapOfConfigFiles.end()) {
      tracker->loadConfigFile(it_config->second);
    } else {
      throw vpException(vpTrackingException::initializationError, "Missing configuration file for camera: %s!",
                        it_tracker->first.c_str());
    }
  }

  // Set the reference camera parameters
  std::map<std::string, TrackerWrapper *>::iterator it = m_mapOfTrackers.find(m_referenceCameraName);
  if (it != m_mapOfTrackers.end()) {
    TrackerWrapper *tracker = it->second;
    tracker->getCameraParameters(cam);

    // Set clipping
    this->clippingFlag = tracker->getClipping();
    this->angleAppears = tracker->getAngleAppear();
    this->angleDisappears = tracker->getAngleDisappear();
  } else {
    throw vpException(vpTrackingException::initializationError, "The reference camera: %s does not exist!",
                      m_referenceCameraName.c_str());
  }
}

void vpMbGenericTracker::loadModel(const std::string &modelFile, const bool verbose, const vpHomogeneousMatrix &T)
{
  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->loadModel(modelFile, verbose, T);
  }
}

void vpMbGenericTracker::loadModel(const std::string &modelFile1, const std::string &modelFile2, const bool verbose,
                                   const vpHomogeneousMatrix &T1, const vpHomogeneousMatrix &T2)
{
  if (m_mapOfTrackers.size() != 2) {
    throw vpException(vpException::fatalError, "The tracker is not set in a stereo configuration!");
  }

  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.begin();
  TrackerWrapper *tracker = it_tracker->second;
  tracker->loadModel(modelFile1, verbose, T1);

  ++it_tracker;
  tracker = it_tracker->second;
  tracker->loadModel(modelFile2, verbose, T2);
}

void vpMbGenericTracker::loadModel(const std::map<std::string, std::string> &mapOfModelFiles, const bool verbose,
                                   const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
{
  for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.begin();
       it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
    std::map<std::string, std::string>::const_iterator it_model = mapOfModelFiles.find(it_tracker->first);

    if (it_model != mapOfModelFiles.end()) {
      TrackerWrapper *tracker = it_tracker->second;
      std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);

      if (it_T != mapOfT.end())
        tracker->loadModel(it_model->second, verbose, it_T->second);
      else
        tracker->loadModel(it_model->second, verbose);
    } else {
      throw vpException(vpTrackingException::initializationError, "Cannot load model for camera: %s",
                        it_tracker->first.c_str());
    }
  }
}

#ifdef VISP_HAVE_PCL
void vpMbGenericTracker::preTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                                     std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> &mapOfPointClouds)
{
  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->preTracking(mapOfImages[it->first], mapOfPointClouds[it->first]);
  }
}
#endif

void vpMbGenericTracker::preTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                                     std::map<std::string, const std::vector<vpColVector> *> &mapOfPointClouds,
                                     std::map<std::string, unsigned int> &mapOfPointCloudWidths,
                                     std::map<std::string, unsigned int> &mapOfPointCloudHeights)
{
  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->preTracking(mapOfImages[it->first], mapOfPointClouds[it->first], mapOfPointCloudWidths[it->first],
                         mapOfPointCloudHeights[it->first]);
  }
}

void vpMbGenericTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
                                     const vpHomogeneousMatrix &cMo_, const bool verbose,
                                     const vpHomogeneousMatrix &T)
{
  if (m_mapOfTrackers.size() != 1) {
    throw vpException(vpTrackingException::fatalError, "This method requires exactly one camera, there are %d cameras!",
                      m_mapOfTrackers.size());
  }

  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
  if (it_tracker != m_mapOfTrackers.end()) {
    TrackerWrapper *tracker = it_tracker->second;
    tracker->reInitModel(I, cad_name, cMo_, verbose, T);

    // Set reference pose
    tracker->getPose(cMo);
  } else {
    throw vpException(vpTrackingException::fatalError, "Cannot reInitModel() the reference camera!");
  }

  modelInitialised = true;
}

void vpMbGenericTracker::reInitModel(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
                                     const std::string &cad_name1, const std::string &cad_name2,
                                     const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
                                     const bool verbose,
                                     const vpHomogeneousMatrix &T1, const vpHomogeneousMatrix &T2)
{
  if (m_mapOfTrackers.size() == 2) {
    std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.begin();

    it_tracker->second->reInitModel(I1, cad_name1, c1Mo, verbose, T1);

    ++it_tracker;

    it_tracker->second->reInitModel(I2, cad_name2, c2Mo, verbose, T2);

    it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
    if (it_tracker != m_mapOfTrackers.end()) {
      // Set reference pose
      it_tracker->second->getPose(cMo);
    }
  } else {
    throw vpException(vpTrackingException::fatalError, "This method requires exactly two cameras!");
  }

  modelInitialised = true;
}

void vpMbGenericTracker::reInitModel(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                                     const std::map<std::string, std::string> &mapOfModelFiles,
                                     const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
                                     const bool verbose,
                                     const std::map<std::string, vpHomogeneousMatrix> &mapOfT)
{
  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
      mapOfImages.find(m_referenceCameraName);
  std::map<std::string, std::string>::const_iterator it_model = mapOfModelFiles.find(m_referenceCameraName);
  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(m_referenceCameraName);

  if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_model != mapOfModelFiles.end() &&
      it_camPose != mapOfCameraPoses.end()) {
    TrackerWrapper *tracker = it_tracker->second;
    std::map<std::string, vpHomogeneousMatrix>::const_iterator it_T = mapOfT.find(it_tracker->first);
    if (it_T != mapOfT.end())
      tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose, it_T->second);
    else
      tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose);

    // Set reference pose
    tracker->getPose(cMo);
  } else {
    throw vpException(vpTrackingException::fatalError, "Cannot reInitModel() for reference camera!");
  }

  std::vector<std::string> vectorOfMissingCameras;
  for (it_tracker = m_mapOfTrackers.begin(); it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
    if (it_tracker->first != m_referenceCameraName) {
      it_img = mapOfImages.find(it_tracker->first);
      it_model = mapOfModelFiles.find(it_tracker->first);
      it_camPose = mapOfCameraPoses.find(it_tracker->first);

      if (it_img != mapOfImages.end() && it_model != mapOfModelFiles.end() && it_camPose != mapOfCameraPoses.end()) {
        TrackerWrapper *tracker = it_tracker->second;
        tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose);
      } else {
        vectorOfMissingCameras.push_back(it_tracker->first);
      }
    }
  }

  for (std::vector<std::string>::const_iterator it = vectorOfMissingCameras.begin(); it != vectorOfMissingCameras.end();
       ++it) {
    it_img = mapOfImages.find(*it);
    it_model = mapOfModelFiles.find(*it);
    std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
        m_mapOfCameraTransformationMatrix.find(*it);

    if (it_img != mapOfImages.end() && it_model != mapOfModelFiles.end() &&
        it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
      vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo;
      m_mapOfTrackers[*it]->reInitModel(*it_img->second, it_model->second, cCurrentMo, verbose);
    }
  }

  modelInitialised = true;
}

void vpMbGenericTracker::resetTracker()
{
  cMo.eye();

  useScanLine = false;

#ifdef VISP_HAVE_OGRE
  useOgre = false;
#endif

  m_computeInteraction = true;
  m_lambda = 1.0;

  angleAppears = vpMath::rad(89);
  angleDisappears = vpMath::rad(89);
  clippingFlag = vpPolygon3D::NO_CLIPPING;
  distNearClip = 0.001;
  distFarClip = 100;

  m_optimizationMethod = vpMbTracker::GAUSS_NEWTON_OPT;
  m_maxIter = 30;
  m_stopCriteriaEpsilon = 1e-8;
  m_initialMu = 0.01;

  // Only for Edge
  m_percentageGdPt = 0.4;

  // Only for KLT
  m_thresholdOutlier = 0.5;

  // Reset default ponderation between each feature type
  m_mapOfFeatureFactors[EDGE_TRACKER] = 1.0;

#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
  m_mapOfFeatureFactors[KLT_TRACKER] = 1.0;
#endif

  m_mapOfFeatureFactors[DEPTH_NORMAL_TRACKER] = 1.0;
  m_mapOfFeatureFactors[DEPTH_DENSE_TRACKER] = 1.0;

  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->resetTracker();
  }
}

void vpMbGenericTracker::setAngleAppear(const double &a)
{
  vpMbTracker::setAngleAppear(a);

  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setAngleAppear(a);
  }
}

void vpMbGenericTracker::setAngleAppear(const double &a1, const double &a2)
{
  if (m_mapOfTrackers.size() == 2) {
    std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
    it->second->setAngleAppear(a1);

    ++it;
    it->second->setAngleAppear(a2);

    it = m_mapOfTrackers.find(m_referenceCameraName);
    if (it != m_mapOfTrackers.end()) {
      angleAppears = it->second->getAngleAppear();
    } else {
      std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
    }
  } else {
    throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!",
                      m_mapOfTrackers.size());
  }
}

void vpMbGenericTracker::setAngleAppear(const std::map<std::string, double> &mapOfAngles)
{
  for (std::map<std::string, double>::const_iterator it = mapOfAngles.begin(); it != mapOfAngles.end(); ++it) {
    std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);

    if (it_tracker != m_mapOfTrackers.end()) {
      TrackerWrapper *tracker = it_tracker->second;
      tracker->setAngleAppear(it->second);

      if (it->first == m_referenceCameraName) {
        angleAppears = it->second;
      }
    }
  }
}

void vpMbGenericTracker::setAngleDisappear(const double &a)
{
  vpMbTracker::setAngleDisappear(a);

  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setAngleDisappear(a);
  }
}

void vpMbGenericTracker::setAngleDisappear(const double &a1, const double &a2)
{
  if (m_mapOfTrackers.size() == 2) {
    std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
    it->second->setAngleDisappear(a1);

    ++it;
    it->second->setAngleDisappear(a2);

    it = m_mapOfTrackers.find(m_referenceCameraName);
    if (it != m_mapOfTrackers.end()) {
      angleDisappears = it->second->getAngleDisappear();
    } else {
      std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
    }
  } else {
    throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!",
                      m_mapOfTrackers.size());
  }
}

void vpMbGenericTracker::setAngleDisappear(const std::map<std::string, double> &mapOfAngles)
{
  for (std::map<std::string, double>::const_iterator it = mapOfAngles.begin(); it != mapOfAngles.end(); ++it) {
    std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);

    if (it_tracker != m_mapOfTrackers.end()) {
      TrackerWrapper *tracker = it_tracker->second;
      tracker->setAngleDisappear(it->second);

      if (it->first == m_referenceCameraName) {
        angleDisappears = it->second;
      }
    }
  }
}

void vpMbGenericTracker::setCameraParameters(const vpCameraParameters &camera)
{
  vpMbTracker::setCameraParameters(camera);

  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setCameraParameters(camera);
  }
}

void vpMbGenericTracker::setCameraParameters(const vpCameraParameters &camera1, const vpCameraParameters &camera2)
{
  if (m_mapOfTrackers.size() == 2) {
    std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
    it->second->setCameraParameters(camera1);

    ++it;
    it->second->setCameraParameters(camera2);

    it = m_mapOfTrackers.find(m_referenceCameraName);
    if (it != m_mapOfTrackers.end()) {
      it->second->getCameraParameters(cam);
    } else {
      std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
    }
  } else {
    throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!",
                      m_mapOfTrackers.size());
  }
}

void vpMbGenericTracker::setCameraParameters(const std::map<std::string, vpCameraParameters> &mapOfCameraParameters)
{
  for (std::map<std::string, vpCameraParameters>::const_iterator it = mapOfCameraParameters.begin();
       it != mapOfCameraParameters.end(); ++it) {
    std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);

    if (it_tracker != m_mapOfTrackers.end()) {
      TrackerWrapper *tracker = it_tracker->second;
      tracker->setCameraParameters(it->second);

      if (it->first == m_referenceCameraName) {
        cam = it->second;
      }
    }
  }
}

void vpMbGenericTracker::setCameraTransformationMatrix(const std::string &cameraName,
                                                       const vpHomogeneousMatrix &cameraTransformationMatrix)
{
  std::map<std::string, vpHomogeneousMatrix>::iterator it = m_mapOfCameraTransformationMatrix.find(cameraName);

  if (it != m_mapOfCameraTransformationMatrix.end()) {
    it->second = cameraTransformationMatrix;
  } else {
    throw vpException(vpTrackingException::fatalError, "Cannot find camera: %s!", cameraName.c_str());
  }
}

void vpMbGenericTracker::setCameraTransformationMatrix(
    const std::map<std::string, vpHomogeneousMatrix> &mapOfTransformationMatrix)
{
  for (std::map<std::string, vpHomogeneousMatrix>::const_iterator it = mapOfTransformationMatrix.begin();
       it != mapOfTransformationMatrix.end(); ++it) {
    std::map<std::string, vpHomogeneousMatrix>::iterator it_camTrans =
        m_mapOfCameraTransformationMatrix.find(it->first);

    if (it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
      it_camTrans->second = it->second;
    }
  }
}

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

  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setClipping(flags);
  }
}

void vpMbGenericTracker::setClipping(const unsigned int &flags1, const unsigned int &flags2)
{
  if (m_mapOfTrackers.size() == 2) {
    std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
    it->second->setClipping(flags1);

    ++it;
    it->second->setClipping(flags2);

    it = m_mapOfTrackers.find(m_referenceCameraName);
    if (it != m_mapOfTrackers.end()) {
      clippingFlag = it->second->getClipping();
    } else {
      std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
    }
  } else {
    std::stringstream ss;
    ss << "Require two cameras! There are " << m_mapOfTrackers.size() << " cameras!";
    throw vpException(vpTrackingException::fatalError, ss.str());
  }
}

void vpMbGenericTracker::setClipping(const std::map<std::string, unsigned int> &mapOfClippingFlags)
{
  for (std::map<std::string, unsigned int>::const_iterator it = mapOfClippingFlags.begin();
       it != mapOfClippingFlags.end(); ++it) {
    std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);

    if (it_tracker != m_mapOfTrackers.end()) {
      TrackerWrapper *tracker = it_tracker->second;
      tracker->setClipping(it->second);

      if (it->first == m_referenceCameraName) {
        clippingFlag = it->second;
      }
    }
  }
}

void vpMbGenericTracker::setDepthDenseFilteringMaxDistance(const double maxDistance)
{
  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setDepthDenseFilteringMaxDistance(maxDistance);
  }
}

void vpMbGenericTracker::setDepthDenseFilteringMethod(const int method)
{
  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setDepthDenseFilteringMethod(method);
  }
}

void vpMbGenericTracker::setDepthDenseFilteringMinDistance(const double minDistance)
{
  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setDepthDenseFilteringMinDistance(minDistance);
  }
}

void vpMbGenericTracker::setDepthDenseFilteringOccupancyRatio(const double occupancyRatio)
{
  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setDepthDenseFilteringOccupancyRatio(occupancyRatio);
  }
}

void vpMbGenericTracker::setDepthDenseSamplingStep(const unsigned int stepX, const unsigned int stepY)
{
  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setDepthDenseSamplingStep(stepX, stepY);
  }
}

void vpMbGenericTracker::setDepthNormalFaceCentroidMethod(const vpMbtFaceDepthNormal::vpFaceCentroidType &method)
{
  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setDepthNormalFaceCentroidMethod(method);
  }
}

void vpMbGenericTracker::setDepthNormalFeatureEstimationMethod(
    const vpMbtFaceDepthNormal::vpFeatureEstimationType &method)
{
  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setDepthNormalFeatureEstimationMethod(method);
  }
}

void vpMbGenericTracker::setDepthNormalPclPlaneEstimationMethod(const int method)
{
  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setDepthNormalPclPlaneEstimationMethod(method);
  }
}

void vpMbGenericTracker::setDepthNormalPclPlaneEstimationRansacMaxIter(const int maxIter)
{
  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setDepthNormalPclPlaneEstimationRansacMaxIter(maxIter);
  }
}

void vpMbGenericTracker::setDepthNormalPclPlaneEstimationRansacThreshold(const double thresold)
{
  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setDepthNormalPclPlaneEstimationRansacThreshold(thresold);
  }
}

void vpMbGenericTracker::setDepthNormalSamplingStep(const unsigned int stepX, const unsigned int stepY)
{
  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setDepthNormalSamplingStep(stepX, stepY);
  }
}

void vpMbGenericTracker::setDisplayFeatures(const bool displayF)
{
  vpMbTracker::setDisplayFeatures(displayF);

  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setDisplayFeatures(displayF);
  }
}

void vpMbGenericTracker::setFarClippingDistance(const double &dist)
{
  vpMbTracker::setFarClippingDistance(dist);

  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setFarClippingDistance(dist);
  }
}

void vpMbGenericTracker::setFarClippingDistance(const double &dist1, const double &dist2)
{
  if (m_mapOfTrackers.size() == 2) {
    std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
    it->second->setFarClippingDistance(dist1);

    ++it;
    it->second->setFarClippingDistance(dist2);

    it = m_mapOfTrackers.find(m_referenceCameraName);
    if (it != m_mapOfTrackers.end()) {
      distFarClip = it->second->getFarClippingDistance();
    } else {
      std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
    }
  } else {
    throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!",
                      m_mapOfTrackers.size());
  }
}

void vpMbGenericTracker::setFarClippingDistance(const std::map<std::string, double> &mapOfClippingDists)
{
  for (std::map<std::string, double>::const_iterator it = mapOfClippingDists.begin(); it != mapOfClippingDists.end();
       ++it) {
    std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);

    if (it_tracker != m_mapOfTrackers.end()) {
      TrackerWrapper *tracker = it_tracker->second;
      tracker->setFarClippingDistance(it->second);

      if (it->first == m_referenceCameraName) {
        distFarClip = it->second;
      }
    }
  }
}

void vpMbGenericTracker::setFeatureFactors(const std::map<vpTrackerType, double> &mapOfFeatureFactors)
{
  for (std::map<vpTrackerType, double>::iterator it = m_mapOfFeatureFactors.begin(); it != m_mapOfFeatureFactors.end();
       ++it) {
    std::map<vpTrackerType, double>::const_iterator it_factor = mapOfFeatureFactors.find(it->first);
    if (it_factor != mapOfFeatureFactors.end()) {
      it->second = it_factor->second;
    }
  }
}

void vpMbGenericTracker::setGoodMovingEdgesRatioThreshold(const double threshold)
{
  m_percentageGdPt = threshold;

  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setGoodMovingEdgesRatioThreshold(threshold);
  }
}

#ifdef VISP_HAVE_OGRE

void vpMbGenericTracker::setGoodNbRayCastingAttemptsRatio(const double &ratio)
{
  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setGoodNbRayCastingAttemptsRatio(ratio);
  }
}

void vpMbGenericTracker::setNbRayCastingAttemptsForVisibility(const unsigned int &attempts)
{
  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setNbRayCastingAttemptsForVisibility(attempts);
  }
}
#endif

#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))

void vpMbGenericTracker::setKltOpencv(const vpKltOpencv &t)
{
  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setKltOpencv(t);
  }
}

void vpMbGenericTracker::setKltOpencv(const vpKltOpencv &t1, const vpKltOpencv &t2)
{
  if (m_mapOfTrackers.size() == 2) {
    std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
    it->second->setKltOpencv(t1);

    ++it;
    it->second->setKltOpencv(t2);
  } else {
    throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!",
                      m_mapOfTrackers.size());
  }
}

void vpMbGenericTracker::setKltOpencv(const std::map<std::string, vpKltOpencv> &mapOfKlts)
{
  for (std::map<std::string, vpKltOpencv>::const_iterator it = mapOfKlts.begin(); it != mapOfKlts.end(); ++it) {
    std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);

    if (it_tracker != m_mapOfTrackers.end()) {
      TrackerWrapper *tracker = it_tracker->second;
      tracker->setKltOpencv(it->second);
    }
  }
}

void vpMbGenericTracker::setKltThresholdAcceptation(const double th)
{
  m_thresholdOutlier = th;

  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setKltThresholdAcceptation(th);
  }
}
#endif

void vpMbGenericTracker::setLod(const bool useLod, const std::string &name)
{
  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setLod(useLod, name);
  }
}

#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))

void vpMbGenericTracker::setKltMaskBorder(const unsigned int &e)
{
  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setKltMaskBorder(e);
  }
}

void vpMbGenericTracker::setKltMaskBorder(const unsigned int &e1, const unsigned int &e2)
{
  if (m_mapOfTrackers.size() == 2) {
    std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
    it->second->setKltMaskBorder(e1);

    ++it;

    it->second->setKltMaskBorder(e2);
  } else {
    throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!",
                      m_mapOfTrackers.size());
  }
}

void vpMbGenericTracker::setKltMaskBorder(const std::map<std::string, unsigned int> &mapOfErosions)
{
  for (std::map<std::string, unsigned int>::const_iterator it = mapOfErosions.begin(); it != mapOfErosions.end();
       ++it) {
    std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);

    if (it_tracker != m_mapOfTrackers.end()) {
      TrackerWrapper *tracker = it_tracker->second;
      tracker->setKltMaskBorder(it->second);
    }
  }
}
#endif

void vpMbGenericTracker::setMask(const vpImage<bool> &mask)
{
  vpMbTracker::setMask(mask);

  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
    it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setMask(mask);
  }
}


void vpMbGenericTracker::setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name)
{
  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setMinLineLengthThresh(minLineLengthThresh, name);
  }
}

void vpMbGenericTracker::setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name)
{
  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setMinPolygonAreaThresh(minPolygonAreaThresh, name);
  }
}

void vpMbGenericTracker::setMovingEdge(const vpMe &me)
{
  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setMovingEdge(me);
  }
}

void vpMbGenericTracker::setMovingEdge(const vpMe &me1, const vpMe &me2)
{
  if (m_mapOfTrackers.size() == 2) {
    std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
    it->second->setMovingEdge(me1);

    ++it;

    it->second->setMovingEdge(me2);
  } else {
    throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!",
                      m_mapOfTrackers.size());
  }
}

void vpMbGenericTracker::setMovingEdge(const std::map<std::string, vpMe> &mapOfMe)
{
  for (std::map<std::string, vpMe>::const_iterator it = mapOfMe.begin(); it != mapOfMe.end(); ++it) {
    std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);

    if (it_tracker != m_mapOfTrackers.end()) {
      TrackerWrapper *tracker = it_tracker->second;
      tracker->setMovingEdge(it->second);
    }
  }
}

void vpMbGenericTracker::setNearClippingDistance(const double &dist)
{
  vpMbTracker::setNearClippingDistance(dist);

  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setNearClippingDistance(dist);
  }
}

void vpMbGenericTracker::setNearClippingDistance(const double &dist1, const double &dist2)
{
  if (m_mapOfTrackers.size() == 2) {
    std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
    it->second->setNearClippingDistance(dist1);

    ++it;

    it->second->setNearClippingDistance(dist2);

    it = m_mapOfTrackers.find(m_referenceCameraName);
    if (it != m_mapOfTrackers.end()) {
      distNearClip = it->second->getNearClippingDistance();
    } else {
      std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
    }
  } else {
    throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!",
                      m_mapOfTrackers.size());
  }
}

void vpMbGenericTracker::setNearClippingDistance(const std::map<std::string, double> &mapOfDists)
{
  for (std::map<std::string, double>::const_iterator it = mapOfDists.begin(); it != mapOfDists.end(); ++it) {
    std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);

    if (it_tracker != m_mapOfTrackers.end()) {
      TrackerWrapper *tracker = it_tracker->second;
      tracker->setNearClippingDistance(it->second);

      if (it->first == m_referenceCameraName) {
        distNearClip = it->second;
      }
    }
  }
}

void vpMbGenericTracker::setOgreShowConfigDialog(const bool showConfigDialog)
{
  vpMbTracker::setOgreShowConfigDialog(showConfigDialog);

  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setOgreShowConfigDialog(showConfigDialog);
  }
}

void vpMbGenericTracker::setOgreVisibilityTest(const bool &v)
{
  vpMbTracker::setOgreVisibilityTest(v);

  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setOgreVisibilityTest(v);
  }

#ifdef VISP_HAVE_OGRE
  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->faces.getOgreContext()->setWindowName("Multi Generic MBT (" + it->first + ")");
  }
#endif
}

void vpMbGenericTracker::setOptimizationMethod(const vpMbtOptimizationMethod &opt)
{
  vpMbTracker::setOptimizationMethod(opt);

  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setOptimizationMethod(opt);
  }
}

void vpMbGenericTracker::setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cdMo)
{
  if (m_mapOfTrackers.size() > 1) {
    throw vpException(vpTrackingException::initializationError, "The function setPose() requires the generic tracker "
                                                                "to be configured with only one camera!");
  }

  cMo = cdMo;

  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
  if (it != m_mapOfTrackers.end()) {
    TrackerWrapper *tracker = it->second;
    tracker->setPose(I, cdMo);
  } else {
    throw vpException(vpTrackingException::initializationError, "The reference camera: %s does not exist!",
                      m_referenceCameraName.c_str());
  }
}

void vpMbGenericTracker::setPose(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
                                 const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo)
{
  if (m_mapOfTrackers.size() == 2) {
    std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
    it->second->setPose(I1, c1Mo);

    ++it;

    it->second->setPose(I2, c2Mo);

    it = m_mapOfTrackers.find(m_referenceCameraName);
    if (it != m_mapOfTrackers.end()) {
      // Set reference pose
      it->second->getPose(cMo);
    } else {
      throw vpException(vpTrackingException::fatalError, "The reference camera: %s does not exist!",
                        m_referenceCameraName.c_str());
    }
  } else {
    throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!",
                      m_mapOfTrackers.size());
  }
}

void vpMbGenericTracker::setPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                                 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
{
  // Set the reference cMo
  std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
  std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
      mapOfImages.find(m_referenceCameraName);
  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(m_referenceCameraName);

  if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
    TrackerWrapper *tracker = it_tracker->second;
    tracker->setPose(*it_img->second, it_camPose->second);
    tracker->getPose(cMo);
  } else {
    throw vpException(vpTrackingException::fatalError, "Cannot set pose for the reference camera!");
  }

  // Vector of missing pose matrices for cameras
  std::vector<std::string> vectorOfMissingCameraPoses;

  // Set pose for the specified cameras
  for (it_tracker = m_mapOfTrackers.begin(); it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
    if (it_tracker->first != m_referenceCameraName) {
      it_img = mapOfImages.find(it_tracker->first);
      it_camPose = mapOfCameraPoses.find(it_tracker->first);

      if (it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
        // Set pose
        TrackerWrapper *tracker = it_tracker->second;
        tracker->setPose(*it_img->second, it_camPose->second);
      } else {
        vectorOfMissingCameraPoses.push_back(it_tracker->first);
      }
    }
  }

  for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
       it != vectorOfMissingCameraPoses.end(); ++it) {
    it_img = mapOfImages.find(*it);
    std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
        m_mapOfCameraTransformationMatrix.find(*it);

    if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
      vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo;
      m_mapOfTrackers[*it]->setPose(*it_img->second, cCurrentMo);
    } else {
      throw vpException(vpTrackingException::fatalError,
                        "Missing image or missing camera transformation "
                        "matrix! Cannot set pose for camera: %s!",
                        it->c_str());
    }
  }
}

void vpMbGenericTracker::setProjectionErrorComputation(const bool &flag)
{
  vpMbTracker::setProjectionErrorComputation(flag);

  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setProjectionErrorComputation(flag);
  }
}

void vpMbGenericTracker::setProjectionErrorDisplay(const bool display)
{
  vpMbTracker::setProjectionErrorDisplay(display);

  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setProjectionErrorDisplay(display);
  }
}

void vpMbGenericTracker::setProjectionErrorDisplayArrowLength(const unsigned int length)
{
  vpMbTracker::setProjectionErrorDisplayArrowLength(length);

  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setProjectionErrorDisplayArrowLength(length);
  }
}

void vpMbGenericTracker::setProjectionErrorDisplayArrowThickness(const unsigned int thickness)
{
  vpMbTracker::setProjectionErrorDisplayArrowThickness(thickness);

  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setProjectionErrorDisplayArrowThickness(thickness);
  }
}

void vpMbGenericTracker::setReferenceCameraName(const std::string &referenceCameraName)
{
  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(referenceCameraName);
  if (it != m_mapOfTrackers.end()) {
    m_referenceCameraName = referenceCameraName;
  } else {
    std::cerr << "The reference camera: " << referenceCameraName << " does not exist!";
  }
}

void vpMbGenericTracker::setScanLineVisibilityTest(const bool &v)
{
  vpMbTracker::setScanLineVisibilityTest(v);

  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setScanLineVisibilityTest(v);
  }
}

void vpMbGenericTracker::setTrackerType(const int type)
{
  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setTrackerType(type);
  }
}

void vpMbGenericTracker::setTrackerType(const std::map<std::string, int> &mapOfTrackerTypes)
{
  for (std::map<std::string, int>::const_iterator it = mapOfTrackerTypes.begin(); it != mapOfTrackerTypes.end(); ++it) {
    std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);
    if (it_tracker != m_mapOfTrackers.end()) {
      TrackerWrapper *tracker = it_tracker->second;
      tracker->setTrackerType(it->second);
    }
  }
}

void vpMbGenericTracker::setUseDepthDenseTracking(const std::string &name, const bool &useDepthDenseTracking)
{
  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setUseDepthDenseTracking(name, useDepthDenseTracking);
  }
}

void vpMbGenericTracker::setUseDepthNormalTracking(const std::string &name, const bool &useDepthNormalTracking)
{
  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setUseDepthNormalTracking(name, useDepthNormalTracking);
  }
}

void vpMbGenericTracker::setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking)
{
  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setUseEdgeTracking(name, useEdgeTracking);
  }
}

#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))

void vpMbGenericTracker::setUseKltTracking(const std::string &name, const bool &useKltTracking)
{
  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->setUseKltTracking(name, useKltTracking);
  }
}
#endif

void vpMbGenericTracker::testTracking()
{
  // Test tracking fails only if all testTracking have failed
  bool isOneTestTrackingOk = false;
  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    try {
      tracker->testTracking();
      isOneTestTrackingOk = true;
    } catch (...) {
    }
  }

  if (!isOneTestTrackingOk) {
    std::ostringstream oss;
    oss << "Not enough moving edges to track the object. Try to reduce the "
           "threshold="
        << m_percentageGdPt << " using setGoodMovingEdgesRatioThreshold()";
    throw vpTrackingException(vpTrackingException::fatalError, oss.str());
  }
}

void vpMbGenericTracker::track(const vpImage<unsigned char> &I)
{
  std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
  mapOfImages[m_referenceCameraName] = &I;

  std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
  std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;

  track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
}

void vpMbGenericTracker::track(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2)
{
  if (m_mapOfTrackers.size() == 2) {
    std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
    std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
    mapOfImages[it->first] = &I1;
    ++it;

    mapOfImages[it->first] = &I2;

    std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
    std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;

    track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
  } else {
    std::stringstream ss;
    ss << "Require two cameras! There are " << m_mapOfTrackers.size() << " cameras!";
    throw vpException(vpTrackingException::fatalError, ss.str().c_str());
  }
}

void vpMbGenericTracker::track(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages)
{
  std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
  std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;

  track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
}

#ifdef VISP_HAVE_PCL

void vpMbGenericTracker::track(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                               std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> &mapOfPointClouds)
{
  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;

    if ((tracker->m_trackerType & (EDGE_TRACKER |
#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
                                   KLT_TRACKER |
#endif
                                   DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER)) == 0) {
      throw vpException(vpException::fatalError, "Bad tracker type: %d", tracker->m_trackerType);
    }

    if (tracker->m_trackerType & (EDGE_TRACKER
#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
                                  | KLT_TRACKER
#endif
                                  ) &&
        mapOfImages[it->first] == NULL) {
      throw vpException(vpException::fatalError, "Image pointer is NULL!");
    }

    if (tracker->m_trackerType & (DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER) &&
        mapOfPointClouds[it->first] == nullptr) {
      throw vpException(vpException::fatalError, "Pointcloud smart pointer is NULL!");
    }
  }

  preTracking(mapOfImages, mapOfPointClouds);

  try {
    computeVVS(mapOfImages);
  } catch (...) {
    covarianceMatrix = -1;
    throw; // throw the original exception
  }

  testTracking();

  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;

    tracker->postTracking(mapOfImages[it->first], mapOfPointClouds[it->first]);
  }

  computeProjectionError();
}
#endif

void vpMbGenericTracker::track(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                               std::map<std::string, const std::vector<vpColVector> *> &mapOfPointClouds,
                               std::map<std::string, unsigned int> &mapOfPointCloudWidths,
                               std::map<std::string, unsigned int> &mapOfPointCloudHeights)
{
  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;

    if ((tracker->m_trackerType & (EDGE_TRACKER |
#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
                                   KLT_TRACKER |
#endif
                                   DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER)) == 0) {
      throw vpException(vpException::fatalError, "Bad tracker type: %d", tracker->m_trackerType);
    }

    if (tracker->m_trackerType & (EDGE_TRACKER
#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
                                  | KLT_TRACKER
#endif
                                  ) &&
        mapOfImages[it->first] == NULL) {
      throw vpException(vpException::fatalError, "Image pointer is NULL!");
    }

    if (tracker->m_trackerType & (DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER) &&
        (mapOfPointClouds[it->first] == NULL)) {
      throw vpException(vpException::fatalError, "Pointcloud is NULL!");
    }
  }

  preTracking(mapOfImages, mapOfPointClouds, mapOfPointCloudWidths, mapOfPointCloudHeights);

  try {
    computeVVS(mapOfImages);
  } catch (...) {
    covarianceMatrix = -1;
    throw; // throw the original exception
  }

  testTracking();

  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
       it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;

    tracker->postTracking(mapOfImages[it->first], mapOfPointCloudWidths[it->first], mapOfPointCloudHeights[it->first]);
  }

  computeProjectionError();
}

vpMbGenericTracker::TrackerWrapper::TrackerWrapper()
  : m_error(), m_L(), m_trackerType(EDGE_TRACKER), m_w(), m_weightedError()
{
  m_lambda = 1.0;
  m_maxIter = 30;

#ifdef VISP_HAVE_OGRE
  faces.getOgreContext()->setWindowName("MBT TrackerWrapper");

  m_projectionErrorFaces.getOgreContext()->setWindowName("MBT TrackerWrapper (projection error)");
#endif
}

vpMbGenericTracker::TrackerWrapper::TrackerWrapper(const int trackerType)
  : m_error(), m_L(), m_trackerType(trackerType), m_w(), m_weightedError()
{
  if ((m_trackerType & (EDGE_TRACKER |
#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
                        KLT_TRACKER |
#endif
                        DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER)) == 0) {
    throw vpException(vpTrackingException::badValue, "Bad value for tracker type: %d!", m_trackerType);
  }

  m_lambda = 1.0;
  m_maxIter = 30;

#ifdef VISP_HAVE_OGRE
  faces.getOgreContext()->setWindowName("MBT TrackerWrapper");

  m_projectionErrorFaces.getOgreContext()->setWindowName("MBT TrackerWrapper (projection error)");
#endif
}

vpMbGenericTracker::TrackerWrapper::~TrackerWrapper() { }

// Implemented only for debugging purposes: use TrackerWrapper as a standalone tracker
void vpMbGenericTracker::TrackerWrapper::computeVVS(const vpImage<unsigned char> *const ptr_I)
{
  computeVVSInit(ptr_I);

  if (m_error.getRows() < 4) {
    throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
  }

  double normRes = 0;
  double normRes_1 = -1;
  unsigned int iter = 0;

  double factorEdge = 1.0;
#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
  double factorKlt = 1.0;
#endif
  double factorDepth = 1.0;
  double factorDepthDense = 1.0;

  vpMatrix LTL;
  vpColVector LTR, v;
  vpColVector error_prev;

  double mu = m_initialMu;
  vpHomogeneousMatrix cMo_prev;
#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
  vpHomogeneousMatrix ctTc0_Prev; // Only for KLT
#endif
  bool isoJoIdentity_ = true;

  // Covariance
  vpColVector W_true(m_error.getRows());
  vpMatrix L_true, LVJ_true;

  unsigned int nb_edge_features = m_error_edge.getRows();
#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
  unsigned int nb_klt_features = m_error_klt.getRows();
#endif
  unsigned int nb_depth_features = m_error_depthNormal.getRows();
  unsigned int nb_depth_dense_features = m_error_depthDense.getRows();

  while (std::fabs(normRes_1 - normRes) > m_stopCriteriaEpsilon && (iter < m_maxIter)) {
    computeVVSInteractionMatrixAndResidu(ptr_I);

    bool reStartFromLastIncrement = false;
    computeVVSCheckLevenbergMarquardt(iter, m_error, error_prev, cMo_prev, mu, reStartFromLastIncrement);

#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
    if (reStartFromLastIncrement) {
      if (m_trackerType & KLT_TRACKER) {
        ctTc0 = ctTc0_Prev;
      }
    }
#endif

    if (!reStartFromLastIncrement) {
      computeVVSWeights();

      if (computeCovariance) {
        L_true = m_L;
        if (!isoJoIdentity_) {
          vpVelocityTwistMatrix cVo;
          cVo.buildFrom(cMo);
          LVJ_true = (m_L * cVo * oJo);
        }
      }

      vpVelocityTwistMatrix cVo;
      if (iter == 0) {
        isoJoIdentity_ = true;
        oJo.eye();

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

          vpMatrix K; // kernel
          unsigned int rank = (m_L * cVo).kernel(K);
          if (rank == 0) {
            throw vpException(vpException::fatalError, "Rank=0, cannot estimate the pose !");
          }

          if (rank != 6) {
            vpMatrix I; // Identity
            I.eye(6);
            oJo = I - K.AtA();

            isoJoIdentity_ = false;
          }
        }
      }

      // Weighting
      double num = 0;
      double den = 0;

      unsigned int start_index = 0;
      if (m_trackerType & EDGE_TRACKER) {
        for (unsigned int i = 0; i < nb_edge_features; i++) {
          double wi = m_w_edge[i] * m_factor[i] * factorEdge;
          W_true[i] = wi;
          m_weightedError[i] = wi * m_error[i];

          num += wi * vpMath::sqr(m_error[i]);
          den += wi;

          for (unsigned int j = 0; j < m_L.getCols(); j++) {
            m_L[i][j] *= wi;
          }
        }

        start_index += nb_edge_features;
      }

#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
      if (m_trackerType & KLT_TRACKER) {
        for (unsigned int i = 0; i < nb_klt_features; i++) {
          double wi = m_w_klt[i] * factorKlt;
          W_true[start_index + i] = wi;
          m_weightedError[start_index + i] = wi * m_error_klt[i];

          num += wi * vpMath::sqr(m_error[start_index + i]);
          den += wi;

          for (unsigned int j = 0; j < m_L.getCols(); j++) {
            m_L[start_index + i][j] *= wi;
          }
        }

        start_index += nb_klt_features;
      }
#endif

      if (m_trackerType & DEPTH_NORMAL_TRACKER) {
        for (unsigned int i = 0; i < nb_depth_features; i++) {
          double wi = m_w_depthNormal[i] * factorDepth;
          m_w[start_index + i] = m_w_depthNormal[i];
          m_weightedError[start_index + i] = wi * m_error[start_index + i];

          num += wi * vpMath::sqr(m_error[start_index + i]);
          den += wi;

          for (unsigned int j = 0; j < m_L.getCols(); j++) {
            m_L[start_index + i][j] *= wi;
          }
        }

        start_index += nb_depth_features;
      }

      if (m_trackerType & DEPTH_DENSE_TRACKER) {
        for (unsigned int i = 0; i < nb_depth_dense_features; i++) {
          double wi = m_w_depthDense[i] * factorDepthDense;
          m_w[start_index + i] = m_w_depthDense[i];
          m_weightedError[start_index + i] = wi * m_error[start_index + i];

          num += wi * vpMath::sqr(m_error[start_index + i]);
          den += wi;

          for (unsigned int j = 0; j < m_L.getCols(); j++) {
            m_L[start_index + i][j] *= wi;
          }
        }

        //        start_index += nb_depth_dense_features;
      }

      computeVVSPoseEstimation(isoJoIdentity_, iter, m_L, LTL, m_weightedError, m_error, error_prev, LTR, mu, v);

      cMo_prev = cMo;
#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
      if (m_trackerType & KLT_TRACKER) {
        ctTc0_Prev = ctTc0;
      }
#endif

      cMo = vpExponentialMap::direct(v).inverse() * cMo;

#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
      if (m_trackerType & KLT_TRACKER) {
        ctTc0 = vpExponentialMap::direct(v).inverse() * ctTc0;
      }
#endif
      normRes_1 = normRes;

      normRes = sqrt(num / den);
    }

    iter++;
  }

  computeCovarianceMatrixVVS(isoJoIdentity_, W_true, cMo_prev, L_true, LVJ_true, m_error);

  if (m_trackerType & EDGE_TRACKER) {
    vpMbEdgeTracker::updateMovingEdgeWeights();
  }
}

void vpMbGenericTracker::TrackerWrapper::computeVVSInit()
{
  throw vpException(vpException::fatalError, "vpMbGenericTracker::"
                                             "TrackerWrapper::computeVVSInit("
                                             ") should not be called!");
}

void vpMbGenericTracker::TrackerWrapper::computeVVSInit(const vpImage<unsigned char> *const ptr_I)
{
  initMbtTracking(ptr_I);

  unsigned int nbFeatures = 0;

  if (m_trackerType & EDGE_TRACKER) {
    nbFeatures += m_error_edge.getRows();
  } else {
    m_error_edge.clear();
    m_weightedError_edge.clear();
    m_L_edge.clear();
    m_w_edge.clear();
  }

#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
  if (m_trackerType & KLT_TRACKER) {
    vpMbKltTracker::computeVVSInit();
    nbFeatures += m_error_klt.getRows();
  } else {
    m_error_klt.clear();
    m_weightedError_klt.clear();
    m_L_klt.clear();
    m_w_klt.clear();
  }
#endif

  if (m_trackerType & DEPTH_NORMAL_TRACKER) {
    vpMbDepthNormalTracker::computeVVSInit();
    nbFeatures += m_error_depthNormal.getRows();
  } else {
    m_error_depthNormal.clear();
    m_weightedError_depthNormal.clear();
    m_L_depthNormal.clear();
    m_w_depthNormal.clear();
  }

  if (m_trackerType & DEPTH_DENSE_TRACKER) {
    vpMbDepthDenseTracker::computeVVSInit();
    nbFeatures += m_error_depthDense.getRows();
  } else {
    m_error_depthDense.clear();
    m_weightedError_depthDense.clear();
    m_L_depthDense.clear();
    m_w_depthDense.clear();
  }

  m_L.resize(nbFeatures, 6, false, false);
  m_error.resize(nbFeatures, false);

  m_weightedError.resize(nbFeatures, false);
  m_w.resize(nbFeatures, false);
  m_w = 1;
}

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

void vpMbGenericTracker::TrackerWrapper::computeVVSInteractionMatrixAndResidu(const vpImage<unsigned char> *const ptr_I)
{
  if (m_trackerType & EDGE_TRACKER) {
    vpMbEdgeTracker::computeVVSInteractionMatrixAndResidu(*ptr_I);
  }

#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
  if (m_trackerType & KLT_TRACKER) {
    vpMbKltTracker::computeVVSInteractionMatrixAndResidu();
  }
#endif

  if (m_trackerType & DEPTH_NORMAL_TRACKER) {
    vpMbDepthNormalTracker::computeVVSInteractionMatrixAndResidu();
  }

  if (m_trackerType & DEPTH_DENSE_TRACKER) {
    vpMbDepthDenseTracker::computeVVSInteractionMatrixAndResidu();
  }

  unsigned int start_index = 0;
  if (m_trackerType & EDGE_TRACKER) {
    m_L.insert(m_L_edge, start_index, 0);
    m_error.insert(start_index, m_error_edge);

    start_index += m_error_edge.getRows();
  }

#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
  if (m_trackerType & KLT_TRACKER) {
    m_L.insert(m_L_klt, start_index, 0);
    m_error.insert(start_index, m_error_klt);

    start_index += m_error_klt.getRows();
  }
#endif

  if (m_trackerType & DEPTH_NORMAL_TRACKER) {
    m_L.insert(m_L_depthNormal, start_index, 0);
    m_error.insert(start_index, m_error_depthNormal);

    start_index += m_error_depthNormal.getRows();
  }

  if (m_trackerType & DEPTH_DENSE_TRACKER) {
    m_L.insert(m_L_depthDense, start_index, 0);
    m_error.insert(start_index, m_error_depthDense);

    //    start_index += m_error_depthDense.getRows();
  }
}

void vpMbGenericTracker::TrackerWrapper::computeVVSWeights()
{
  unsigned int start_index = 0;

  if (m_trackerType & EDGE_TRACKER) {
    vpMbEdgeTracker::computeVVSWeights();
    m_w.insert(start_index, m_w_edge);

    start_index += m_w_edge.getRows();
  }

#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
  if (m_trackerType & KLT_TRACKER) {
    vpMbTracker::computeVVSWeights(m_robust_klt, m_error_klt, m_w_klt);
    m_w.insert(start_index, m_w_klt);

    start_index += m_w_klt.getRows();
  }
#endif

  if (m_trackerType & DEPTH_NORMAL_TRACKER) {
    if (m_depthNormalUseRobust) {
      vpMbTracker::computeVVSWeights(m_robust_depthNormal, m_error_depthNormal, m_w_depthNormal);
      m_w.insert(start_index, m_w_depthNormal);
    }

    start_index += m_w_depthNormal.getRows();
  }

  if (m_trackerType & DEPTH_DENSE_TRACKER) {
    vpMbDepthDenseTracker::computeVVSWeights();
    m_w.insert(start_index, m_w_depthDense);

    //    start_index += m_w_depthDense.getRows();
  }
}

void vpMbGenericTracker::TrackerWrapper::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo_,
                                                 const vpCameraParameters &camera, const vpColor &col,
                                                 const unsigned int thickness, const bool displayFullModel)
{
  if (m_trackerType == EDGE_TRACKER) {
    vpMbEdgeTracker::display(I, cMo_, camera, col, thickness, displayFullModel);
#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
  } else if (m_trackerType == KLT_TRACKER) {
    vpMbKltTracker::display(I, cMo_, camera, col, thickness, displayFullModel);
#endif
  } else if (m_trackerType == DEPTH_NORMAL_TRACKER) {
    vpMbDepthNormalTracker::display(I, cMo_, camera, col, thickness, displayFullModel);
  } else if (m_trackerType == DEPTH_DENSE_TRACKER) {
    vpMbDepthDenseTracker::display(I, cMo_, camera, col, thickness, displayFullModel);
  } else {
    if (m_trackerType & EDGE_TRACKER) {
      for (unsigned int i = 0; i < scales.size(); i += 1) {
        if (scales[i]) {
          for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin();
               it != lines[scaleLevel].end(); ++it) {
            (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
          }

          for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
               it != cylinders[scaleLevel].end(); ++it) {
            (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
          }

          for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
               it != circles[scaleLevel].end(); ++it) {
            (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
          }

          break; // display model on one scale only
        }
      }
    }

#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
    if (m_trackerType & KLT_TRACKER) {
      for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end();
           ++it) {
        vpMbtDistanceKltPoints *kltpoly = *it;
        if (displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) {
          kltpoly->displayPrimitive(I);
        }
      }

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

    if (m_trackerType & DEPTH_NORMAL_TRACKER) {
      vpMbDepthNormalTracker::display(I, cMo_, camera, col, thickness, displayFullModel);
    }

    if (m_trackerType & DEPTH_DENSE_TRACKER) {
      vpMbDepthDenseTracker::display(I, cMo_, camera, col, thickness, displayFullModel);
    }

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

void vpMbGenericTracker::TrackerWrapper::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo_,
                                                 const vpCameraParameters &camera, const vpColor &col,
                                                 const unsigned int thickness, const bool displayFullModel)
{
  if (m_trackerType == EDGE_TRACKER) {
    vpMbEdgeTracker::display(I, cMo_, camera, col, thickness, displayFullModel);
#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
  } else if (m_trackerType == KLT_TRACKER) {
    vpMbKltTracker::display(I, cMo_, camera, col, thickness, displayFullModel);
#endif
  } else if (m_trackerType == DEPTH_NORMAL_TRACKER) {
    vpMbDepthNormalTracker::display(I, cMo_, camera, col, thickness, displayFullModel);
  } else if (m_trackerType == DEPTH_DENSE_TRACKER) {
    vpMbDepthDenseTracker::display(I, cMo_, camera, col, thickness, displayFullModel);
  } else {
    if (m_trackerType & EDGE_TRACKER) {
      for (unsigned int i = 0; i < scales.size(); i += 1) {
        if (scales[i]) {
          for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin();
               it != lines[scaleLevel].end(); ++it) {
            (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
          }

          for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
               it != cylinders[scaleLevel].end(); ++it) {
            (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
          }

          for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
               it != circles[scaleLevel].end(); ++it) {
            (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
          }

          break; // display model on one scale only
        }
      }
    }

#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
    if (m_trackerType & KLT_TRACKER) {
      for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end();
           ++it) {
        vpMbtDistanceKltPoints *kltpoly = *it;
        if (displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) {
          kltpoly->displayPrimitive(I);
        }
      }

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

    if (m_trackerType & DEPTH_NORMAL_TRACKER) {
      vpMbDepthNormalTracker::display(I, cMo_, camera, col, thickness, displayFullModel);
    }

    if (m_trackerType & DEPTH_DENSE_TRACKER) {
      vpMbDepthNormalTracker::display(I, cMo_, camera, col, thickness, displayFullModel);
    }

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

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

  if (useScanLine || clippingFlag > 3)
    cam.computeFov(I.getWidth(), I.getHeight());

  bool reInitialisation = false;
  if (!useOgre) {
    faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
  } else {
#ifdef VISP_HAVE_OGRE
    if (!faces.isOgreInitialised()) {
      faces.setBackgroundSizeOgre(I.getHeight(), I.getWidth());

      faces.setOgreShowConfigDialog(ogreShowConfigDialog);
      faces.initOgre(cam);
      // Turn off Ogre config dialog display for the next call to this
      // function since settings are saved in the ogre.cfg file and used
      // during the next call
      ogreShowConfigDialog = false;
    }

    faces.setVisibleOgre(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
#else
    faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
#endif
  }

  if (useScanLine) {
    faces.computeClippedPolygons(cMo, cam);
    faces.computeScanLineRender(cam, I.getWidth(), I.getHeight());
  }

#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
  if (m_trackerType & KLT_TRACKER)
    vpMbKltTracker::reinit(I);
#endif

  if (m_trackerType & EDGE_TRACKER) {
    vpMbEdgeTracker::resetMovingEdge();

    bool a = false;
    vpMbEdgeTracker::visibleFace(I, cMo, a); // should be useless, but keep it for nbvisiblepolygone

    initMovingEdge(I, cMo);
  }

  if (m_trackerType & DEPTH_NORMAL_TRACKER)
    vpMbDepthNormalTracker::computeVisibility(I.getWidth(), I.getHeight()); // vpMbDepthNormalTracker::init(I);

  if (m_trackerType & DEPTH_DENSE_TRACKER)
    vpMbDepthDenseTracker::computeVisibility(I.getWidth(), I.getHeight()); // vpMbDepthDenseTracker::init(I);
}

void vpMbGenericTracker::TrackerWrapper::initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3,
                                                    const double radius, const int idFace, const std::string &name)
{
  if (m_trackerType & EDGE_TRACKER)
    vpMbEdgeTracker::initCircle(p1, p2, p3, radius, idFace, name);

#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
  if (m_trackerType & KLT_TRACKER)
    vpMbKltTracker::initCircle(p1, p2, p3, radius, idFace, name);
#endif
}

void vpMbGenericTracker::TrackerWrapper::initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius,
                                                      const int idFace, const std::string &name)
{
  if (m_trackerType & EDGE_TRACKER)
    vpMbEdgeTracker::initCylinder(p1, p2, radius, idFace, name);

#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
  if (m_trackerType & KLT_TRACKER)
    vpMbKltTracker::initCylinder(p1, p2, radius, idFace, name);
#endif
}

void vpMbGenericTracker::TrackerWrapper::initFaceFromCorners(vpMbtPolygon &polygon)
{
  if (m_trackerType & EDGE_TRACKER)
    vpMbEdgeTracker::initFaceFromCorners(polygon);

#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
  if (m_trackerType & KLT_TRACKER)
    vpMbKltTracker::initFaceFromCorners(polygon);
#endif

  if (m_trackerType & DEPTH_NORMAL_TRACKER)
    vpMbDepthNormalTracker::initFaceFromCorners(polygon);

  if (m_trackerType & DEPTH_DENSE_TRACKER)
    vpMbDepthDenseTracker::initFaceFromCorners(polygon);
}

void vpMbGenericTracker::TrackerWrapper::initFaceFromLines(vpMbtPolygon &polygon)
{
  if (m_trackerType & EDGE_TRACKER)
    vpMbEdgeTracker::initFaceFromLines(polygon);

#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
  if (m_trackerType & KLT_TRACKER)
    vpMbKltTracker::initFaceFromLines(polygon);
#endif

  if (m_trackerType & DEPTH_NORMAL_TRACKER)
    vpMbDepthNormalTracker::initFaceFromLines(polygon);

  if (m_trackerType & DEPTH_DENSE_TRACKER)
    vpMbDepthDenseTracker::initFaceFromLines(polygon);
}

void vpMbGenericTracker::TrackerWrapper::initMbtTracking(const vpImage<unsigned char> *const ptr_I)
{
  if (m_trackerType & EDGE_TRACKER) {
    vpMbEdgeTracker::computeVVSInit();
    vpMbEdgeTracker::computeVVSFirstPhaseFactor(*ptr_I, 0);
  }
}

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

#ifdef VISP_HAVE_XML2
  vpMbtXmlGenericParser xmlp((vpMbtXmlGenericParser::vpParserType)m_trackerType);

  xmlp.setCameraParameters(cam);
  xmlp.setAngleAppear(vpMath::deg(angleAppears));
  xmlp.setAngleDisappear(vpMath::deg(angleDisappears));

  // Edge
  xmlp.setEdgeMe(me);

  // KLT
  xmlp.setKltMaxFeatures(10000);
  xmlp.setKltWindowSize(5);
  xmlp.setKltQuality(0.01);
  xmlp.setKltMinDistance(5);
  xmlp.setKltHarrisParam(0.01);
  xmlp.setKltBlockSize(3);
  xmlp.setKltPyramidLevels(3);
#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
  xmlp.setKltMaskBorder(maskBorder);
#endif

  // Depth normal
  xmlp.setDepthNormalFeatureEstimationMethod(m_depthNormalFeatureEstimationMethod);
  xmlp.setDepthNormalPclPlaneEstimationMethod(m_depthNormalPclPlaneEstimationMethod);
  xmlp.setDepthNormalPclPlaneEstimationRansacMaxIter(m_depthNormalPclPlaneEstimationRansacMaxIter);
  xmlp.setDepthNormalPclPlaneEstimationRansacThreshold(m_depthNormalPclPlaneEstimationRansacThreshold);
  xmlp.setDepthNormalSamplingStepX(m_depthNormalSamplingStepX);
  xmlp.setDepthNormalSamplingStepY(m_depthNormalSamplingStepY);

  // Depth dense
  xmlp.setDepthDenseSamplingStepX(m_depthDenseSamplingStepX);
  xmlp.setDepthDenseSamplingStepY(m_depthDenseSamplingStepY);

  try {

    std::cout << " *********** Parsing XML for";

    std::vector<std::string> tracker_names;
    if (m_trackerType & EDGE_TRACKER)
      tracker_names.push_back("Edge");
#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
    if (m_trackerType & KLT_TRACKER)
      tracker_names.push_back("Klt");
#endif
    if (m_trackerType & DEPTH_NORMAL_TRACKER)
      tracker_names.push_back("Depth Normal");
    if (m_trackerType & DEPTH_DENSE_TRACKER)
      tracker_names.push_back("Depth Dense");

    for (size_t i = 0; i < tracker_names.size(); i++) {
      std::cout << " " << tracker_names[i];
      if (i == tracker_names.size() - 1) {
        std::cout << " ";
      }
    }

    std::cout << "Model-Based Tracker ************ " << std::endl;
    xmlp.parse(configFile);
  } catch (...) {
    throw vpException(vpException::ioError, "Can't open XML file \"%s\"\n ", configFile.c_str());
  }

  vpCameraParameters camera;
  xmlp.getCameraParameters(camera);
  setCameraParameters(camera);

  angleAppears = vpMath::rad(xmlp.getAngleAppear());
  angleDisappears = vpMath::rad(xmlp.getAngleDisappear());

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

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

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

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

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

  // Edge
  vpMe meParser;
  xmlp.getEdgeMe(meParser);
  vpMbEdgeTracker::setMovingEdge(meParser);

// KLT
#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
  tracker.setMaxFeatures((int)xmlp.getKltMaxFeatures());
  tracker.setWindowSize((int)xmlp.getKltWindowSize());
  tracker.setQuality(xmlp.getKltQuality());
  tracker.setMinDistance(xmlp.getKltMinDistance());
  tracker.setHarrisFreeParameter(xmlp.getKltHarrisParam());
  tracker.setBlockSize((int)xmlp.getKltBlockSize());
  tracker.setPyramidLevels((int)xmlp.getKltPyramidLevels());
  maskBorder = xmlp.getKltMaskBorder();

  // if(useScanLine)
  faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
#endif

  // Depth normal
  setDepthNormalFeatureEstimationMethod(xmlp.getDepthNormalFeatureEstimationMethod());
  setDepthNormalPclPlaneEstimationMethod(xmlp.getDepthNormalPclPlaneEstimationMethod());
  setDepthNormalPclPlaneEstimationRansacMaxIter(xmlp.getDepthNormalPclPlaneEstimationRansacMaxIter());
  setDepthNormalPclPlaneEstimationRansacThreshold(xmlp.getDepthNormalPclPlaneEstimationRansacThreshold());
  setDepthNormalSamplingStep(xmlp.getDepthNormalSamplingStepX(), xmlp.getDepthNormalSamplingStepY());

  // Depth dense
  setDepthDenseSamplingStep(xmlp.getDepthDenseSamplingStepX(), xmlp.getDepthDenseSamplingStepY());
#else
  std::cerr << "You need the libXML2 to read the config file: " << configFile << std::endl;
#endif
}

#ifdef VISP_HAVE_PCL
void vpMbGenericTracker::TrackerWrapper::postTracking(const vpImage<unsigned char> *const ptr_I,
                                                      const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
{
  if (displayFeatures) {
    if (m_trackerType & EDGE_TRACKER) {
      vpMbEdgeTracker::displayFeaturesOnImage(*ptr_I, 0);
    }
  }

#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
  // KLT
  if (m_trackerType & KLT_TRACKER) {
    if (vpMbKltTracker::postTracking(*ptr_I, m_w_klt)) {
      vpMbKltTracker::reinit(*ptr_I);
    }
  }
#endif

  // Looking for new visible face
  if (m_trackerType & EDGE_TRACKER) {
    bool newvisibleface = false;
    vpMbEdgeTracker::visibleFace(*ptr_I, cMo, newvisibleface);

    if (useScanLine) {
      faces.computeClippedPolygons(cMo, cam);
      faces.computeScanLineRender(cam, ptr_I->getWidth(), ptr_I->getHeight());
    }
  }

  // Depth normal
  if (m_trackerType & DEPTH_NORMAL_TRACKER)
    vpMbDepthNormalTracker::computeVisibility(point_cloud->width, point_cloud->height);

  // Depth dense
  if (m_trackerType & DEPTH_DENSE_TRACKER)
    vpMbDepthDenseTracker::computeVisibility(point_cloud->width, point_cloud->height);

  // Edge
  if (m_trackerType & EDGE_TRACKER) {
    vpMbEdgeTracker::updateMovingEdge(*ptr_I);

    vpMbEdgeTracker::initMovingEdge(*ptr_I, cMo);
    // Reinit the moving edge for the lines which need it.
    vpMbEdgeTracker::reinitMovingEdge(*ptr_I, cMo);

    if (computeProjError) {
      vpMbEdgeTracker::computeProjectionError(*ptr_I);
    }
  }
}

void vpMbGenericTracker::TrackerWrapper::preTracking(const vpImage<unsigned char> *const ptr_I,
                                                     const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
{
  if (m_trackerType & EDGE_TRACKER) {
    try {
      vpMbEdgeTracker::trackMovingEdge(*ptr_I);
    } catch (...) {
      std::cerr << "Error in moving edge tracking" << std::endl;
      throw;
    }
  }

#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
  if (m_trackerType & KLT_TRACKER) {
    try {
      vpMbKltTracker::preTracking(*ptr_I);
    } catch (const vpException &e) {
      std::cerr << "Error in KLT tracking: " << e.what() << std::endl;
      throw;
    }
  }
#endif

  if (m_trackerType & DEPTH_NORMAL_TRACKER) {
    try {
      vpMbDepthNormalTracker::segmentPointCloud(point_cloud);
    } catch (...) {
      std::cerr << "Error in Depth normal tracking" << std::endl;
      throw;
    }
  }

  if (m_trackerType & DEPTH_DENSE_TRACKER) {
    try {
      vpMbDepthDenseTracker::segmentPointCloud(point_cloud);
    } catch (...) {
      std::cerr << "Error in Depth dense tracking" << std::endl;
      throw;
    }
  }
}
#endif

void vpMbGenericTracker::TrackerWrapper::postTracking(const vpImage<unsigned char> *const ptr_I,
                                                      const unsigned int pointcloud_width,
                                                      const unsigned int pointcloud_height)
{
  if (displayFeatures) {
    if (m_trackerType & EDGE_TRACKER) {
      vpMbEdgeTracker::displayFeaturesOnImage(*ptr_I, 0);
    }
  }

#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
  // KLT
  if (m_trackerType & KLT_TRACKER) {
    if (vpMbKltTracker::postTracking(*ptr_I, m_w_klt)) {
      vpMbKltTracker::reinit(*ptr_I);
    }
  }
#endif

  // Looking for new visible face
  if (m_trackerType & EDGE_TRACKER) {
    bool newvisibleface = false;
    vpMbEdgeTracker::visibleFace(*ptr_I, cMo, newvisibleface);

    if (useScanLine) {
      faces.computeClippedPolygons(cMo, cam);
      faces.computeScanLineRender(cam, ptr_I->getWidth(), ptr_I->getHeight());
    }
  }

  // Depth normal
  if (m_trackerType & DEPTH_NORMAL_TRACKER)
    vpMbDepthNormalTracker::computeVisibility(pointcloud_width, pointcloud_height);

  // Depth dense
  if (m_trackerType & DEPTH_DENSE_TRACKER)
    vpMbDepthDenseTracker::computeVisibility(pointcloud_width, pointcloud_height);

  // Edge
  if (m_trackerType & EDGE_TRACKER) {
    vpMbEdgeTracker::updateMovingEdge(*ptr_I);

    vpMbEdgeTracker::initMovingEdge(*ptr_I, cMo);
    // Reinit the moving edge for the lines which need it.
    vpMbEdgeTracker::reinitMovingEdge(*ptr_I, cMo);

    if (computeProjError) {
      vpMbEdgeTracker::computeProjectionError(*ptr_I);
    }
  }
}

void vpMbGenericTracker::TrackerWrapper::preTracking(const vpImage<unsigned char> *const ptr_I,
                                                     const std::vector<vpColVector> *const point_cloud,
                                                     const unsigned int pointcloud_width,
                                                     const unsigned int pointcloud_height)
{
  if (m_trackerType & EDGE_TRACKER) {
    try {
      vpMbEdgeTracker::trackMovingEdge(*ptr_I);
    } catch (...) {
      std::cerr << "Error in moving edge tracking" << std::endl;
      throw;
    }
  }

#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
  if (m_trackerType & KLT_TRACKER) {
    try {
      vpMbKltTracker::preTracking(*ptr_I);
    } catch (const vpException &e) {
      std::cerr << "Error in KLT tracking: " << e.what() << std::endl;
      throw;
    }
  }
#endif

  if (m_trackerType & DEPTH_NORMAL_TRACKER) {
    try {
      vpMbDepthNormalTracker::segmentPointCloud(*point_cloud, pointcloud_width, pointcloud_height);
    } catch (...) {
      std::cerr << "Error in Depth tracking" << std::endl;
      throw;
    }
  }

  if (m_trackerType & DEPTH_DENSE_TRACKER) {
    try {
      vpMbDepthDenseTracker::segmentPointCloud(*point_cloud, pointcloud_width, pointcloud_height);
    } catch (...) {
      std::cerr << "Error in Depth dense tracking" << std::endl;
      throw;
    }
  }
}

void vpMbGenericTracker::TrackerWrapper::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
                                                     const vpHomogeneousMatrix &cMo_, const bool verbose,
                                                     const vpHomogeneousMatrix &T)
{
  cMo.eye();

  // Edge
  vpMbtDistanceLine *l;
  vpMbtDistanceCylinder *cy;
  vpMbtDistanceCircle *ci;

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

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

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

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

  nline = 0;
  ncylinder = 0;
  ncircle = 0;
  nbvisiblepolygone = 0;

// KLT
#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
  if (cur != NULL) {
    cvReleaseImage(&cur);
    cur = NULL;
  }
#endif

  // delete the Klt Polygon features
  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
    vpMbtDistanceKltPoints *kltpoly = *it;
    if (kltpoly != NULL) {
      delete kltpoly;
    }
    kltpoly = NULL;
  }
  kltPolygons.clear();

  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
       ++it) {
    vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
    if (kltPolyCylinder != NULL) {
      delete kltPolyCylinder;
    }
    kltPolyCylinder = NULL;
  }
  kltCylinders.clear();

  // delete the structures used to display circles
  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
    ci = *it;
    if (ci != NULL) {
      delete ci;
    }
    ci = NULL;
  }
  circles_disp.clear();

  firstInitialisation = true;

#endif

  // Depth normal
  for (size_t i = 0; i < m_depthNormalFaces.size(); i++) {
    delete m_depthNormalFaces[i];
    m_depthNormalFaces[i] = NULL;
  }
  m_depthNormalFaces.clear();

  // Depth dense
  for (size_t i = 0; i < m_depthDenseFaces.size(); i++) {
    delete m_depthDenseFaces[i];
    m_depthDenseFaces[i] = NULL;
  }
  m_depthDenseFaces.clear();

  faces.reset();

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

void vpMbGenericTracker::TrackerWrapper::resetTracker()
{
  vpMbEdgeTracker::resetTracker();
#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
  vpMbKltTracker::resetTracker();
#endif
  vpMbDepthNormalTracker::resetTracker();
  vpMbDepthDenseTracker::resetTracker();
}

void vpMbGenericTracker::TrackerWrapper::setCameraParameters(const vpCameraParameters &camera)
{
  this->cam = camera;

  vpMbEdgeTracker::setCameraParameters(cam);
#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
  vpMbKltTracker::setCameraParameters(cam);
#endif
  vpMbDepthNormalTracker::setCameraParameters(cam);
  vpMbDepthDenseTracker::setCameraParameters(cam);
}

void vpMbGenericTracker::TrackerWrapper::setClipping(const unsigned int &flags)
{
  vpMbEdgeTracker::setClipping(flags);
}

void vpMbGenericTracker::TrackerWrapper::setFarClippingDistance(const double &dist)
{
  vpMbEdgeTracker::setFarClippingDistance(dist);
}

void vpMbGenericTracker::TrackerWrapper::setNearClippingDistance(const double &dist)
{
  vpMbEdgeTracker::setNearClippingDistance(dist);
}

void vpMbGenericTracker::TrackerWrapper::setOgreVisibilityTest(const bool &v)
{
  vpMbTracker::setOgreVisibilityTest(v);
#ifdef VISP_HAVE_OGRE
  faces.getOgreContext()->setWindowName("TrackerWrapper");
#endif
}

void vpMbGenericTracker::TrackerWrapper::setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cdMo)
{
  bool performKltSetPose = false;

#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
  if (m_trackerType & KLT_TRACKER) {
    performKltSetPose = true;

    if (useScanLine || clippingFlag > 3)
      cam.computeFov(I.getWidth(), I.getHeight());

    vpMbKltTracker::setPose(I, cdMo);
  }
#endif

  if (!performKltSetPose) {
    cMo = cdMo;
    init(I);
    return;
  }

  if (m_trackerType & EDGE_TRACKER)
    resetMovingEdge();

  if (useScanLine) {
    faces.computeClippedPolygons(cMo, cam);
    faces.computeScanLineRender(cam, I.getWidth(), I.getHeight());
  }

#if 0
  if (m_trackerType & EDGE_TRACKER) {
    initPyramid(I, Ipyramid);

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

    cleanPyramid(Ipyramid);
  }
#else
  if (m_trackerType & EDGE_TRACKER)
    initMovingEdge(I, cMo);
#endif

  // Depth normal
  vpMbDepthNormalTracker::computeVisibility(I.getWidth(), I.getHeight());

  // Depth dense
  vpMbDepthDenseTracker::computeVisibility(I.getWidth(), I.getHeight());
}

void vpMbGenericTracker::TrackerWrapper::setProjectionErrorComputation(const bool &flag)
{
  vpMbEdgeTracker::setProjectionErrorComputation(flag);
}

void vpMbGenericTracker::TrackerWrapper::setScanLineVisibilityTest(const bool &v)
{
  vpMbEdgeTracker::setScanLineVisibilityTest(v);
#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
  vpMbKltTracker::setScanLineVisibilityTest(v);
#endif
  vpMbDepthNormalTracker::setScanLineVisibilityTest(v);
  vpMbDepthDenseTracker::setScanLineVisibilityTest(v);
}

void vpMbGenericTracker::TrackerWrapper::setTrackerType(const int type)
{
  if ((type & (EDGE_TRACKER |
#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
               KLT_TRACKER |
#endif
               DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER)) == 0) {
    throw vpException(vpTrackingException::badValue, "bad value for tracker type: !", type);
  }

  m_trackerType = type;
}

void vpMbGenericTracker::TrackerWrapper::testTracking()
{
  if (m_trackerType & EDGE_TRACKER) {
    vpMbEdgeTracker::testTracking();
  }
}

void vpMbGenericTracker::TrackerWrapper::track(const vpImage<unsigned char> &
#ifdef VISP_HAVE_PCL
                                                   I
#endif
)
{
  if ((m_trackerType & (EDGE_TRACKER
#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
                        | KLT_TRACKER
#endif
                        )) == 0) {
    std::cerr << "Bad tracker type: " << m_trackerType << std::endl;
    return;
  }

#ifdef VISP_HAVE_PCL
  track(&I, nullptr);
#endif
}

#ifdef VISP_HAVE_PCL
void vpMbGenericTracker::TrackerWrapper::track(const vpImage<unsigned char> *const ptr_I,
                                               const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
{
  if ((m_trackerType & (EDGE_TRACKER |
#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
                        KLT_TRACKER |
#endif
                        DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER)) == 0) {
    std::cerr << "Bad tracker type: " << m_trackerType << std::endl;
    return;
  }

  if (m_trackerType & (EDGE_TRACKER
#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
                       | KLT_TRACKER
#endif
                       ) &&
      ptr_I == NULL) {
    throw vpException(vpException::fatalError, "Image pointer is NULL!");
  }

  if (m_trackerType & (DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER) && point_cloud == nullptr) {
    throw vpException(vpException::fatalError, "Pointcloud smart pointer is NULL!");
  }

  // Back-up cMo in case of exception
  vpHomogeneousMatrix cMo_1 = cMo;
  try {
    preTracking(ptr_I, point_cloud);

    try {
      computeVVS(ptr_I);
    } catch (...) {
      covarianceMatrix = -1;
      throw; // throw the original exception
    }

    if (m_trackerType == EDGE_TRACKER)
      testTracking();

    postTracking(ptr_I, point_cloud);

  } catch (const vpException &e) {
    std::cerr << "Exception: " << e.what() << std::endl;
    cMo = cMo_1;
    throw; // rethrowing the original exception
  }
}
#endif