vpMbKltMultiTracker.cpp

/****************************************************************************
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2016 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
 * ("GPL") version 2 as published by the Free Software Foundation.
 * 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://www.irisa.fr/lagadic/visp/visp.html for more information.
 *
 * This software was developed at:
 * INRIA Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 * http://www.irisa.fr/lagadic
 *
 * 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_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_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") {

  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") {
  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(std::map<std::string, unsigned int> &mapOfNbInfos, vpColVector &w) {
  vpMatrix L;     // interaction matrix
  vpColVector R;  // residu
  vpMatrix L_true;     // interaction matrix
  vpMatrix LVJ_true;
  //vpColVector R_true;  // residu
  vpColVector w_true;
  vpColVector v;  // "speed" for VVS

  unsigned int nbInfos = 0;
  for(std::map<std::string, unsigned int>::const_iterator it = mapOfNbInfos.begin(); it != mapOfNbInfos.end(); ++it) {
    nbInfos += it->second;
  }

  std::map<std::string, vpRobust> mapOfRobusts;

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

  vpMatrix LTL;
  vpColVector LTR;
  vpHomogeneousMatrix cMoPrev;
  vpHomogeneousMatrix ctTc0_Prev;
  vpColVector error_prev(2*nbInfos);
  double mu = 0.01;

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

  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<maxIter) ) {
    L.resize(0,0);
    R.resize(0);

    for(std::map<std::string, vpMbKltTracker*>::const_iterator it1 = m_mapOfKltTrackers.begin();
        it1 != m_mapOfKltTrackers.end(); ++it1) {
      unsigned int shift = 0;
      vpColVector R_current;  // residu
      vpMatrix L_current;     // interaction matrix
      vpHomography H_current;

      R_current.resize(2 * mapOfNbInfos[it1->first]);
      L_current.resize(2 * mapOfNbInfos[it1->first], 6, 0);

      //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) {
        computeVVSInteractionMatrixAndResidu(shift, R_current, L_current, H_current,
            it1->second->kltPolygons, it1->second->kltCylinders, ctTc0);
      } else {
        vpHomogeneousMatrix c_curr_tTc_curr0 = m_mapOfCameraTransformationMatrix[it1->first] *
            cMo * it1->second->c0Mo.inverse();
        computeVVSInteractionMatrixAndResidu(shift, R_current, L_current, H_current,
            it1->second->kltPolygons, it1->second->kltCylinders, c_curr_tTc_curr0);
      }

      //VelocityTwistMatrix
      L_current = L_current*mapOfVelocityTwist[it1->first];

      //Stack residu and interaction matrix
      R.stack(R_current);
      L.stack(L_current);
    }

    bool reStartFromLastIncrement = false;
    computeVVSCheckLevenbergMarquardtKlt(iter, nbInfos, cMoPrev, error_prev, ctTc0_Prev, mu, reStartFromLastIncrement);

    if(!reStartFromLastIncrement) {
      vpMbKltMultiTracker::computeVVSWeights(iter, nbInfos, mapOfNbInfos, R, w_true, w, mapOfRobusts, 2.0);

      computeVVSPoseEstimation(iter, L, w, L_true, LVJ_true, normRes, normRes_1, w_true, R, LTL, LTR,
          error_prev, v, mu, cMoPrev, ctTc0_Prev);
    } // endif(!reStartFromLastIncrement)

    iter++;
  }

  computeVVSCovariance(w_true, cMoPrev, L_true, LVJ_true);
}

void vpMbKltMultiTracker::computeVVSWeights(const unsigned int iter, const unsigned int nbInfos,
    std::map<std::string, unsigned int> &mapOfNbInfos, vpColVector &R, vpColVector &w_true, vpColVector &w,
    std::map<std::string, vpRobust> &mapOfRobusts, double threshold) {
  /* robust */
  if(iter == 0) {
    w_true.resize(2*nbInfos);
    w.resize(2*nbInfos);
    w = 1;
    w_true = 1;

    for(std::map<std::string, vpMbKltTracker*>::const_iterator it = m_mapOfKltTrackers.begin();
        it != m_mapOfKltTrackers.end(); ++it) {
      vpCameraParameters camTmp;
      it->second->getCameraParameters(camTmp);
      mapOfRobusts[it->first].setThreshold(threshold / camTmp.get_px());
    }
  }

  unsigned int shift = 0;
  for(std::map<std::string, vpMbKltTracker*>::const_iterator it = m_mapOfKltTrackers.begin();
      it != m_mapOfKltTrackers.end(); ++it) {
    if(mapOfNbInfos[it->first] > 0) {
      vpSubColVector sub_w(w, shift, 2*mapOfNbInfos[it->first]);
      vpSubColVector sub_r(R, shift, 2*mapOfNbInfos[it->first]);
      shift += 2*mapOfNbInfos[it->first];

      mapOfRobusts[it->first].setIteration(iter);
      mapOfRobusts[it->first].MEstimator(vpRobust::TUKEY, sub_r, sub_w);
    }
  }
}

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

  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) {
  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);
      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) {
  for(std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
      it != m_mapOfKltTrackers.end(); ++it) {
    it->second->loadModel(modelFile, verbose);
  }

  modelInitialised = true;
}

void vpMbKltMultiTracker::preTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
    std::map<std::string, unsigned int> &mapOfNbInfos,
    std::map<std::string, unsigned int> &mapOfNbFaceUsed) {
  for(std::map<std::string, vpMbKltTracker*>::const_iterator it = m_mapOfKltTrackers.begin();
      it != m_mapOfKltTrackers.end(); ++it) {
    mapOfNbInfos[it->first] = 0;
    mapOfNbFaceUsed[it->first] = 0;
  }

  for (std::map<std::string, vpMbKltTracker*>::const_iterator it =
      m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) {
    try {
      it->second->preTracking(*mapOfImages[it->first], mapOfNbInfos[it->first], mapOfNbFaceUsed[it->first]);
    } catch (/*vpException &e*/...) {
//      throw e;
    }
  }
}

void vpMbKltMultiTracker::postTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
    std::map<std::string, unsigned int> &mapOfNbInfos, vpColVector &w_klt) {
  unsigned int shift = 0;
  for(std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
      it != m_mapOfKltTrackers.end(); ++it) {
    //Set the camera pose
    it->second->cMo = m_mapOfCameraTransformationMatrix[it->first]*cMo;

    if(mapOfNbInfos[it->first] > 0) {
      vpSubColVector sub_w(w_klt, shift, 2*mapOfNbInfos[it->first]);
      shift += 2*mapOfNbInfos[it->first];
      if(it->second->postTracking(*mapOfImages[it->first], sub_w)) {
        it->second->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) {
  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);

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

  compute_interaction = 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;

  lambda = 0.8;
  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::setMaskBorder(const unsigned int &e) {
  for(std::map<std::string, vpMbKltTracker*>::const_iterator it = m_mapOfKltTrackers.begin();
      it != m_mapOfKltTrackers.end(); ++it) {
    it->second->setMaskBorder(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::setThresholdAcceptation(const double th) {
  for(std::map<std::string, vpMbKltTracker*>::const_iterator it = m_mapOfKltTrackers.begin();
      it != m_mapOfKltTrackers.end(); ++it) {
    it->second->setThresholdAcceptation(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 !");
    }
  }

  std::map<std::string, unsigned int> mapOfNbInfos;
  std::map<std::string, unsigned int> mapOfNbFaceUsed;

  preTracking(mapOfImages, mapOfNbInfos, mapOfNbFaceUsed);

  bool atLeastOneTrackerOk = false;
  for(std::map<std::string, vpMbKltTracker*>::const_iterator it = m_mapOfKltTrackers.begin();
        it != m_mapOfKltTrackers.end(); ++it) {
    if(mapOfNbInfos[it->first] >= 4 && mapOfNbFaceUsed[it->first] != 0) {
      atLeastOneTrackerOk = true;
    }
  }

  if(!atLeastOneTrackerOk) {
    vpERROR_TRACE("\n\t\t Error-> not enough data") ;
    throw vpTrackingException(vpTrackingException::notEnoughPointError, "\n\t\t Error-> not enough data");
  }

  computeVVS(mapOfNbInfos, m_w);

  postTracking(mapOfImages, mapOfNbInfos, m_w);
}

#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