vpMbKltMultiTracker.cpp

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See http://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * If you have questions regarding the use of this file, please contact
 * Inria at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * Model-based klt tracker with multiple cameras.
 *
 * Authors:
 * Souriya Trinh
 *
 *****************************************************************************/

#include <visp3/core/vpConfig.h>

#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)

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

#include <visp3/core/vpTrackingException.h>
#include <visp3/core/vpVelocityTwistMatrix.h>
#include <visp3/mbt/vpMbKltMultiTracker.h>

vpMbKltMultiTracker::vpMbKltMultiTracker()
  : m_mapOfCameraTransformationMatrix(), m_mapOfKltTrackers(), m_referenceCameraName("Camera"), m_L_kltMulti(),
    m_error_kltMulti(), m_w_kltMulti(), m_weightedError_kltMulti()
{
  m_mapOfKltTrackers["Camera"] = new vpMbKltTracker();

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

vpMbKltMultiTracker::vpMbKltMultiTracker(const unsigned int nbCameras)
  : m_mapOfCameraTransformationMatrix(), m_mapOfKltTrackers(), m_referenceCameraName("Camera"), m_L_kltMulti(),
    m_error_kltMulti(), m_w_kltMulti(), m_weightedError_kltMulti()
{

  if (nbCameras == 0) {
    throw vpException(vpTrackingException::fatalError, "Cannot construct a vpMbkltMultiTracker with no camera !");
  } else if (nbCameras == 1) {
    m_mapOfKltTrackers["Camera"] = new vpMbKltTracker();

    // Add default camera transformation matrix
    m_mapOfCameraTransformationMatrix["Camera"] = vpHomogeneousMatrix();
  } else if (nbCameras == 2) {
    m_mapOfKltTrackers["Camera1"] = new vpMbKltTracker();
    m_mapOfKltTrackers["Camera2"] = new vpMbKltTracker();

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

    // Set by default the reference camera
    m_referenceCameraName = "Camera1";
  } else {
    for (unsigned int i = 1; i <= nbCameras; i++) {
      std::stringstream ss;
      ss << "Camera" << i;
      m_mapOfKltTrackers[ss.str()] = new vpMbKltTracker();

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

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

vpMbKltMultiTracker::vpMbKltMultiTracker(const std::vector<std::string> &cameraNames)
  : m_mapOfCameraTransformationMatrix(), m_mapOfKltTrackers(), m_referenceCameraName("Camera"), m_L_kltMulti(),
    m_error_kltMulti(), m_w_kltMulti(), m_weightedError_kltMulti()
{
  if (cameraNames.empty()) {
    throw vpException(vpTrackingException::fatalError, "Cannot construct a vpMbKltMultiTracker with no camera !");
  }

  for (std::vector<std::string>::const_iterator it = cameraNames.begin(); it != cameraNames.end(); ++it) {
    m_mapOfKltTrackers[*it] = new vpMbKltTracker();
  }

  // Set by default the reference camera
  m_referenceCameraName = cameraNames.front();
}

vpMbKltMultiTracker::~vpMbKltMultiTracker()
{
  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
       it != m_mapOfKltTrackers.end(); ++it) {
    delete it->second;
  }
}

void vpMbKltMultiTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r,
                                    const std::string &name)
{
  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
       it != m_mapOfKltTrackers.end(); ++it) {
    it->second->addCircle(P1, P2, P3, r, name);
  }
}

void vpMbKltMultiTracker::computeVVS()
{
  vpMatrix L_true;
  vpMatrix LVJ_true;
  vpColVector v;

  vpMatrix LTL;
  vpColVector LTR;
  vpHomogeneousMatrix cMoPrev;
  vpHomogeneousMatrix ctTc0_Prev;
  vpColVector error_prev;
  double mu = m_initialMu;

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

  computeVVSInit();
  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
       it != m_mapOfKltTrackers.end(); ++it) {
    vpMbKltTracker *klt = it->second;
    klt->computeVVSInit();
  }

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

  while (((int)((normRes - normRes_1) * 1e8) != 0) && (iter < m_maxIter)) {
    computeVVSInteractionMatrixAndResidu(mapOfVelocityTwist);

    bool reStartFromLastIncrement = false;
    computeVVSCheckLevenbergMarquardt(iter, m_error_kltMulti, error_prev, cMoPrev, mu, reStartFromLastIncrement);
    if (reStartFromLastIncrement) {
      ctTc0 = ctTc0_Prev;
    }

    if (!reStartFromLastIncrement) {
      vpMbKltMultiTracker::computeVVSWeights();

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

      normRes_1 = normRes;
      normRes = 0.0;

      for (unsigned int i = 0; i < m_weightedError_kltMulti.getRows(); i++) {
        m_weightedError_kltMulti[i] = m_error_kltMulti[i] * m_w_kltMulti[i];
        normRes += m_weightedError_kltMulti[i];
      }

      if ((iter == 0) || m_computeInteraction) {
        for (unsigned int i = 0; i < m_L_kltMulti.getRows(); i++) {
          for (unsigned int j = 0; j < 6; j++) {
            m_L_kltMulti[i][j] *= m_w_kltMulti[i];
          }
        }
      }

      computeVVSPoseEstimation(isoJoIdentity, iter, m_L_kltMulti, LTL, m_weightedError_kltMulti, m_error_kltMulti,
                               error_prev, LTR, mu, v);

      cMoPrev = cMo;
      ctTc0_Prev = ctTc0;
      ctTc0 = vpExponentialMap::direct(v).inverse() * ctTc0;
      cMo = ctTc0 * c0Mo;
    } // endif(!reStartFromLastIncrement)

    iter++;
  }

  computeCovarianceMatrixVVS(isoJoIdentity, m_w_kltMulti, cMoPrev, L_true, LVJ_true, m_error_kltMulti);
}

void vpMbKltMultiTracker::computeVVSInit()
{
  unsigned int nbFeatures = 2 * m_nbInfos;

  m_L_kltMulti.resize(nbFeatures, 6, false, false);
  m_w_kltMulti.resize(nbFeatures, false);
  m_error_kltMulti.resize(nbFeatures, false);
  m_weightedError_kltMulti.resize(nbFeatures, false);
}

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

void vpMbKltMultiTracker::computeVVSInteractionMatrixAndResidu(
    std::map<std::string, vpVelocityTwistMatrix> &mapOfVelocityTwist)
{
  unsigned int startIdx = 0;

  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
       it != m_mapOfKltTrackers.end(); ++it) {
    vpMbKltTracker *klt = it->second;

    // Use the ctTc0 variable instead of the formula in the monocular case
    // to ensure that we have the same result than vpMbKltTracker
    // as some slight differences can occur due to numerical imprecision
    if (m_mapOfKltTrackers.size() == 1) {
      klt->ctTc0 = ctTc0;
      klt->computeVVSInteractionMatrixAndResidu();
    } else {
      vpHomogeneousMatrix c_curr_tTc_curr0 =
          m_mapOfCameraTransformationMatrix[it->first] * cMo * it->second->c0Mo.inverse();
      klt->ctTc0 = c_curr_tTc_curr0;
      klt->computeVVSInteractionMatrixAndResidu();
    }

    m_error_kltMulti.insert(startIdx, klt->m_error_klt);
    m_L_kltMulti.insert(klt->m_L_klt * mapOfVelocityTwist[it->first], startIdx, 0);

    startIdx += klt->m_error_klt.getRows();
  }
}

void vpMbKltMultiTracker::computeVVSWeights()
{
  unsigned int startIdx = 0;
  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
       it != m_mapOfKltTrackers.end(); ++it) {
    vpMbKltTracker *klt = it->second;
    klt->computeVVSWeights(klt->m_robust_klt, klt->m_error_klt, klt->m_w_klt);

    m_w_kltMulti.insert(startIdx, klt->m_w_klt);
    startIdx += klt->m_w_klt.getRows();
  }
}

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

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

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

    it->second->display(I2, c2Mo, cam2, color, thickness, displayFullModel);
  } else {
    std::cerr << "This display is only for the stereo case ! There are " << m_mapOfKltTrackers.size() << " cameras !"
              << std::endl;
  }
}

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

    it->second->display(I2, c2Mo, cam2, color, thickness, displayFullModel);
  } else {
    std::cerr << "This display is only for the stereo case ! There are " << m_mapOfKltTrackers.size() << " cameras !"
              << std::endl;
  }
}

void vpMbKltMultiTracker::display(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                                  const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
                                  const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
                                  const vpColor &col, const unsigned int thickness, const bool displayFullModel)
{

  // Display only for the given images
  for (std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.begin();
       it_img != mapOfImages.end(); ++it_img) {
    std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.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_klt != m_mapOfKltTrackers.end() && it_camPose != mapOfCameraPoses.end() &&
        it_cam != mapOfCameraParameters.end()) {
      it_klt->second->display(*it_img->second, it_camPose->second, it_cam->second, col, thickness, displayFullModel);
    } else {
      std::cerr << "Missing elements ! " << std::endl;
    }
  }
}

void vpMbKltMultiTracker::display(const std::map<std::string, const vpImage<vpRGBa> *> &mapOfImages,
                                  const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
                                  const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
                                  const vpColor &col, const unsigned int thickness, const bool displayFullModel)
{

  // Display only for the given images
  for (std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img = mapOfImages.begin();
       it_img != mapOfImages.end(); ++it_img) {
    std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.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_klt != m_mapOfKltTrackers.end() && it_camPose != mapOfCameraPoses.end() &&
        it_cam != mapOfCameraParameters.end()) {
      it_klt->second->display(*it_img->second, it_camPose->second, it_cam->second, col, thickness, displayFullModel);
    } else {
      std::cerr << "Missing elements ! " << std::endl;
    }
  }
}

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

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

  return cameraNames;
}

void vpMbKltMultiTracker::getCameraParameters(vpCameraParameters &camera) const
{
  // Get the reference camera parameters
  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(m_referenceCameraName);
  if (it != m_mapOfKltTrackers.end()) {
    it->second->getCameraParameters(camera);
  } else {
    std::cerr << "The reference camera name: " << m_referenceCameraName << " does not exist !" << std::endl;
  }
}

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

    it->second->getCameraParameters(cam2);
  } else {
    std::cerr << "Problem with the number of cameras ! There are " << m_mapOfKltTrackers.size() << " cameras !"
              << std::endl;
  }
}

void vpMbKltMultiTracker::getCameraParameters(const std::string &cameraName, vpCameraParameters &camera) const
{
  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(cameraName);
  if (it != m_mapOfKltTrackers.end()) {
    it->second->getCameraParameters(camera);
  } else {
    std::cerr << "The camera: " << cameraName << " does not exist !" << std::endl;
  }
}

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

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

unsigned int vpMbKltMultiTracker::getClipping(const std::string &cameraName) const
{
  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(cameraName);
  if (it != m_mapOfKltTrackers.end()) {
    return it->second->getClipping();
  } else {
    std::cerr << "Cannot find camera: " << cameraName << std::endl;
  }

  return vpMbTracker::getClipping();
}

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

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

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

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

std::map<std::string, vpMbHiddenFaces<vpMbtPolygon> > vpMbKltMultiTracker::getFaces() const
{
  std::map<std::string, vpMbHiddenFaces<vpMbtPolygon> > mapOfFaces;
  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
       it != m_mapOfKltTrackers.end(); ++it) {
    mapOfFaces[it->first] = it->second->faces;
  }

  return mapOfFaces;
}

std::list<vpMbtDistanceCircle *> &vpMbKltMultiTracker::getFeaturesCircle()
{
  std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);

  if (it_klt != m_mapOfKltTrackers.end()) {
    return it_klt->second->getFeaturesCircle();
  } else {
    std::cerr << "Cannot find reference camera: " << m_referenceCameraName << " !" << std::endl;
  }

  return circles_disp;
}

std::list<vpMbtDistanceCircle *> &vpMbKltMultiTracker::getFeaturesCircle(const std::string &cameraName)
{
  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(cameraName);

  if (it != m_mapOfKltTrackers.end()) {
    return it->second->getFeaturesCircle();
  } else {
    std::cerr << "The camera: " << cameraName << " does not exist !";
  }

  return circles_disp;
}

std::list<vpMbtDistanceKltPoints *> &vpMbKltMultiTracker::getFeaturesKlt()
{
  std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);

  if (it_klt != m_mapOfKltTrackers.end()) {
    return it_klt->second->getFeaturesKlt();
  } else {
    std::cerr << "Cannot find reference camera: " << m_referenceCameraName << " !" << std::endl;
  }

  return kltPolygons;
}

std::list<vpMbtDistanceKltPoints *> &vpMbKltMultiTracker::getFeaturesKlt(const std::string &cameraName)
{
  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(cameraName);

  if (it != m_mapOfKltTrackers.end()) {
    return it->second->getFeaturesKlt();
  } else {
    std::cerr << "The camera: " << cameraName << " does not exist !";
  }

  return kltPolygons;
}

std::list<vpMbtDistanceKltCylinder *> &vpMbKltMultiTracker::getFeaturesKltCylinder()
{
  std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);

  if (it_klt != m_mapOfKltTrackers.end()) {
    return it_klt->second->getFeaturesKltCylinder();
  } else {
    std::cerr << "Cannot find reference camera: " << m_referenceCameraName << " !" << std::endl;
  }

  return kltCylinders;
}

std::list<vpMbtDistanceKltCylinder *> &vpMbKltMultiTracker::getFeaturesKltCylinder(const std::string &cameraName)
{
  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(cameraName);

  if (it != m_mapOfKltTrackers.end()) {
    return it->second->getFeaturesKltCylinder();
  } else {
    std::cerr << "The camera: " << cameraName << " does not exist !";
  }

  return kltCylinders;
}

std::map<std::string, std::vector<vpImagePoint> > vpMbKltMultiTracker::getKltImagePoints() const
{
  std::map<std::string, std::vector<vpImagePoint> > mapOfFeatures;

  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
       it != m_mapOfKltTrackers.end(); ++it) {
    mapOfFeatures[it->first] = it->second->getKltImagePoints();
  }

  return mapOfFeatures;
}

std::map<std::string, std::map<int, vpImagePoint> > vpMbKltMultiTracker::getKltImagePointsWithId() const
{
  std::map<std::string, std::map<int, vpImagePoint> > mapOfFeatures;

  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
       it != m_mapOfKltTrackers.end(); ++it) {
    mapOfFeatures[it->first] = it->second->getKltImagePointsWithId();
  }

  return mapOfFeatures;
}

std::map<std::string, vpKltOpencv> vpMbKltMultiTracker::getKltOpencv() const
{
  std::map<std::string, vpKltOpencv> mapOfKltOpenCVTracker;

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

  return mapOfKltOpenCVTracker;
}

#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
std::map<std::string, std::vector<cv::Point2f> > vpMbKltMultiTracker::getKltPoints() const
{
  std::map<std::string, std::vector<cv::Point2f> > mapOfFeatures;

  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
       it != m_mapOfKltTrackers.end(); ++it) {
    mapOfFeatures[it->first] = it->second->getKltPoints();
  }

  return mapOfFeatures;
}
#else
std::map<std::string, CvPoint2D32f *> vpMbKltMultiTracker::getKltPoints()
{
  std::map<std::string, CvPoint2D32f *> mapOfFeatures;

  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
       it != m_mapOfKltTrackers.end(); ++it) {
    mapOfFeatures[it->first] = it->second->getKltPoints();
  }

  return mapOfFeatures;
}
#endif

std::map<std::string, int> vpMbKltMultiTracker::getKltNbPoints() const
{
  std::map<std::string, int> mapOfFeatures;

  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
       it != m_mapOfKltTrackers.end(); ++it) {
    mapOfFeatures[it->first] = it->second->getKltNbPoints();
  }

  return mapOfFeatures;
}

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

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

std::map<std::string, unsigned int> vpMbKltMultiTracker::getMultiNbPolygon() const
{
  std::map<std::string, unsigned int> mapOfNbPolygons;
  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
       it != m_mapOfKltTrackers.end(); ++it) {
    mapOfNbPolygons[it->first] = it->second->getNbPolygon();
  }

  return mapOfNbPolygons;
}

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

    it->second->getPose(c2Mo);
  } else {
    std::cerr << "Require two cameras ! There are " << m_mapOfKltTrackers.size() << " cameras !" << std::endl;
  }
}

void vpMbKltMultiTracker::getPose(const std::string &cameraName, vpHomogeneousMatrix &cMo_) const
{
  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(cameraName);
  if (it != m_mapOfKltTrackers.end()) {
    it->second->getPose(cMo_);
  } else {
    std::cerr << "The camera: " << cameraName << " does not exist !" << std::endl;
  }
}

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

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

void vpMbKltMultiTracker::init(const vpImage<unsigned char> & /*I*/) {}

#ifdef VISP_HAVE_MODULE_GUI

void vpMbKltMultiTracker::initClick(const vpImage<unsigned char> &I, const std::vector<vpPoint> &points3D_list,
                                    const std::string &displayFile)
{
  if (m_mapOfKltTrackers.empty()) {
    throw vpException(vpTrackingException::initializationError, "There is no camera !");
  } else if (m_mapOfKltTrackers.size() > 1) {
    throw vpException(vpTrackingException::initializationError, "There is more than one camera !");
  } else {
    // Get the vpMbKltTracker object for the reference camera name
    std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(m_referenceCameraName);
    if (it != m_mapOfKltTrackers.end()) {
      it->second->initClick(I, points3D_list, displayFile);
      it->second->getPose(cMo);

      // Init c0Mo
      c0Mo = cMo;
      ctTc0.eye();
    } else {
      std::stringstream ss;
      ss << "Cannot initClick as the reference camera: " << m_referenceCameraName << " does not exist !";
      throw vpException(vpTrackingException::initializationError, ss.str().c_str());
    }
  }
}

void vpMbKltMultiTracker::initClick(const vpImage<unsigned char> &I, const std::string &initFile,
                                    const bool displayHelp, const vpHomogeneousMatrix &T)
{
  if (m_mapOfKltTrackers.empty()) {
    throw vpException(vpTrackingException::initializationError, "There is no camera !");
  } else if (m_mapOfKltTrackers.size() > 1) {
    throw vpException(vpTrackingException::initializationError, "There is more than one camera !");
  } else {
    // Get the vpMbKltTracker object for the reference camera name
    std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(m_referenceCameraName);
    if (it != m_mapOfKltTrackers.end()) {
      it->second->initClick(I, initFile, displayHelp, T);
      it->second->getPose(cMo);

      // Init c0Mo
      c0Mo = cMo;
      ctTc0.eye();
    } else {
      std::stringstream ss;
      ss << "Cannot initClick as the reference camera: " << m_referenceCameraName << " does not exist !";
      throw vpException(vpTrackingException::initializationError, ss.str().c_str());
    }
  }
}

void vpMbKltMultiTracker::initClick(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
                                    const std::string &initFile1, const std::string &initFile2, const bool displayHelp,
                                    const bool firstCameraIsReference)
{
  if (m_mapOfKltTrackers.size() == 2) {
    std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
    it->second->initClick(I1, initFile1, displayHelp);

    if (firstCameraIsReference) {
      // Set the reference cMo
      it->second->getPose(cMo);

      // Set the reference camera parameters
      it->second->getCameraParameters(this->cam);

      // Init c0Mo and ctTc0
      c0Mo = cMo;
      ctTc0.eye();
    }

    ++it;

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

    if (!firstCameraIsReference) {
      // Set the reference cMo
      it->second->getPose(cMo);

      // Set the reference camera parameters
      it->second->getCameraParameters(this->cam);

      // Init c0Mo and ctTc0
      c0Mo = cMo;
      ctTc0.eye();
    }
  } else {
    std::stringstream ss;
    ss << "Cannot init click ! Require two cameras but there are " << m_mapOfKltTrackers.size() << " cameras !";
    throw vpException(vpTrackingException::initializationError, ss.str().c_str());
  }
}

void vpMbKltMultiTracker::initClick(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                                    const std::string &initFile, const bool displayHelp)
{
  std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);
  if (it_klt != m_mapOfKltTrackers.end()) {
    std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
        mapOfImages.find(m_referenceCameraName);

    if (it_img != mapOfImages.end()) {
      it_klt->second->initClick(*it_img->second, initFile, displayHelp);

      // Set the reference cMo
      it_klt->second->getPose(cMo);

      // Init c0Mo
      c0Mo = cMo;
      ctTc0.eye();

      // Set the pose for the others cameras
      for (it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
        if (it_klt->first != m_referenceCameraName) {
          it_img = mapOfImages.find(it_klt->first);
          std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
              m_mapOfCameraTransformationMatrix.find(it_klt->first);

          if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
            vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo;
            it_klt->second->cMo = cCurrentMo;
            it_klt->second->init(*it_img->second);
          } else {
            std::stringstream ss;
            ss << "Cannot init click ! Missing image for camera: " << m_referenceCameraName << " !";
            throw vpException(vpTrackingException::initializationError, ss.str().c_str());
          }
        }
      }
    } else {
      std::stringstream ss;
      ss << "Cannot init click ! Missing image for camera: " << m_referenceCameraName << " !";
      throw vpException(vpTrackingException::initializationError, ss.str().c_str());
    }
  } else {
    std::stringstream ss;
    ss << "Cannot init click ! The reference camera: " << m_referenceCameraName << " does not exist !";
    throw vpException(vpTrackingException::initializationError, ss.str().c_str());
  }
}

void vpMbKltMultiTracker::initClick(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                                    const std::map<std::string, std::string> &mapOfInitFiles, const bool displayHelp)
{
  std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.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_klt != m_mapOfKltTrackers.end() && it_img != mapOfImages.end() && it_initFile != mapOfInitFiles.end()) {
    it_klt->second->initClick(*it_img->second, it_initFile->second, displayHelp);
    it_klt->second->getPose(cMo);

    c0Mo = this->cMo;
    ctTc0.eye();
  } 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_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
    if (it_klt->first != m_referenceCameraName) {
      it_img = mapOfImages.find(it_klt->first);
      it_initFile = mapOfInitFiles.find(it_klt->first);

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

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

    if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
      vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo;
      m_mapOfKltTrackers[*it1]->cMo = cCurrentMo;
      m_mapOfKltTrackers[*it1]->init(*it_img->second);
    } else {
      std::stringstream ss;
      ss << "Missing image or missing camera transformation matrix ! Cannot "
            "set the pose for camera: "
         << (*it1) << " !";
      throw vpException(vpTrackingException::initializationError, ss.str().c_str());
    }
  }
}
#endif //#ifdef VISP_HAVE_MODULE_GUI

void vpMbKltMultiTracker::initFromPose(const vpImage<unsigned char> &I, const std::string &initFile)
{
  // Monocular case only !
  if (m_mapOfKltTrackers.size() > 1) {
    throw vpException(vpTrackingException::fatalError, "This function can only be used for the monocular case !");
  }

  char s[FILENAME_MAX];
  std::fstream finit;
  vpPoseVector init_pos;

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

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

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

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

  // Set the new pose for the reference camera
  cMo.buildFrom(init_pos);
  c0Mo = cMo;
  ctTc0.eye();

  // Init for the reference camera
  std::map<std::string, vpMbKltTracker *>::iterator it_ref = m_mapOfKltTrackers.find(m_referenceCameraName);
  if (it_ref == m_mapOfKltTrackers.end()) {
    throw vpException(vpTrackingException::initializationError, "Cannot find the reference camera !");
  }

  it_ref->second->initFromPose(I, cMo);
}

void vpMbKltMultiTracker::initFromPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo_)
{
  this->cMo = cMo_;
  c0Mo = cMo;
  ctTc0.eye();

  // Init for the reference camera
  std::map<std::string, vpMbKltTracker *>::iterator it_ref = m_mapOfKltTrackers.find(m_referenceCameraName);
  if (it_ref == m_mapOfKltTrackers.end()) {
    throw vpException(vpTrackingException::initializationError, "Cannot find the reference camera !");
  }

  it_ref->second->initFromPose(I, cMo);
}

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

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

    ++it;

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

    if (firstCameraIsReference) {
      this->cMo = c1Mo;
    } else {
      this->cMo = c2Mo;
    }

    c0Mo = this->cMo;
    ctTc0.eye();
  } else {
    std::stringstream ss;
    ss << "This method requires 2 cameras but there are " << m_mapOfKltTrackers.size() << " cameras !";
    throw vpException(vpTrackingException::initializationError, ss.str());
  }
}

void vpMbKltMultiTracker::initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                                       const vpHomogeneousMatrix &cMo_)
{
  std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);

  if (it_klt != m_mapOfKltTrackers.end()) {
    std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.find(it_klt->first);

    if (it_img != mapOfImages.end()) {
      it_klt->second->initFromPose(*it_img->second, cMo_);

      cMo = cMo_;
      c0Mo = this->cMo;
      ctTc0.eye();

      for (it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
        if (it_klt->first != m_referenceCameraName) {
          std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
              m_mapOfCameraTransformationMatrix.find(it_klt->first);
          it_img = mapOfImages.find(it_klt->first);

          if (it_camTrans != m_mapOfCameraTransformationMatrix.end() && it_img != mapOfImages.end()) {
            vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo;
            it_klt->second->initFromPose(*it_img->second, cCurrentMo);
          } else {
            throw vpException(vpTrackingException::initializationError,
                              "Cannot find camera transformation matrix or image !");
          }
        }
      }
    } else {
      throw vpException(vpTrackingException::initializationError, "Cannot find image for reference camera !");
    }
  } else {
    std::stringstream ss;
    ss << "Cannot find reference camera: " << m_referenceCameraName << std::endl;
    throw vpException(vpTrackingException::initializationError, ss.str());
  }
}

void vpMbKltMultiTracker::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, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.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_klt != m_mapOfKltTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
    it_klt->second->initFromPose(*it_img->second, it_camPose->second);
    it_klt->second->getPose(cMo);

    c0Mo = this->cMo;
    ctTc0.eye();
  } 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_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
    it_img = mapOfImages.find(it_klt->first);
    it_camPose = mapOfCameraPoses.find(it_klt->first);

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

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

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

void vpMbKltMultiTracker::loadConfigFile(const std::string &configFile)
{
  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(m_referenceCameraName);
  if (it != m_mapOfKltTrackers.end()) {
    // Load ConfigFile for reference camera
    it->second->loadConfigFile(configFile);
    it->second->getCameraParameters(cam);

    // Set clipping
    this->clippingFlag = it->second->getClipping();
    this->angleAppears = it->second->getAngleAppear();
    this->angleDisappears = it->second->getAngleDisappear();
  } else {
    std::stringstream ss;
    ss << "The reference camera: " << m_referenceCameraName << " does not exist !";
    throw vpException(vpTrackingException::initializationError, ss.str().c_str());
  }
}

void vpMbKltMultiTracker::loadConfigFile(const std::string &configFile1, const std::string &configFile2,
                                         const bool firstCameraIsReference)
{
  if (m_mapOfKltTrackers.size() == 2) {
    std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
    it->second->loadConfigFile(configFile1);

    if (firstCameraIsReference) {
      it->second->getCameraParameters(cam);

      // Set clipping
      this->clippingFlag = it->second->getClipping();
      this->angleAppears = it->second->getAngleAppear();
      this->angleDisappears = it->second->getAngleDisappear();
    }
    ++it;

    it->second->loadConfigFile(configFile2);

    if (!firstCameraIsReference) {
      it->second->getCameraParameters(cam);

      // Set clipping
      this->clippingFlag = it->second->getClipping();
      this->angleAppears = it->second->getAngleAppear();
      this->angleDisappears = it->second->getAngleDisappear();
    }
  } else {
    std::stringstream ss;
    ss << "Cannot loadConfigFile. Require two cameras ! There are " << m_mapOfKltTrackers.size() << " cameras !";
    throw vpException(vpTrackingException::initializationError, ss.str().c_str());
  }
}

void vpMbKltMultiTracker::loadConfigFile(const std::map<std::string, std::string> &mapOfConfigFiles)
{
  for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
       it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
    std::map<std::string, std::string>::const_iterator it_config = mapOfConfigFiles.find(it_klt->first);
    if (it_config != mapOfConfigFiles.end()) {
      it_klt->second->loadConfigFile(it_config->second);
    } else {
      std::stringstream ss;
      ss << "Missing configuration file for camera: " << it_klt->first << " !";
      throw vpException(vpTrackingException::initializationError, ss.str().c_str());
    }
  }

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

    // Set clipping
    this->clippingFlag = it->second->getClipping();
    this->angleAppears = it->second->getAngleAppear();
    this->angleDisappears = it->second->getAngleDisappear();
  } else {
    std::stringstream ss;
    ss << "The reference camera: " << m_referenceCameraName << " does not exist !";
    throw vpException(vpTrackingException::initializationError, ss.str().c_str());
  }
}

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

  modelInitialised = true;
}

void vpMbKltMultiTracker::preTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages)
{
  m_nbInfos = 0;
  m_nbFaceUsed = 0;

  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
       it != m_mapOfKltTrackers.end(); ++it) {
    vpMbKltTracker *klt = it->second;

    try {
      klt->preTracking(*mapOfImages[it->first]);
      m_nbInfos += klt->m_nbInfos;
      m_nbFaceUsed += klt->m_nbFaceUsed;
    } catch (...) {
    }
  }
}

void vpMbKltMultiTracker::postTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages)
{

  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
       it != m_mapOfKltTrackers.end(); ++it) {
    vpMbKltTracker *klt = it->second;

    // Set the camera pose
    it->second->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo;

    if (klt->m_nbInfos > 0 && klt->postTracking(*mapOfImages[it->first], klt->m_w_klt)) {
      klt->reinit(*mapOfImages[it->first]);

      // set ctTc0 to identity
      if (it->first == m_referenceCameraName) {
        reinit(/*mapOfImages[it->first]*/);
      }
    }
  }
}

void vpMbKltMultiTracker::reinit(/* const vpImage<unsigned char>& I*/)
{
  c0Mo = cMo;
  ctTc0.eye();
}

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

  firstInitialisation = true;
  modelInitialised = true;

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

    // Set reference pose
    it_klt->second->getPose(cMo);

    c0Mo = cMo;
    ctTc0.eye();
  } else {
    throw vpException(vpTrackingException::fatalError, "Cannot reInitModel the reference camera !");
  }
}

void vpMbKltMultiTracker::reInitModel(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
                                      const std::string &cad_name, const vpHomogeneousMatrix &c1Mo,
                                      const vpHomogeneousMatrix &c2Mo, const bool verbose,
                                      const bool firstCameraIsReference)
{
  if (m_mapOfKltTrackers.size() == 2) {
    std::map<std::string, vpMbKltTracker *>::const_iterator it_edge = m_mapOfKltTrackers.begin();

    it_edge->second->reInitModel(I1, cad_name, c1Mo, verbose);

    if (firstCameraIsReference) {
      // Set reference pose
      it_edge->second->getPose(cMo);
    }

    ++it_edge;

    it_edge->second->reInitModel(I2, cad_name, c2Mo, verbose);

    if (!firstCameraIsReference) {
      // Set reference pose
      it_edge->second->getPose(cMo);
    }

    c0Mo = cMo;
    ctTc0.eye();
  } else {
    throw vpException(vpTrackingException::fatalError, "This method requires exactly two cameras !");
  }
}

void vpMbKltMultiTracker::reInitModel(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                                      const std::string &cad_name,
                                      const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
                                      const bool verbose)
{
  std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.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_klt != m_mapOfKltTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
    it_klt->second->reInitModel(*it_img->second, cad_name, it_camPose->second, verbose);
    modelInitialised = true;

    // Set reference pose
    it_klt->second->getPose(cMo);

    c0Mo = cMo;
    ctTc0.eye();
  } else {
    throw vpException(vpTrackingException::fatalError, "Cannot reInitModel for reference camera !");
  }

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

      if (it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
        it_klt->second->reInitModel(*it_img->second, cad_name, it_camPose->second, verbose);
      } else {
        vectorOfMissingCameras.push_back(it_klt->first);
      }
    }
  }

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

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

void vpMbKltMultiTracker::resetTracker()
{
  cMo.eye();
  ctTc0.eye();

  // Call resetTracker for all the cameras
  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
       it != m_mapOfKltTrackers.end(); ++it) {
    it->second->resetTracker();
  }

  m_computeInteraction = true;
  firstInitialisation = true;
  computeCovariance = false;

  angleAppears = vpMath::rad(65);
  angleDisappears = vpMath::rad(75);

  clippingFlag = vpPolygon3D::NO_CLIPPING;

  maskBorder = 5;
  threshold_outlier = 0.5;
  percentGood = 0.7;

  m_lambda = 0.8;
  m_maxIter = 200;

  m_optimizationMethod = vpMbTracker::GAUSS_NEWTON_OPT;

  useScanLine = false;

#ifdef VISP_HAVE_OGRE
  useOgre = false;
#endif
}

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

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

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

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

void vpMbKltMultiTracker::setCameraParameters(const vpCameraParameters &camera)
{
  if (m_mapOfKltTrackers.empty()) {
    throw vpException(vpTrackingException::fatalError, "There is no camera !");
  } else if (m_mapOfKltTrackers.size() > 1) {
    throw vpException(vpTrackingException::fatalError, "There is more than one camera !");
  } else {
    std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(m_referenceCameraName);
    if (it != m_mapOfKltTrackers.end()) {
      it->second->setCameraParameters(camera);

      // Set reference camera parameters
      this->cam = camera;
    } else {
      std::stringstream ss;
      ss << "The reference camera: " << m_referenceCameraName << " does not exist !";
      throw vpException(vpTrackingException::fatalError, ss.str());
    }
  }
}

void vpMbKltMultiTracker::setCameraParameters(const vpCameraParameters &camera1, const vpCameraParameters &camera2,
                                              const bool firstCameraIsReference)
{
  if (m_mapOfKltTrackers.empty()) {
    throw vpException(vpTrackingException::fatalError, "There is no camera !");
  } else if (m_mapOfKltTrackers.size() == 2) {
    std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
    it->second->setCameraParameters(camera1);

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

    if (firstCameraIsReference) {
      this->cam = camera1;
    } else {
      this->cam = camera2;
    }
  } else {
    std::stringstream ss;
    ss << "Require two cameras ! There are " << m_mapOfKltTrackers.size() << " cameras !";
    throw vpException(vpTrackingException::fatalError, ss.str());
  }
}

void vpMbKltMultiTracker::setCameraParameters(const std::string &cameraName, const vpCameraParameters &camera)
{
  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(cameraName);
  if (it != m_mapOfKltTrackers.end()) {
    it->second->setCameraParameters(camera);

    if (it->first == m_referenceCameraName) {
      this->cam = camera;
    }
  } else {
    std::stringstream ss;
    ss << "The camera: " << cameraName << " does not exist !";
    throw vpException(vpTrackingException::fatalError, ss.str());
  }
}

void vpMbKltMultiTracker::setCameraParameters(const std::map<std::string, vpCameraParameters> &mapOfCameraParameters)
{
  for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
       it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
    std::map<std::string, vpCameraParameters>::const_iterator it_cam = mapOfCameraParameters.find(it_klt->first);
    if (it_cam != mapOfCameraParameters.end()) {
      it_klt->second->setCameraParameters(it_cam->second);

      if (it_klt->first == m_referenceCameraName) {
        this->cam = it_cam->second;
      }
    } else {
      std::stringstream ss;
      ss << "Missing camera parameters for camera: " << it_klt->first << " !";
      throw vpException(vpTrackingException::fatalError, ss.str());
    }
  }
}

void vpMbKltMultiTracker::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 {
    std::stringstream ss;
    ss << "Cannot find camera: " << cameraName << " !";
    throw vpException(vpTrackingException::fatalError, ss.str());
  }
}

void vpMbKltMultiTracker::setCameraTransformationMatrix(
    const std::map<std::string, vpHomogeneousMatrix> &mapOfTransformationMatrix)
{
  // Check if all cameras have a transformation matrix
  for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
       it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
    std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
        mapOfTransformationMatrix.find(it_klt->first);

    if (it_camTrans == mapOfTransformationMatrix.end()) {
      throw vpException(vpTrackingException::initializationError, "Missing camera transformation matrix !");
    }
  }

  m_mapOfCameraTransformationMatrix = mapOfTransformationMatrix;
}

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

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

void vpMbKltMultiTracker::setClipping(const std::string &cameraName, const unsigned int &flags)
{
  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(cameraName);
  if (it != m_mapOfKltTrackers.end()) {
    it->second->setClipping(flags);
  } else {
    std::cerr << "Camera: " << cameraName << " does not exist !" << std::endl;
  }
}

void vpMbKltMultiTracker::setCovarianceComputation(const bool &flag)
{
  vpMbTracker::setCovarianceComputation(flag);

  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
       it != m_mapOfKltTrackers.end(); ++it) {
    it->second->setCovarianceComputation(flag);
  }
}

void vpMbKltMultiTracker::setDisplayFeatures(const bool displayF)
{
  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
       it != m_mapOfKltTrackers.end(); ++it) {
    it->second->setDisplayFeatures(displayF);
  }

  displayFeatures = displayF;
}

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

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

void vpMbKltMultiTracker::setFarClippingDistance(const std::string &cameraName, const double &dist)
{
  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(cameraName);
  if (it != m_mapOfKltTrackers.end()) {
    it->second->setFarClippingDistance(dist);
  } else {
    std::cerr << "Camera: " << cameraName << " does not exist !" << std::endl;
  }
}

#ifdef VISP_HAVE_OGRE

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

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

void vpMbKltMultiTracker::setKltOpencv(const vpKltOpencv &t)
{
  for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
       it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
    it_klt->second->setKltOpencv(t);
  }
}

void vpMbKltMultiTracker::setKltOpencv(const std::map<std::string, vpKltOpencv> &mapOfOpenCVTrackers)
{
  for (std::map<std::string, vpKltOpencv>::const_iterator it_kltOpenCV = mapOfOpenCVTrackers.begin();
       it_kltOpenCV != mapOfOpenCVTrackers.end(); ++it_kltOpenCV) {
    std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(it_kltOpenCV->first);
    if (it_klt != m_mapOfKltTrackers.end()) {
      it_klt->second->setKltOpencv(it_kltOpenCV->second);
    } else {
      std::cerr << "The camera: " << it_kltOpenCV->first << " does not exist !" << std::endl;
    }
  }
}

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

void vpMbKltMultiTracker::setLod(const bool useLod, const std::string &cameraName, const std::string &name)
{
  std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(cameraName);

  if (it_klt != m_mapOfKltTrackers.end()) {
    it_klt->second->setLod(useLod, name);
  } else {
    std::cerr << "The camera: " << cameraName << " cannot be found !" << std::endl;
  }
}

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

  maskBorder = e;
}

void vpMbKltMultiTracker::setMinLineLengthThresh(const double /*minLineLengthThresh*/, const std::string & /*name*/)
{
  std::cerr << "Useless for KLT tracker !" << std::endl;
}

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

void vpMbKltMultiTracker::setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &cameraName,
                                                  const std::string &name)
{
  std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(cameraName);

  if (it_klt != m_mapOfKltTrackers.end()) {
    it_klt->second->setMinPolygonAreaThresh(minPolygonAreaThresh, name);
  } else {
    std::cerr << "The camera: " << cameraName << " cannot be found !" << std::endl;
  }
}

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

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

void vpMbKltMultiTracker::setOgreShowConfigDialog(const bool showConfigDialog)
{
  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
       it != m_mapOfKltTrackers.end(); ++it) {
    it->second->setOgreShowConfigDialog(showConfigDialog);
  }
}

void vpMbKltMultiTracker::setOgreVisibilityTest(const bool &v)
{
  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
       it != m_mapOfKltTrackers.end(); ++it) {
    it->second->setOgreVisibilityTest(v);
  }

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

  useOgre = v;
}

void vpMbKltMultiTracker::setNearClippingDistance(const std::string &cameraName, const double &dist)
{
  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(cameraName);
  if (it != m_mapOfKltTrackers.end()) {
    it->second->setNearClippingDistance(dist);
  } else {
    std::cerr << "Camera: " << cameraName << " does not exist !" << std::endl;
  }
}

void vpMbKltMultiTracker::setOptimizationMethod(const vpMbtOptimizationMethod &opt)
{
  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
       it != m_mapOfKltTrackers.end(); ++it) {
    it->second->setOptimizationMethod(opt);
  }

  m_optimizationMethod = opt;
}

void vpMbKltMultiTracker::setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo_)
{
  if (m_mapOfKltTrackers.size() == 1) {
    std::map<std::string, vpMbKltTracker *>::iterator it = m_mapOfKltTrackers.find(m_referenceCameraName);
    if (it != m_mapOfKltTrackers.end()) {
      it->second->setPose(I, cMo_);
      this->cMo = cMo_;

      c0Mo = this->cMo;
      ctTc0.eye();
    } else {
      std::stringstream ss;
      ss << "Cannot find the reference camera: " << m_referenceCameraName << " !";
      throw vpException(vpTrackingException::fatalError, ss.str());
    }
  } else {
    std::stringstream ss;
    ss << "You are trying to set the pose with only one image and cMo "
          "but there are multiple cameras !";
    throw vpException(vpTrackingException::fatalError, ss.str());
  }
}

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

    ++it;

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

    if (firstCameraIsReference) {
      this->cMo = c1Mo;
    } else {
      this->cMo = c2Mo;
    }

    c0Mo = this->cMo;
    ctTc0.eye();
  } else {
    std::stringstream ss;
    ss << "This method requires 2 cameras but there are " << m_mapOfKltTrackers.size() << " cameras !";
    throw vpException(vpTrackingException::fatalError, ss.str());
  }
}

void vpMbKltMultiTracker::setPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                                  const vpHomogeneousMatrix &cMo_)
{
  std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);
  if (it_klt != m_mapOfKltTrackers.end()) {
    std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
        mapOfImages.find(m_referenceCameraName);

    if (it_img != mapOfImages.end()) {
      // Set pose on reference camera
      it_klt->second->setPose(*it_img->second, cMo_);

      // Set the reference cMo
      cMo = cMo_;

      c0Mo = this->cMo;
      ctTc0.eye();

      // Set the pose for the others cameras
      for (it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
        if (it_klt->first != m_referenceCameraName) {
          std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
              m_mapOfCameraTransformationMatrix.find(it_klt->first);
          it_img = mapOfImages.find(it_klt->first);

          if (it_camTrans != m_mapOfCameraTransformationMatrix.end() && it_img != mapOfImages.end()) {
            vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo;
            it_klt->second->setPose(*it_img->second, cCurrentMo);
          } else {
            throw vpException(vpTrackingException::fatalError, "Cannot find camera transformation matrix or image !");
          }
        }
      }
    } else {
      std::stringstream ss;
      ss << "Missing image for reference camera: " << m_referenceCameraName << " !";
      throw vpException(vpTrackingException::fatalError, ss.str().c_str());
    }
  } else {
    std::stringstream ss;
    ss << "The reference camera: " << m_referenceCameraName << " does not exist !";
    throw vpException(vpTrackingException::fatalError, ss.str().c_str());
  }
}

void vpMbKltMultiTracker::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, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.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_klt != m_mapOfKltTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
    it_klt->second->setPose(*it_img->second, it_camPose->second);
    it_klt->second->getPose(cMo);

    c0Mo = this->cMo;
    ctTc0.eye();
  } 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_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
    if (it_klt->first != m_referenceCameraName) {
      it_img = mapOfImages.find(it_klt->first);
      it_camPose = mapOfCameraPoses.find(it_klt->first);

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

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

    if (it_img != mapOfImages.end() && it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
      vpHomogeneousMatrix cCurrentMo = it_camTrans->second * cMo;
      m_mapOfKltTrackers[*it1]->setPose(*it_img->second, cCurrentMo);
    } else {
      std::stringstream ss;
      ss << "Missing image or missing camera transformation matrix ! Cannot "
            "set the pose for camera: "
         << (*it1) << " !";
      throw vpException(vpTrackingException::fatalError, ss.str().c_str());
    }
  }
}

void vpMbKltMultiTracker::setReferenceCameraName(const std::string &referenceCameraName)
{
  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(referenceCameraName);
  if (it != m_mapOfKltTrackers.end()) {
    m_referenceCameraName = referenceCameraName;
  } else {
    std::stringstream ss;
    ss << "The reference camera: " << referenceCameraName << " does not exist !";
    throw vpException(vpTrackingException::fatalError, ss.str().c_str());
  }
}

void vpMbKltMultiTracker::setScanLineVisibilityTest(const bool &v)
{
  // Set general setScanLineVisibilityTest
  vpMbTracker::setScanLineVisibilityTest(v);

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

void vpMbKltMultiTracker::setKltThresholdAcceptation(const double th)
{
  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
       it != m_mapOfKltTrackers.end(); ++it) {
    it->second->setKltThresholdAcceptation(th);
  }

  threshold_outlier = th;
}

void vpMbKltMultiTracker::setUseKltTracking(const std::string &name, const bool &useKltTracking)
{
  for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
       it != m_mapOfKltTrackers.end(); ++it) {
    it->second->setUseKltTracking(name, useKltTracking);
  }
}

void vpMbKltMultiTracker::track(const vpImage<unsigned char> &I)
{
  // Track only with reference camera
  // Get the reference camera parameters
  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(m_referenceCameraName);

  if (it != m_mapOfKltTrackers.end()) {
    it->second->track(I);
    it->second->getPose(cMo);
  } else {
    std::stringstream ss;
    ss << "The reference camera: " << m_referenceCameraName << " does not exist !";
    throw vpException(vpTrackingException::fatalError, ss.str().c_str());
  }
}

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

    mapOfImages[it->first] = &I2;
    track(mapOfImages);
  } else {
    std::stringstream ss;
    ss << "Require two cameras ! There are " << m_mapOfKltTrackers.size() << " cameras !";
    throw vpException(vpTrackingException::fatalError, ss.str().c_str());
  }
}

void vpMbKltMultiTracker::track(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages)
{
  // Check if there is an image for each camera
  for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
       it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
    std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.find(it_klt->first);

    if (it_img == mapOfImages.end()) {
      throw vpException(vpTrackingException::fatalError, "Missing images !");
    }
  }

  preTracking(mapOfImages);

  if (m_nbInfos < 4 || m_nbFaceUsed == 0) {
    throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
  }

  computeVVS();

  postTracking(mapOfImages);
}


std::map<std::string, int> vpMbKltMultiTracker::getNbKltPoints() const { return getKltNbPoints(); }

void vpMbKltMultiTracker::setMaskBorder(const unsigned int &e) { setKltMaskBorder(e); }

void vpMbKltMultiTracker::setThresholdAcceptation(const double th) { setKltThresholdAcceptation(th); }

#elif !defined(VISP_BUILD_SHARED_LIBS)
// Work arround to avoid warning:
// libvisp_mbt.a(dummy_vpMbKltMultiTracker.cpp.o) has no symbols
void dummy_vpMbKltMultiTracker(){}
#endif // VISP_HAVE_OPENCV
#elif !defined(VISP_BUILD_SHARED_LIBS)
// Work arround to avoid warning: libvisp_mbt.a(dummy_vpMbKltMultiTracker.cpp.o) has no symbols
void dummy_vpMbKltMultiTracker(){}
#endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)