vpMbGenericTracker.cpp

/*
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2025 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 https://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/core/vpIoTools.h>
#include <visp3/mbt/vpMbtXmlGenericParser.h>
 
#ifdef VISP_HAVE_NLOHMANN_JSON
#include VISP_NLOHMANN_JSON(json.hpp)
using json = nlohmann::json; 
#endif
 
BEGIN_VISP_NAMESPACE
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_nb_feat_edge(0), m_nb_feat_klt(0), m_nb_feat_depthNormal(0), m_nb_feat_depthDense(0)
{
  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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
  m_mapOfFeatureFactors[KLT_TRACKER] = 1.0;
#endif
 
  m_mapOfFeatureFactors[DEPTH_NORMAL_TRACKER] = 1.0;
  m_mapOfFeatureFactors[DEPTH_DENSE_TRACKER] = 1.0;
}
 
vpMbGenericTracker::vpMbGenericTracker(unsigned int nbCameras, 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(),
  m_nb_feat_edge(0), m_nb_feat_klt(0), m_nb_feat_depthNormal(0), m_nb_feat_depthDense(0)
{
  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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
  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(),
  m_nb_feat_edge(0), m_nb_feat_klt(0), m_nb_feat_depthNormal(0), m_nb_feat_depthDense(0)
{
  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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
  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(),
  m_nb_feat_edge(0), m_nb_feat_klt(0), m_nb_feat_depthNormal(0), m_nb_feat_depthDense(0)
{
  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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
  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 = nullptr;
  }
}

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 / static_cast<double>(nbTotalFeaturesUsed));
  }
 
  return 90.0;
}

double vpMbGenericTracker::computeCurrentProjectionError(const vpImage<vpRGBa> &I_color,
  const vpHomogeneousMatrix &_cMo,
  const vpCameraParameters &_cam)
{
  vpImage<unsigned char> I;
  vpImageConvert::convert(I_color, I); // FS: Shouldn't we use here m_I that was converted in track() ?
 
  return computeCurrentProjectionError(I, _cMo, _cam);
}
 
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 / static_cast<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 = m_isoJoIdentity; // Backup since it can be modified if L is not full rank
  if (isoJoIdentity)
    oJo.eye();
 
  // 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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
  double factorKlt = m_mapOfFeatureFactors[KLT_TRACKER];
#endif
  double factorDepth = m_mapOfFeatureFactors[DEPTH_NORMAL_TRACKER];
  double factorDepthDense = m_mapOfFeatureFactors[DEPTH_DENSE_TRACKER];
 
  m_nb_feat_edge = 0;
  m_nb_feat_klt = 0;
  m_nb_feat_depthNormal = 0;
  m_nb_feat_depthDense = 0;
 
  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->m_cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo_prev;
 
#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
        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(m_cMo);
          LVJ_true = (m_L * (cVo * oJo));
        }
      }
 
      vpVelocityTwistMatrix cVo;
      if (iter == 0) {
        // 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 considering
        // circles (rank 5) and cylinders (rank 4)
        if (isoJoIdentity) {
          cVo.buildFrom(m_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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
        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 = m_cMo;
 
      m_cMo = vpExponentialMap::direct(v).inverse() * m_cMo;
 
#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
      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] * m_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->m_cMo = m_mapOfCameraTransformationMatrix[it->first] * m_cMo;
      }
    }
 
    iter++;
  }
 
  // Update features number
  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) {
      m_nb_feat_edge += tracker->m_error_edge.size();
    }
#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
    if (tracker->m_trackerType & KLT_TRACKER) {
      m_nb_feat_klt += tracker->m_error_klt.size();
    }
#endif
    if (tracker->m_trackerType & DEPTH_NORMAL_TRACKER) {
      m_nb_feat_depthNormal += tracker->m_error_depthNormal.size();
    }
    if (tracker->m_trackerType & DEPTH_DENSE_TRACKER) {
      m_nb_feat_depthDense += tracker->m_error_depthDense.size();
    }
  }
 
  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->m_cMo = m_mapOfCameraTransformationMatrix[it->first] * m_cMo;
#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
    vpHomogeneousMatrix c_curr_tTc_curr0 =
      m_mapOfCameraTransformationMatrix[it->first] * m_cMo * tracker->c0Mo.inverse();
    tracker->ctTc0 = c_curr_tTc_curr0;
#endif
 
    tracker->computeVVSInteractionMatrixAndResidu(mapOfImages[it->first]);
 
    if (tracker->m_L.getRows() > 0) {
      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, unsigned int thickness,
  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, unsigned int thickness,
  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,
  unsigned int thickness, 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, unsigned int thickness,
  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, unsigned int thickness, 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, unsigned int thickness, 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 &camera) const
{
  vpMbTracker::getCameraParameters(camera);
}

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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
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

std::vector<std::vector<double> > vpMbGenericTracker::getFeaturesForDisplay()
{
  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
 
  if (it != m_mapOfTrackers.end()) {
    return it->second->getFeaturesForDisplay();
  }
  else {
    std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist!" << std::endl;
  }
 
  return std::vector<std::vector<double> >();
}

void vpMbGenericTracker::getFeaturesForDisplay(std::map<std::string, std::vector<std::vector<double> > > &mapOfFeatures)
{
  // Clear the input map
  mapOfFeatures.clear();
 
  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
    it != m_mapOfTrackers.end(); ++it) {
    mapOfFeatures[it->first] = it->second->getFeaturesForDisplay();
  }
}

double vpMbGenericTracker::getGoodMovingEdgesRatioThreshold() const { return m_percentageGdPt; }
 
#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
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();
  }
}

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

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

void vpMbGenericTracker::getLcircle(std::list<vpMbtDistanceCircle *> &circlesList, 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,
  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, 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,
  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, 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,
  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;
  }
}

std::vector<std::vector<double> > vpMbGenericTracker::getModelForDisplay(unsigned int width, unsigned int height,
  const vpHomogeneousMatrix &cMo,
  const vpCameraParameters &cam,
  bool displayFullModel)
{
  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
 
  if (it != m_mapOfTrackers.end()) {
    return it->second->getModelForDisplay(width, height, cMo, cam, displayFullModel);
  }
  else {
    std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist!" << std::endl;
  }
 
  return std::vector<std::vector<double> >();
}

void vpMbGenericTracker::getModelForDisplay(std::map<std::string, std::vector<std::vector<double> > > &mapOfModels,
  const std::map<std::string, unsigned int> &mapOfwidths,
  const std::map<std::string, unsigned int> &mapOfheights,
  const std::map<std::string, vpHomogeneousMatrix> &mapOfcMos,
  const std::map<std::string, vpCameraParameters> &mapOfCams,
  bool displayFullModel)
{
  // Clear the input map
  mapOfModels.clear();
 
  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
    it != m_mapOfTrackers.end(); ++it) {
    std::map<std::string, unsigned int>::const_iterator findWidth = mapOfwidths.find(it->first);
    std::map<std::string, unsigned int>::const_iterator findHeight = mapOfheights.find(it->first);
    std::map<std::string, vpHomogeneousMatrix>::const_iterator findcMo = mapOfcMos.find(it->first);
    std::map<std::string, vpCameraParameters>::const_iterator findCam = mapOfCams.find(it->first);
 
    if (findWidth != mapOfwidths.end() && findHeight != mapOfheights.end() && findcMo != mapOfcMos.end() &&
      findCam != mapOfCams.end()) {
      mapOfModels[it->first] = it->second->getModelForDisplay(findWidth->second, findHeight->second, findcMo->second,
        findCam->second, displayFullModel);
    }
  }
}

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(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, 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(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 nullptr;
}

vpMbtPolygon *vpMbGenericTracker::getPolygon(const std::string &cameraName, 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 nullptr;
}

std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > >
vpMbGenericTracker::getPolygonFaces(bool orderPolygons, bool useVisibility, 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,
  bool orderPolygons, bool useVisibility, 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 &cMo) const { vpMbTracker::getPose(cMo); }

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

std::string vpMbGenericTracker::getReferenceCameraName() const { return m_referenceCameraName; }

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->m_cMo = m_mapOfCameraTransformationMatrix[it->first] * m_cMo;
    tracker->init(I);
  }
}
 
void vpMbGenericTracker::initCircle(const vpPoint & /*p1*/, const vpPoint & /*p2*/, const vpPoint & /*p3*/,
                                    double /*radius*/, 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, 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(m_cMo);
    }
  }
  else {
    throw vpException(vpTrackingException::initializationError,
      "Cannot initClick()! Require two cameras but there are %d cameras!", m_mapOfTrackers.size());
  }
}

void vpMbGenericTracker::initClick(const vpImage<vpRGBa> &I_color1, const vpImage<vpRGBa> &I_color2,
  const std::string &initFile1, const std::string &initFile2, 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(I_color1, initFile1, displayHelp, T1);
 
    ++it;
 
    tracker = it->second;
    tracker->initClick(I_color2, initFile2, displayHelp, T2);
 
    it = m_mapOfTrackers.find(m_referenceCameraName);
    if (it != m_mapOfTrackers.end()) {
      tracker = it->second;
 
      // Set the reference cMo
      tracker->getPose(m_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, 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(m_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 * m_cMo;
      m_mapOfTrackers[*it]->m_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());
    }
  }
}

void vpMbGenericTracker::initClick(const std::map<std::string, const vpImage<vpRGBa> *> &mapOfColorImages,
  const std::map<std::string, std::string> &mapOfInitFiles, 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<vpRGBa> *>::const_iterator it_img = mapOfColorImages.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 != mapOfColorImages.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(m_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 = mapOfColorImages.find(it_tracker->first);
      it_initFile = mapOfInitFiles.find(it_tracker->first);
 
      if (it_img != mapOfColorImages.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 = mapOfColorImages.find(*it);
    std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
      m_mapOfCameraTransformationMatrix.find(*it);
 
    if (it_img != mapOfColorImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
      vpHomogeneousMatrix cCurrentMo = it_camTrans->second * m_cMo;
      m_mapOfTrackers[*it]->m_cMo = cCurrentMo;
      vpImageConvert::convert(*it_img->second, m_mapOfTrackers[*it]->m_I);
      m_mapOfTrackers[*it]->init(m_mapOfTrackers[*it]->m_I);
    }
    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(m_cMo);
 
      // Set the reference camera parameters
      tracker->getCameraParameters(m_cam);
    }
  }
  else {
    throw vpException(vpTrackingException::initializationError,
      "Cannot initFromPoints()! Require two cameras but "
      "there are %d cameras!",
      m_mapOfTrackers.size());
  }
}

void vpMbGenericTracker::initFromPoints(const vpImage<vpRGBa> &I_color1, const vpImage<vpRGBa> &I_color2,
  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(I_color1, initFile1);
 
    ++it;
 
    tracker = it->second;
    tracker->initFromPoints(I_color2, initFile2);
 
    it = m_mapOfTrackers.find(m_referenceCameraName);
    if (it != m_mapOfTrackers.end()) {
      tracker = it->second;
 
      // Set the reference cMo
      tracker->getPose(m_cMo);
 
      // Set the reference camera parameters
      tracker->getCameraParameters(m_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(m_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 * m_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::initFromPoints(const std::map<std::string, const vpImage<vpRGBa> *> &mapOfColorImages,
  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<vpRGBa> *>::const_iterator it_img = mapOfColorImages.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 != mapOfColorImages.end() &&
    it_initPoints != mapOfInitPoints.end()) {
    TrackerWrapper *tracker = it_tracker->second;
    tracker->initFromPoints(*it_img->second, it_initPoints->second);
    tracker->getPose(m_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 = mapOfColorImages.find(it_tracker->first);
    it_initPoints = mapOfInitPoints.find(it_tracker->first);
 
    if (it_img != mapOfColorImages.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 = mapOfColorImages.find(*it);
    std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
      m_mapOfCameraTransformationMatrix.find(*it);
 
    if (it_img != mapOfColorImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
      vpHomogeneousMatrix cCurrentMo = it_camTrans->second * m_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> &I, const vpHomogeneousMatrix &cMo)
{
  vpMbTracker::initFromPose(I, cMo);
}

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

void vpMbGenericTracker::initFromPose(const vpImage<vpRGBa> &I_color1, const vpImage<vpRGBa> &I_color2,
  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(I_color1, initFile1);
 
    ++it;
 
    tracker = it->second;
    tracker->initFromPose(I_color2, initFile2);
 
    it = m_mapOfTrackers.find(m_referenceCameraName);
    if (it != m_mapOfTrackers.end()) {
      tracker = it->second;
 
      // Set the reference cMo
      tracker->getPose(m_cMo);
 
      // Set the reference camera parameters
      tracker->getCameraParameters(m_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(m_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 * m_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 std::map<std::string, const vpImage<vpRGBa> *> &mapOfColorImages,
  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<vpRGBa> *>::const_iterator it_img = mapOfColorImages.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 != mapOfColorImages.end() && it_initPose != mapOfInitPoses.end()) {
    TrackerWrapper *tracker = it_tracker->second;
    tracker->initFromPose(*it_img->second, it_initPose->second);
    tracker->getPose(m_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 = mapOfColorImages.find(it_tracker->first);
    it_initPose = mapOfInitPoses.find(it_tracker->first);
 
    if (it_img != mapOfColorImages.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 = mapOfColorImages.find(*it);
    std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
      m_mapOfCameraTransformationMatrix.find(*it);
 
    if (it_img != mapOfColorImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
      vpHomogeneousMatrix cCurrentMo = it_camTrans->second * m_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);
 
    m_cMo = c1Mo;
  }
  else {
    throw vpException(vpTrackingException::initializationError,
      "This method requires 2 cameras but there are %d cameras!", m_mapOfTrackers.size());
  }
}

void vpMbGenericTracker::initFromPose(const vpImage<vpRGBa> &I_color1, const vpImage<vpRGBa> &I_color2,
  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(I_color1, c1Mo);
 
    ++it;
 
    it->second->initFromPose(I_color2, c2Mo);
 
    m_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(m_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 * m_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::initFromPose(const std::map<std::string, const vpImage<vpRGBa> *> &mapOfColorImages,
  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<vpRGBa> *>::const_iterator it_img = mapOfColorImages.find(m_referenceCameraName);
  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(m_referenceCameraName);
 
  if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfColorImages.end() && it_camPose != mapOfCameraPoses.end()) {
    TrackerWrapper *tracker = it_tracker->second;
    tracker->initFromPose(*it_img->second, it_camPose->second);
    tracker->getPose(m_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 = mapOfColorImages.find(it_tracker->first);
    it_camPose = mapOfCameraPoses.find(it_tracker->first);
 
    if (it_img != mapOfColorImages.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 = mapOfColorImages.find(*it);
    std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
      m_mapOfCameraTransformationMatrix.find(*it);
 
    if (it_img != mapOfColorImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
      vpHomogeneousMatrix cCurrentMo = it_camTrans->second * m_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, bool verbose)
{
  const std::string extension = vpIoTools::getFileExtension(configFile);
  if (extension == ".xml") {
    loadConfigFileXML(configFile, verbose);
  }
#ifdef VISP_HAVE_NLOHMANN_JSON
  else if (extension == ".json") {
    loadConfigFileJSON(configFile, verbose);
  }
#endif
  else {
    throw vpException(vpException::ioError, "MBT config parsing: File format " + extension + "for file " + configFile + " is not supported.");
  }
}

void vpMbGenericTracker::loadConfigFileXML(const std::string &configFile, bool verbose)
{
  for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
    it != m_mapOfTrackers.end(); ++it) {
    TrackerWrapper *tracker = it->second;
    tracker->loadConfigFile(configFile, verbose);
  }
 
  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(m_cam);
  this->angleAppears = m_mapOfTrackers[m_referenceCameraName]->getAngleAppear();
  this->angleDisappears = m_mapOfTrackers[m_referenceCameraName]->getAngleDisappear();
  this->clippingFlag = m_mapOfTrackers[m_referenceCameraName]->getClipping();
}
 
#ifdef VISP_HAVE_NLOHMANN_JSON
void vpMbGenericTracker::loadConfigFileJSON(const std::string &settingsFile, bool verbose)
{
  //Read file
  std::ifstream jsonFile(settingsFile);
  if (!jsonFile.good()) {
    throw vpException(vpException::ioError, "Could not read from settings file " + settingsFile + " to initialise the vpMbGenericTracker");
  }
  json settings;
  try {
    settings = json::parse(jsonFile);
  }
  catch (json::parse_error &e) {
    std::stringstream msg;
    msg << "Could not parse JSON file : \n";
 
    msg << e.what() << std::endl;
    msg << "Byte position of error: " << e.byte;
    throw vpException(vpException::ioError, msg.str());
  }
  jsonFile.close();
 
  if (!settings.contains("version")) {
    throw vpException(vpException::notInitialized, "JSON configuration does not contain versioning information");
  }
  else if (settings["version"].get<std::string>() != MBT_JSON_SETTINGS_VERSION) {
    throw vpException(vpException::badValue, "Trying to load an old configuration file");
  }
 
  //Load Basic settings
  settings.at("referenceCameraName").get_to(m_referenceCameraName);
  json trackersJson;
  trackersJson = settings.at("trackers");
 
  //Find camera that are already present in the tracker but not in the config file: they will be removed
  std::vector<std::string> unusedCameraNames = getCameraNames();
  std::map<std::string, std::string> map_models;
 
  bool refCameraFound = false;
  //Foreach camera
  for (const auto &it : trackersJson.items()) {
    const std::string cameraName = it.key();
    const json trackerJson = it.value();
    refCameraFound = refCameraFound || cameraName == m_referenceCameraName;
 
    //Load transformation between current camera and reference camera, if it exists
    if (trackerJson.contains("camTref")) {
      m_mapOfCameraTransformationMatrix[cameraName] = trackerJson["camTref"].get<vpHomogeneousMatrix>();
    }
    else if (cameraName != m_referenceCameraName) { // No transformation to reference and its not the reference itself
      throw vpException(vpException::notInitialized, "Camera " + cameraName + " has no transformation to the reference camera");
    }
    if (trackerJson.contains("model")) {
      std::string model_filename = trackerJson["model"].get<std::string>();
      map_models[cameraName] = model_filename;
      std::cout << "Set the model to '" << model_filename << "' for camera '" << cameraName << "'" << std::endl;
    }
    if (verbose) {
      std::cout << "Loading tracker " << cameraName << std::endl << " with settings: " << std::endl << trackerJson.dump(2);
    }
    if (m_mapOfTrackers.count(cameraName)) {
      if (verbose) {
        std::cout << "Updating an already existing tracker with JSON configuration." << std::endl;
      }
      from_json(trackerJson, *(m_mapOfTrackers[cameraName]));
    }
    else {
      if (verbose) {
        std::cout << "Creating a new tracker from JSON configuration." << std::endl;
      }
      TrackerWrapper *tw = new TrackerWrapper(); //vpMBTracker is responsible for deleting trackers
      *tw = trackerJson;
      m_mapOfTrackers[cameraName] = tw;
    }
    const auto unusedCamIt = std::remove(unusedCameraNames.begin(), unusedCameraNames.end(), cameraName); // Mark this camera name as used
    unusedCameraNames.erase(unusedCamIt, unusedCameraNames.end());
  }
  if (!refCameraFound) {
    throw vpException(vpException::badValue, "Reference camera not found in trackers");
  }
 
  // All cameras that were defined in the tracker but not in the config file are removed
  for (const std::string &oldCameraName : unusedCameraNames) {
    m_mapOfCameraTransformationMatrix.erase(oldCameraName);
    TrackerWrapper *tw = m_mapOfTrackers[oldCameraName];
    m_mapOfTrackers.erase(oldCameraName);
    delete tw;
  }
 
  const TrackerWrapper *refTracker = m_mapOfTrackers[m_referenceCameraName];
  refTracker->getCameraParameters(m_cam);
  this->angleAppears = refTracker->getAngleAppear();
  this->angleDisappears = refTracker->getAngleDisappear();
  this->clippingFlag = refTracker->getClipping();
  this->distNearClip = refTracker->getNearClippingDistance();
  this->distFarClip = refTracker->getFarClippingDistance();
 
  // These settings can be set in each tracker or globally. Global value overrides local ones.
  if (settings.contains("display")) {
    const json displayJson = settings["display"];
    setDisplayFeatures(displayJson.value("features", displayFeatures));
    setProjectionErrorDisplay(displayJson.value("projectionError", m_projectionErrorDisplay));
  }
  if (settings.contains("visibilityTest")) {
    const json visJson = settings["visibilityTest"];
    setOgreVisibilityTest(visJson.value("ogre", useOgre));
    setScanLineVisibilityTest(visJson.value("scanline", useScanLine));
  }
 
  // VVS global settings
  if (settings.contains("vvs")) {
    const json vvsJson = settings["vvs"];
    setLambda(vvsJson.value("lambda", this->m_lambda));
    setMaxIter(vvsJson.value("maxIter", this->m_maxIter));
    setInitialMu(vvsJson.value("initialMu", this->m_initialMu));
  }
 
  //If a 3D model is defined, load it
  if (settings.contains("model") && (map_models.size() == 0)) {
    loadModel(settings.at("model").get<std::string>(), verbose);
  }
  else if (map_models.size() != 0) {
    if (settings.contains("model")) {
      std::stringstream ss;
      ss << "Both a model common to all the cameras and at least one model for a dedicated camera has been set." << std::endl;
      ss << "Please either set a model for each camera or a single model common to each camera";
      throw(vpException(vpException::ioError, ss.str()));
    }
    else if (map_models.size() == trackersJson.size()) {
      loadModel(map_models, verbose);
    }
    else {
      std::stringstream ss;
      ss << "The model has not been defined for each camera individually" << std::endl;
      ss << "Please either set a model for each camera or a single model common to each camera";
      throw(vpException(vpException::ioError, ss.str()));
    }
  }
}

void vpMbGenericTracker::saveConfigFile(const std::string &settingsFile) const
{
  json j;
  j["referenceCameraName"] = m_referenceCameraName;
  j["version"] = MBT_JSON_SETTINGS_VERSION;
  // j["thresholdOutlier"] = m_thresholdOutlier;
  json trackers;
  for (const auto &kv : m_mapOfTrackers) {
    trackers[kv.first] = *(kv.second);
    const auto itTransformation = m_mapOfCameraTransformationMatrix.find(kv.first);
    if (itTransformation != m_mapOfCameraTransformationMatrix.end()) {
      trackers[kv.first]["camTref"] = itTransformation->second;
    }
  }
  j["trackers"] = trackers;
  j["vvs"] = json {
    {"lambda", m_lambda},
    {"maxIter", m_maxIter},
    {"initialMu", m_initialMu}
  };
 
  std::ofstream f(settingsFile);
  if (f.good()) {
    const unsigned indentLevel = 4;
    f << j.dump(indentLevel);
    f.close();
  }
  else {
    throw vpException(vpException::ioError, "Could not save tracker configuration to JSON file: " + settingsFile);
  }
}
 
#endif

void vpMbGenericTracker::loadConfigFile(const std::string &configFile1, const std::string &configFile2, bool verbose)
{
  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, verbose);
 
  ++it_tracker;
  tracker = it_tracker->second;
  tracker->loadConfigFile(configFile2, verbose);
 
  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(m_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, bool verbose)
{
  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, verbose);
    }
    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(m_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, 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, 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, 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());
    }
  }
}
 
#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS) && defined(VISP_HAVE_PCL_COMMON)
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::preTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
  std::map<std::string, const vpMatrix *> &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, 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(m_cMo);
  }
  else {
    throw vpException(vpTrackingException::fatalError, "Cannot reInitModel() the reference camera!");
  }
 
  modelInitialised = true;
}

void vpMbGenericTracker::reInitModel(const vpImage<vpRGBa> &I_color, const std::string &cad_name,
  const vpHomogeneousMatrix &cMo, 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_color, cad_name, cMo, verbose, T);
 
    // Set reference pose
    tracker->getPose(m_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, 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(m_cMo);
    }
  }
  else {
    throw vpException(vpTrackingException::fatalError, "This method requires exactly two cameras!");
  }
 
  modelInitialised = true;
}

void vpMbGenericTracker::reInitModel(const vpImage<vpRGBa> &I_color1, const vpImage<vpRGBa> &I_color2,
  const std::string &cad_name1, const std::string &cad_name2,
  const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, 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(I_color1, cad_name1, c1Mo, verbose, T1);
 
    ++it_tracker;
 
    it_tracker->second->reInitModel(I_color2, 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(m_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, 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(m_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 * m_cMo;
      m_mapOfTrackers[*it]->reInitModel(*it_img->second, it_model->second, cCurrentMo, verbose);
    }
  }
 
  modelInitialised = true;
}

void vpMbGenericTracker::reInitModel(const std::map<std::string, const vpImage<vpRGBa> *> &mapOfColorImages,
  const std::map<std::string, std::string> &mapOfModelFiles,
  const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses, 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<vpRGBa> *>::const_iterator it_img = mapOfColorImages.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 != mapOfColorImages.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(m_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 = mapOfColorImages.find(it_tracker->first);
      it_model = mapOfModelFiles.find(it_tracker->first);
      it_camPose = mapOfCameraPoses.find(it_tracker->first);
 
      if (it_img != mapOfColorImages.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 = mapOfColorImages.find(*it);
    it_model = mapOfModelFiles.find(*it);
    std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
      m_mapOfCameraTransformationMatrix.find(*it);
 
    if (it_img != mapOfColorImages.end() && it_model != mapOfModelFiles.end() &&
      it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
      vpHomogeneousMatrix cCurrentMo = it_camTrans->second * m_cMo;
      m_mapOfTrackers[*it]->reInitModel(*it_img->second, it_model->second, cCurrentMo, verbose);
    }
  }
 
  modelInitialised = true;
}

void vpMbGenericTracker::resetTracker()
{
  m_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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
  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(m_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) {
        m_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(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(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(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(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(unsigned int stepX, 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(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(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(unsigned int stepX, 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(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(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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
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(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(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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
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(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(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(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!");
  }
 
  m_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<vpRGBa> &I_color, 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!");
  }
 
  m_cMo = cdMo;
 
  std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
  if (it != m_mapOfTrackers.end()) {
    TrackerWrapper *tracker = it->second;
    vpImageConvert::convert(I_color, m_I);
    tracker->setPose(m_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(m_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 vpImage<vpRGBa> &I_color1, const vpImage<vpRGBa> &I_color2,
  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(I_color1, c1Mo);
 
    ++it;
 
    it->second->setPose(I_color2, c2Mo);
 
    it = m_mapOfTrackers.find(m_referenceCameraName);
    if (it != m_mapOfTrackers.end()) {
      // Set reference pose
      it->second->getPose(m_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(m_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 * m_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::setPose(const std::map<std::string, const vpImage<vpRGBa> *> &mapOfColorImages,
  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<vpRGBa> *>::const_iterator it_img = mapOfColorImages.find(m_referenceCameraName);
  std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(m_referenceCameraName);
 
  if (it_tracker != m_mapOfTrackers.end() && it_img != mapOfColorImages.end() && it_camPose != mapOfCameraPoses.end()) {
    TrackerWrapper *tracker = it_tracker->second;
    tracker->setPose(*it_img->second, it_camPose->second);
    tracker->getPose(m_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 = mapOfColorImages.find(it_tracker->first);
      it_camPose = mapOfCameraPoses.find(it_tracker->first);
 
      if (it_img != mapOfColorImages.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 = mapOfColorImages.find(*it);
    std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
      m_mapOfCameraTransformationMatrix.find(*it);
 
    if (it_img != mapOfColorImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
      vpHomogeneousMatrix cCurrentMo = it_camTrans->second * m_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(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(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(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(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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
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 (const vpException &e) {
      std::cerr << "[" << it->first << "] " << e.what() << std::endl;
    }
  }
 
  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<vpRGBa> &I_color)
{
  std::map<std::string, const vpImage<vpRGBa> *> mapOfColorImages;
  mapOfColorImages[m_referenceCameraName] = &I_color;
 
  std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
  std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
 
  track(mapOfColorImages, 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(const vpImage<vpRGBa> &I_color1, const vpImage<vpRGBa> &_colorI2)
{
  if (m_mapOfTrackers.size() == 2) {
    std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
    std::map<std::string, const vpImage<vpRGBa> *> mapOfImages;
    mapOfImages[it->first] = &I_color1;
    ++it;
 
    mapOfImages[it->first] = &_colorI2;
 
    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);
}

void vpMbGenericTracker::track(std::map<std::string, const vpImage<vpRGBa> *> &mapOfColorImages)
{
  std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
  std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
 
  track(mapOfColorImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
}
 
#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS) && defined(VISP_HAVE_PCL_COMMON)
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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
                                   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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
                                  | KLT_TRACKER
#endif
                                  ) &&
      mapOfImages[it->first] == nullptr) {
      throw vpException(vpException::fatalError, "Image pointer is nullptr!");
    }
 
    if (tracker->m_trackerType & (DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER) &&
      !mapOfPointClouds[it->first]) { // mapOfPointClouds[it->first] == nullptr
      throw vpException(vpException::fatalError, "Pointcloud smart pointer is nullptr!");
    }
  }
 
  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;
 
    if (tracker->m_trackerType & EDGE_TRACKER && displayFeatures) {
      tracker->m_featuresToBeDisplayedEdge = tracker->getFeaturesForDisplayEdge();
    }
 
    tracker->postTracking(mapOfImages[it->first], mapOfPointClouds[it->first]);
 
    if (displayFeatures) {
#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
      if (tracker->m_trackerType & KLT_TRACKER) {
        tracker->m_featuresToBeDisplayedKlt = tracker->getFeaturesForDisplayKlt();
      }
#endif
 
      if (tracker->m_trackerType & DEPTH_NORMAL_TRACKER) {
        tracker->m_featuresToBeDisplayedDepthNormal = tracker->getFeaturesForDisplayDepthNormal();
      }
    }
  }
 
  computeProjectionError();
}

void vpMbGenericTracker::track(std::map<std::string, const vpImage<vpRGBa> *> &mapOfColorImages,
  std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> &mapOfPointClouds)
{
  std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
  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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
                                   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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
                                  | KLT_TRACKER
#endif
                                  ) &&
      mapOfImages[it->first] == nullptr) {
      throw vpException(vpException::fatalError, "Image pointer is nullptr!");
    }
    else if (tracker->m_trackerType & (EDGE_TRACKER
#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
                                       | KLT_TRACKER
#endif
                                       ) &&
      mapOfImages[it->first] != nullptr) {
      vpImageConvert::convert(*mapOfColorImages[it->first], tracker->m_I);
      mapOfImages[it->first] = &tracker->m_I; // update grayscale image buffer
    }
 
    if (tracker->m_trackerType & (DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER) &&
      !mapOfPointClouds[it->first]) { // mapOfPointClouds[it->first] == nullptr
      throw vpException(vpException::fatalError, "Pointcloud smart pointer is nullptr!");
    }
  }
 
  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;
 
    if (tracker->m_trackerType & EDGE_TRACKER && displayFeatures) {
      tracker->m_featuresToBeDisplayedEdge = tracker->getFeaturesForDisplayEdge();
    }
 
    tracker->postTracking(mapOfImages[it->first], mapOfPointClouds[it->first]);
 
    if (displayFeatures) {
#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
      if (tracker->m_trackerType & KLT_TRACKER) {
        tracker->m_featuresToBeDisplayedKlt = tracker->getFeaturesForDisplayKlt();
      }
#endif
 
      if (tracker->m_trackerType & DEPTH_NORMAL_TRACKER) {
        tracker->m_featuresToBeDisplayedDepthNormal = tracker->getFeaturesForDisplayDepthNormal();
      }
    }
  }
 
  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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
                                   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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
                                  | KLT_TRACKER
#endif
                                  ) &&
      mapOfImages[it->first] == nullptr) {
      throw vpException(vpException::fatalError, "Image pointer is nullptr!");
    }
 
    if (tracker->m_trackerType & (DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER) &&
      (mapOfPointClouds[it->first] == nullptr)) {
      throw vpException(vpException::fatalError, "Pointcloud is nullptr!");
    }
  }
 
  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;
 
    if (tracker->m_trackerType & EDGE_TRACKER && displayFeatures) {
      tracker->m_featuresToBeDisplayedEdge = tracker->getFeaturesForDisplayEdge();
    }
 
    tracker->postTracking(mapOfImages[it->first], mapOfPointCloudWidths[it->first], mapOfPointCloudHeights[it->first]);
 
    if (displayFeatures) {
#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
      if (tracker->m_trackerType & KLT_TRACKER) {
        tracker->m_featuresToBeDisplayedKlt = tracker->getFeaturesForDisplayKlt();
      }
#endif
 
      if (tracker->m_trackerType & DEPTH_NORMAL_TRACKER) {
        tracker->m_featuresToBeDisplayedDepthNormal = tracker->getFeaturesForDisplayDepthNormal();
      }
    }
  }
 
  computeProjectionError();
}

void vpMbGenericTracker::track(std::map<std::string, const vpImage<vpRGBa> *> &mapOfColorImages,
  std::map<std::string, const std::vector<vpColVector> *> &mapOfPointClouds,
  std::map<std::string, unsigned int> &mapOfPointCloudWidths,
  std::map<std::string, unsigned int> &mapOfPointCloudHeights)
{
  std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
  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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
                                   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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
                                  | KLT_TRACKER
#endif
                                  ) &&
      mapOfColorImages[it->first] == nullptr) {
      throw vpException(vpException::fatalError, "Image pointer is nullptr!");
    }
    else if (tracker->m_trackerType & (EDGE_TRACKER
#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
                                       | KLT_TRACKER
#endif
                                       ) &&
      mapOfColorImages[it->first] != nullptr) {
      vpImageConvert::convert(*mapOfColorImages[it->first], tracker->m_I);
      mapOfImages[it->first] = &tracker->m_I; // update grayscale image buffer
    }
 
    if (tracker->m_trackerType & (DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER) &&
      (mapOfPointClouds[it->first] == nullptr)) {
      throw vpException(vpException::fatalError, "Pointcloud is nullptr!");
    }
  }
 
  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;
 
    if (tracker->m_trackerType & EDGE_TRACKER && displayFeatures) {
      tracker->m_featuresToBeDisplayedEdge = tracker->getFeaturesForDisplayEdge();
    }
 
    tracker->postTracking(mapOfImages[it->first], mapOfPointCloudWidths[it->first], mapOfPointCloudHeights[it->first]);
 
    if (displayFeatures) {
#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
      if (tracker->m_trackerType & KLT_TRACKER) {
        tracker->m_featuresToBeDisplayedKlt = tracker->getFeaturesForDisplayKlt();
      }
#endif
 
      if (tracker->m_trackerType & DEPTH_NORMAL_TRACKER) {
        tracker->m_featuresToBeDisplayedDepthNormal = tracker->getFeaturesForDisplayDepthNormal();
      }
    }
  }
 
  computeProjectionError();
}
 
void vpMbGenericTracker::track(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
  std::map<std::string, const vpMatrix *> &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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
                                   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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
                                  | KLT_TRACKER
#endif
                                  ) &&
      mapOfImages[it->first] == nullptr) {
      throw vpException(vpException::fatalError, "Image pointer is nullptr!");
    }
 
    if (tracker->m_trackerType & (DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER) &&
      (mapOfPointClouds[it->first] == nullptr)) {
      throw vpException(vpException::fatalError, "Pointcloud is nullptr!");
    }
  }
 
  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;
 
    if (tracker->m_trackerType & EDGE_TRACKER && displayFeatures) {
      tracker->m_featuresToBeDisplayedEdge = tracker->getFeaturesForDisplayEdge();
    }
 
    tracker->postTracking(mapOfImages[it->first], mapOfPointCloudWidths[it->first], mapOfPointCloudHeights[it->first]);
 
    if (displayFeatures) {
#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
      if (tracker->m_trackerType & KLT_TRACKER) {
        tracker->m_featuresToBeDisplayedKlt = tracker->getFeaturesForDisplayKlt();
      }
#endif
 
      if (tracker->m_trackerType & DEPTH_NORMAL_TRACKER) {
        tracker->m_featuresToBeDisplayedDepthNormal = tracker->getFeaturesForDisplayDepthNormal();
      }
    }
  }
 
  computeProjectionError();
}

void vpMbGenericTracker::track(std::map<std::string, const vpImage<vpRGBa> *> &mapOfColorImages,
  std::map<std::string, const vpMatrix *> &mapOfPointClouds,
  std::map<std::string, unsigned int> &mapOfPointCloudWidths,
  std::map<std::string, unsigned int> &mapOfPointCloudHeights)
{
  std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
  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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
                                   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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
                                  | KLT_TRACKER
#endif
                                  ) &&
      mapOfColorImages[it->first] == nullptr) {
      throw vpException(vpException::fatalError, "Image pointer is nullptr!");
    }
    else if (tracker->m_trackerType & (EDGE_TRACKER
#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
                                       | KLT_TRACKER
#endif
                                       ) &&
      mapOfColorImages[it->first] != nullptr) {
      vpImageConvert::convert(*mapOfColorImages[it->first], tracker->m_I);
      mapOfImages[it->first] = &tracker->m_I; // update grayscale image buffer
    }
 
    if (tracker->m_trackerType & (DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER) &&
      (mapOfPointClouds[it->first] == nullptr)) {
      throw vpException(vpException::fatalError, "Pointcloud is nullptr!");
    }
  }
 
  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;
 
    if (tracker->m_trackerType & EDGE_TRACKER && displayFeatures) {
      tracker->m_featuresToBeDisplayedEdge = tracker->getFeaturesForDisplayEdge();
    }
 
    tracker->postTracking(mapOfImages[it->first], mapOfPointCloudWidths[it->first], mapOfPointCloudHeights[it->first]);
 
    if (displayFeatures) {
#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
      if (tracker->m_trackerType & KLT_TRACKER) {
        tracker->m_featuresToBeDisplayedKlt = tracker->getFeaturesForDisplayKlt();
      }
#endif
 
      if (tracker->m_trackerType & DEPTH_NORMAL_TRACKER) {
        tracker->m_featuresToBeDisplayedDepthNormal = tracker->getFeaturesForDisplayDepthNormal();
      }
    }
  }
 
  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(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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
                        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
}
 
// 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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
  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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
  vpHomogeneousMatrix ctTc0_Prev; // Only for KLT
#endif
  bool isoJoIdentity = m_isoJoIdentity; // Backup since it can be modified if L is not full rank
  if (isoJoIdentity)
    oJo.eye();
 
  // 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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
  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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
    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(m_cMo);
          LVJ_true = (m_L * cVo * oJo);
        }
      }
 
      vpVelocityTwistMatrix cVo;
      if (iter == 0) {
        // 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 considering
        // circles (rank 5) and cylinders (rank 4)
        if (isoJoIdentity) {
          cVo.buildFrom(m_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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
      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 = m_cMo;
#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
      if (m_trackerType & KLT_TRACKER) {
        ctTc0_Prev = ctTc0;
      }
#endif
 
      m_cMo = vpExponentialMap::direct(v).inverse() * m_cMo;
 
#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
      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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
  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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
  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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
  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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
  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 &cam, const vpColor &col,
  unsigned int thickness, bool displayFullModel)
{
  if (displayFeatures) {
    std::vector<std::vector<double> > features = getFeaturesForDisplay();
    for (size_t i = 0; i < features.size(); i++) {
      if (vpMath::equal(features[i][0], 0)) {
        vpImagePoint ip(features[i][1], features[i][2]);
        int state = static_cast<int>(features[i][3]);
 
        switch (state) {
        case vpMeSite::NO_SUPPRESSION:
          vpDisplay::displayCross(I, ip, 3, vpColor::green, 1);
          break;
 
        case vpMeSite::CONTRAST:
          vpDisplay::displayCross(I, ip, 3, vpColor::blue, 1);
          break;
 
        case vpMeSite::THRESHOLD:
          vpDisplay::displayCross(I, ip, 3, vpColor::purple, 1);
          break;
 
        case vpMeSite::M_ESTIMATOR:
          vpDisplay::displayCross(I, ip, 3, vpColor::red, 1);
          break;
 
        case vpMeSite::TOO_NEAR:
          vpDisplay::displayCross(I, ip, 3, vpColor::cyan, 1);
          break;
 
        default:
          vpDisplay::displayCross(I, ip, 3, vpColor::yellow, 1);
        }
      }
      else if (vpMath::equal(features[i][0], 1)) {
        vpImagePoint ip1(features[i][1], features[i][2]);
        vpDisplay::displayCross(I, ip1, 10, vpColor::red);
 
        vpImagePoint ip2(features[i][3], features[i][4]);
        double id = features[i][5];
        std::stringstream ss;
        ss << id;
        vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
      }
      else if (vpMath::equal(features[i][0], 2)) {
        vpImagePoint im_centroid(features[i][1], features[i][2]);
        vpImagePoint im_extremity(features[i][3], features[i][4]);
        bool desired = vpMath::equal(features[i][0], 2);
        vpDisplay::displayArrow(I, im_centroid, im_extremity, desired ? vpColor::blue : vpColor::red, 4, 2, thickness);
      }
    }
  }
 
  std::vector<std::vector<double> > models =
    getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
  for (size_t i = 0; i < models.size(); i++) {
    if (vpMath::equal(models[i][0], 0)) {
      vpImagePoint ip1(models[i][1], models[i][2]);
      vpImagePoint ip2(models[i][3], models[i][4]);
      vpDisplay::displayLine(I, ip1, ip2, col, thickness);
    }
    else if (vpMath::equal(models[i][0], 1)) {
      vpImagePoint center(models[i][1], models[i][2]);
      double n20 = models[i][3];
      double n11 = models[i][4];
      double n02 = models[i][5];
      vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
    }
  }
 
#ifdef VISP_HAVE_OGRE
  if ((m_trackerType & EDGE_TRACKER)
#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
    || (m_trackerType & KLT_TRACKER)
#endif
    ) {
    if (useOgre)
      faces.displayOgre(cMo);
  }
#endif
}
 
void vpMbGenericTracker::TrackerWrapper::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
  const vpCameraParameters &cam, const vpColor &col,
  unsigned int thickness, bool displayFullModel)
{
  if (displayFeatures) {
    std::vector<std::vector<double> > features = getFeaturesForDisplay();
    for (size_t i = 0; i < features.size(); i++) {
      if (vpMath::equal(features[i][0], 0)) {
        vpImagePoint ip(features[i][1], features[i][2]);
        int state = static_cast<int>(features[i][3]);
 
        switch (state) {
        case vpMeSite::NO_SUPPRESSION:
          vpDisplay::displayCross(I, ip, 3, vpColor::green, 1);
          break;
 
        case vpMeSite::CONTRAST:
          vpDisplay::displayCross(I, ip, 3, vpColor::blue, 1);
          break;
 
        case vpMeSite::THRESHOLD:
          vpDisplay::displayCross(I, ip, 3, vpColor::purple, 1);
          break;
 
        case vpMeSite::M_ESTIMATOR:
          vpDisplay::displayCross(I, ip, 3, vpColor::red, 1);
          break;
 
        case vpMeSite::TOO_NEAR:
          vpDisplay::displayCross(I, ip, 3, vpColor::cyan, 1);
          break;
 
        default:
          vpDisplay::displayCross(I, ip, 3, vpColor::yellow, 1);
        }
      }
      else if (vpMath::equal(features[i][0], 1)) {
        vpImagePoint ip1(features[i][1], features[i][2]);
        vpDisplay::displayCross(I, ip1, 10, vpColor::red);
 
        vpImagePoint ip2(features[i][3], features[i][4]);
        double id = features[i][5];
        std::stringstream ss;
        ss << id;
        vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
      }
      else if (vpMath::equal(features[i][0], 2)) {
        vpImagePoint im_centroid(features[i][1], features[i][2]);
        vpImagePoint im_extremity(features[i][3], features[i][4]);
        bool desired = vpMath::equal(features[i][0], 2);
        vpDisplay::displayArrow(I, im_centroid, im_extremity, desired ? vpColor::blue : vpColor::red, 4, 2, thickness);
      }
    }
  }
 
  std::vector<std::vector<double> > models =
    getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
  for (size_t i = 0; i < models.size(); i++) {
    if (vpMath::equal(models[i][0], 0)) {
      vpImagePoint ip1(models[i][1], models[i][2]);
      vpImagePoint ip2(models[i][3], models[i][4]);
      vpDisplay::displayLine(I, ip1, ip2, col, thickness);
    }
    else if (vpMath::equal(models[i][0], 1)) {
      vpImagePoint center(models[i][1], models[i][2]);
      double n20 = models[i][3];
      double n11 = models[i][4];
      double n02 = models[i][5];
      vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
    }
  }
 
#ifdef VISP_HAVE_OGRE
  if ((m_trackerType & EDGE_TRACKER)
#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
    || (m_trackerType & KLT_TRACKER)
#endif
    ) {
    if (useOgre)
      faces.displayOgre(cMo);
  }
#endif
}
 
std::vector<std::vector<double> > vpMbGenericTracker::TrackerWrapper::getFeaturesForDisplay()
{
  std::vector<std::vector<double> > features;
 
  if (m_trackerType & EDGE_TRACKER) {
    // m_featuresToBeDisplayedEdge updated after computeVVS()
    features.insert(features.end(), m_featuresToBeDisplayedEdge.begin(), m_featuresToBeDisplayedEdge.end());
  }
 
#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
  if (m_trackerType & KLT_TRACKER) {
    // m_featuresToBeDisplayedKlt updated after postTracking()
    features.insert(features.end(), m_featuresToBeDisplayedKlt.begin(), m_featuresToBeDisplayedKlt.end());
  }
#endif
 
  if (m_trackerType & DEPTH_NORMAL_TRACKER) {
    // m_featuresToBeDisplayedDepthNormal updated after postTracking()
    features.insert(features.end(), m_featuresToBeDisplayedDepthNormal.begin(),
      m_featuresToBeDisplayedDepthNormal.end());
  }
 
  return features;
}
 
std::vector<std::vector<double> > vpMbGenericTracker::TrackerWrapper::getModelForDisplay(unsigned int width,
  unsigned int height,
  const vpHomogeneousMatrix &cMo,
  const vpCameraParameters &cam,
  bool displayFullModel)
{
  std::vector<std::vector<double> > models;
 
  // Do not add multiple times the same models
  if (m_trackerType == EDGE_TRACKER) {
    models = vpMbEdgeTracker::getModelForDisplay(width, height, cMo, cam, displayFullModel);
  }
#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
  else if (m_trackerType == KLT_TRACKER) {
    models = vpMbKltTracker::getModelForDisplay(width, height, cMo, cam, displayFullModel);
  }
#endif
  else if (m_trackerType == DEPTH_NORMAL_TRACKER) {
    models = vpMbDepthNormalTracker::getModelForDisplay(width, height, cMo, cam, displayFullModel);
  }
  else if (m_trackerType == DEPTH_DENSE_TRACKER) {
    models = vpMbDepthDenseTracker::getModelForDisplay(width, height, cMo, cam, displayFullModel);
  }
  else {
    // Edge and KLT trackers use the same primitives
    if (m_trackerType & EDGE_TRACKER) {
      std::vector<std::vector<double> > edgeModels =
        vpMbEdgeTracker::getModelForDisplay(width, height, cMo, cam, displayFullModel);
      models.insert(models.end(), edgeModels.begin(), edgeModels.end());
    }
 
    // Depth dense and depth normal trackers use the same primitives
    if (m_trackerType & DEPTH_DENSE_TRACKER) {
      std::vector<std::vector<double> > depthDenseModels =
        vpMbDepthDenseTracker::getModelForDisplay(width, height, cMo, cam, displayFullModel);
      models.insert(models.end(), depthDenseModels.begin(), depthDenseModels.end());
    }
  }
 
  return models;
}
 
void vpMbGenericTracker::TrackerWrapper::init(const vpImage<unsigned char> &I)
{
  if (!modelInitialised) {
    throw vpException(vpException::fatalError, "model not initialized");
  }
 
  if (useScanLine || clippingFlag > 3)
    m_cam.computeFov(I.getWidth(), I.getHeight());
 
  bool reInitialisation = false;
  if (!useOgre) {
    faces.setVisible(I.getWidth(), I.getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
  }
  else {
#ifdef VISP_HAVE_OGRE
    if (!faces.isOgreInitialised()) {
      faces.setBackgroundSizeOgre(I.getHeight(), I.getWidth());
 
      faces.setOgreShowConfigDialog(ogreShowConfigDialog);
      faces.initOgre(m_cam);
      // Turn off Ogre config dialog display for the next call to this
      // function since settings are saved in the ogre.cfg file and used
      // during the next call
      ogreShowConfigDialog = false;
    }
 
    faces.setVisibleOgre(I.getWidth(), I.getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
#else
    faces.setVisible(I.getWidth(), I.getHeight(), m_cam, m_cMo, angleAppears, angleDisappears, reInitialisation);
#endif
  }
 
  if (useScanLine) {
    faces.computeClippedPolygons(m_cMo, m_cam);
    faces.computeScanLineRender(m_cam, I.getWidth(), I.getHeight());
  }
 
#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
  if (m_trackerType & KLT_TRACKER)
    vpMbKltTracker::reinit(I);
#endif
 
  if (m_trackerType & EDGE_TRACKER) {
    vpMbEdgeTracker::resetMovingEdge();
 
    bool a = false;
    vpMbEdgeTracker::visibleFace(I, m_cMo, a); // should be useless, but keep it for nbvisiblepolygone
 
    vpMbEdgeTracker::initMovingEdge(I, m_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,
  double radius, 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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
  if (m_trackerType & KLT_TRACKER)
    vpMbKltTracker::initCircle(p1, p2, p3, radius, idFace, name);
#endif
}
 
void vpMbGenericTracker::TrackerWrapper::initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, 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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
  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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
  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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
  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, bool verbose)
{
#if defined(VISP_HAVE_PUGIXML)
  // Load projection error config
  vpMbTracker::loadConfigFile(configFile, verbose);
 
  vpMbtXmlGenericParser xmlp((vpMbtXmlGenericParser::vpParserType)m_trackerType);
  xmlp.setVerbose(verbose);
  xmlp.setCameraParameters(m_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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
  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 {
    if (verbose) {
      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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
    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");
 
    if (verbose) {
      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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
  tracker.setMaxFeatures(static_cast<int>(xmlp.getKltMaxFeatures()));
  tracker.setWindowSize(static_cast<int>(xmlp.getKltWindowSize()));
  tracker.setQuality(xmlp.getKltQuality());
  tracker.setMinDistance(xmlp.getKltMinDistance());
  tracker.setHarrisFreeParameter(xmlp.getKltHarrisParam());
  tracker.setBlockSize(static_cast<int>(xmlp.getKltBlockSize()));
  tracker.setPyramidLevels(static_cast<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
  (void)configFile;
  (void)verbose;
  throw(vpException(vpException::ioError, "vpMbGenericTracker::TrackerWrapper::loadConfigFile() needs pugixml built-in 3rdparty"));
#endif
}
 
#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS) && defined(VISP_HAVE_PCL_COMMON)
void vpMbGenericTracker::TrackerWrapper::postTracking(const vpImage<unsigned char> *const ptr_I,
                                                      const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
{
#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
  // 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, m_cMo, newvisibleface);
 
    if (useScanLine) {
      faces.computeClippedPolygons(m_cMo, m_cam);
      faces.computeScanLineRender(m_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, m_cMo, false);
    // Reinit the moving edge for the lines which need it.
    vpMbEdgeTracker::reinitMovingEdge(*ptr_I, m_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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
  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 defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
  // 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, m_cMo, newvisibleface);
 
    if (useScanLine) {
      faces.computeClippedPolygons(m_cMo, m_cam);
      faces.computeScanLineRender(m_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, m_cMo, false);
    // Reinit the moving edge for the lines which need it.
    vpMbEdgeTracker::reinitMovingEdge(*ptr_I, m_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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
  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::preTracking(const vpImage<unsigned char> *const ptr_I,
  const vpMatrix *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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
  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> *const I,
  const vpImage<vpRGBa> *const I_color, const std::string &cad_name,
  const vpHomogeneousMatrix &cMo, bool verbose,
  const vpHomogeneousMatrix &T)
{
  m_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 != nullptr)
          delete l;
        l = nullptr;
      }
 
      for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
        ++it) {
        cy = *it;
        if (cy != nullptr)
          delete cy;
        cy = nullptr;
      }
 
      for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
        ci = *it;
        if (ci != nullptr)
          delete ci;
        ci = nullptr;
      }
 
      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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
  // delete the Klt Polygon features
  for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
    vpMbtDistanceKltPoints *kltpoly = *it;
    if (kltpoly != nullptr) {
      delete kltpoly;
    }
    kltpoly = nullptr;
  }
  kltPolygons.clear();
 
  for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
    ++it) {
    vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
    if (kltPolyCylinder != nullptr) {
      delete kltPolyCylinder;
    }
    kltPolyCylinder = nullptr;
  }
  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 != nullptr) {
      delete ci;
    }
    ci = nullptr;
  }
  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] = nullptr;
  }
  m_depthNormalFaces.clear();
 
  // Depth dense
  for (size_t i = 0; i < m_depthDenseFaces.size(); i++) {
    delete m_depthDenseFaces[i];
    m_depthDenseFaces[i] = nullptr;
  }
  m_depthDenseFaces.clear();
 
  faces.reset();
 
  loadModel(cad_name, verbose, T);
  if (I) {
    initFromPose(*I, cMo);
  }
  else {
    initFromPose(*I_color, cMo);
  }
}
 
void vpMbGenericTracker::TrackerWrapper::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
  const vpHomogeneousMatrix &cMo, bool verbose,
  const vpHomogeneousMatrix &T)
{
  reInitModel(&I, nullptr, cad_name, cMo, verbose, T);
}
 
void vpMbGenericTracker::TrackerWrapper::reInitModel(const vpImage<vpRGBa> &I_color, const std::string &cad_name,
  const vpHomogeneousMatrix &cMo, bool verbose,
  const vpHomogeneousMatrix &T)
{
  reInitModel(nullptr, &I_color, cad_name, cMo, verbose, T);
}
 
void vpMbGenericTracker::TrackerWrapper::resetTracker()
{
  vpMbEdgeTracker::resetTracker();
#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
  vpMbKltTracker::resetTracker();
#endif
  vpMbDepthNormalTracker::resetTracker();
  vpMbDepthDenseTracker::resetTracker();
}
 
void vpMbGenericTracker::TrackerWrapper::setCameraParameters(const vpCameraParameters &cam)
{
  m_cam = cam;
 
  vpMbEdgeTracker::setCameraParameters(m_cam);
#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
  vpMbKltTracker::setCameraParameters(m_cam);
#endif
  vpMbDepthNormalTracker::setCameraParameters(m_cam);
  vpMbDepthDenseTracker::setCameraParameters(m_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> *const I,
  const vpImage<vpRGBa> *const I_color, const vpHomogeneousMatrix &cdMo)
{
  bool performKltSetPose = false;
  if (I_color) {
    vpImageConvert::convert(*I_color, m_I);
  }
 
#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
  if (m_trackerType & KLT_TRACKER) {
    performKltSetPose = true;
 
    if (useScanLine || clippingFlag > 3) {
      m_cam.computeFov(I ? I->getWidth() : m_I.getWidth(), I ? I->getHeight() : m_I.getHeight());
    }
 
    vpMbKltTracker::setPose(I ? *I : m_I, cdMo);
  }
#endif
 
  if (!performKltSetPose) {
    m_cMo = cdMo;
    init(I ? *I : m_I);
    return;
  }
 
  if (m_trackerType & EDGE_TRACKER) {
    vpMbEdgeTracker::resetMovingEdge();
  }
 
  if (useScanLine) {
    faces.computeClippedPolygons(m_cMo, m_cam);
    faces.computeScanLineRender(m_cam, I ? I->getWidth() : m_I.getWidth(), I ? I->getHeight() : m_I.getHeight());
  }
 
#if 0
  if (m_trackerType & EDGE_TRACKER) {
    initPyramid(I, Ipyramid);
 
    unsigned int i = static_cast<unsigned int>(scales.size());
    do {
      i--;
      if (scales[i]) {
        downScale(i);
        vpMbEdgeTracker::initMovingEdge(*Ipyramid[i], cMo);
        upScale(i);
      }
    } while (i != 0);
 
    cleanPyramid(Ipyramid);
  }
#else
  if (m_trackerType & EDGE_TRACKER) {
    vpMbEdgeTracker::initMovingEdge(I ? *I : m_I, m_cMo);
  }
#endif
 
  // Depth normal
  vpMbDepthNormalTracker::computeVisibility(I ? I->getWidth() : m_I.getWidth(), I ? I->getHeight() : m_I.getHeight());
 
  // Depth dense
  vpMbDepthDenseTracker::computeVisibility(I ? I->getWidth() : m_I.getWidth(), I ? I->getHeight() : m_I.getHeight());
}
 
void vpMbGenericTracker::TrackerWrapper::setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cdMo)
{
  setPose(&I, nullptr, cdMo);
}
 
void vpMbGenericTracker::TrackerWrapper::setPose(const vpImage<vpRGBa> &I_color, const vpHomogeneousMatrix &cdMo)
{
  setPose(nullptr, &I_color, cdMo);
}
 
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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
  vpMbKltTracker::setScanLineVisibilityTest(v);
#endif
  vpMbDepthNormalTracker::setScanLineVisibilityTest(v);
  vpMbDepthDenseTracker::setScanLineVisibilityTest(v);
}
 
void vpMbGenericTracker::TrackerWrapper::setTrackerType(int type)
{
  if ((type & (EDGE_TRACKER |
#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
               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> &
#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS) && defined(VISP_HAVE_PCL_COMMON)
  I
#endif
)
{
  if ((m_trackerType & (EDGE_TRACKER
#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
                        | KLT_TRACKER
#endif
                        )) == 0) {
    std::cerr << "Bad tracker type: " << m_trackerType << std::endl;
    return;
  }
 
#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS) && defined(VISP_HAVE_PCL_COMMON)
  track(&I, nullptr);
#endif
}
 
void vpMbGenericTracker::TrackerWrapper::track(const vpImage<vpRGBa> &)
{
  // not exposed to the public API, only for debug
}
 
#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS) && defined(VISP_HAVE_PCL_COMMON)
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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
                        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) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
                       | KLT_TRACKER
#endif
                       ) &&
    ptr_I == nullptr) {
    throw vpException(vpException::fatalError, "Image pointer is nullptr!");
  }
 
  if (m_trackerType & (DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER) && !point_cloud) { // point_cloud == nullptr
    throw vpException(vpException::fatalError, "Pointcloud smart pointer is nullptr!");
  }
 
  // Back-up cMo in case of exception
  vpHomogeneousMatrix cMo_1 = m_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;
    m_cMo = cMo_1;
    throw; // rethrowing the original exception
  }
}
#endif
END_VISP_NAMESPACE