vpMbEdgeMultiTracker.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 as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact INRIA about acquiring a ViSP Professional
 * Edition License.
 *
 * See https://visp.inria.fr/download/ 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 edge tracker with multiple cameras.
 *
 * Authors:
 * Souriya Trinh
 *
 *****************************************************************************/

#include <visp3/core/vpDebug.h>
#include <visp3/core/vpExponentialMap.h>
#include <visp3/core/vpTrackingException.h>
#include <visp3/core/vpVelocityTwistMatrix.h>
#include <visp3/mbt/vpMbEdgeMultiTracker.h>

vpMbEdgeMultiTracker::vpMbEdgeMultiTracker()
  : m_mapOfCameraTransformationMatrix(), m_mapOfEdgeTrackers(), m_mapOfPyramidalImages(),
    m_referenceCameraName("Camera"), m_L_edgeMulti(), m_error_edgeMulti(), m_w_edgeMulti(), m_weightedError_edgeMulti()
{
  m_mapOfEdgeTrackers["Camera"] = new vpMbEdgeTracker();

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

vpMbEdgeMultiTracker::vpMbEdgeMultiTracker(const unsigned int nbCameras)
  : m_mapOfCameraTransformationMatrix(), m_mapOfEdgeTrackers(), m_mapOfPyramidalImages(),
    m_referenceCameraName("Camera"), m_L_edgeMulti(), m_error_edgeMulti(), m_w_edgeMulti(), m_weightedError_edgeMulti()
{

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

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

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

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

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

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

vpMbEdgeMultiTracker::vpMbEdgeMultiTracker(const std::vector<std::string> &cameraNames)
  : m_mapOfCameraTransformationMatrix(), m_mapOfEdgeTrackers(), m_mapOfPyramidalImages(),
    m_referenceCameraName("Camera"), m_L_edgeMulti(), m_error_edgeMulti(), m_w_edgeMulti(), m_weightedError_edgeMulti()
{

  if (cameraNames.empty()) {
    throw vpException(vpTrackingException::fatalError, "Cannot construct a vpMbEdgeMultiTracker with no camera !");
  }

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

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

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

  m_mapOfEdgeTrackers.clear();

  cleanPyramid(m_mapOfPyramidalImages);
}

void vpMbEdgeMultiTracker::cleanPyramid(std::map<std::string, std::vector<const vpImage<unsigned char> *> > &pyramid)
{
  for (std::map<std::string, std::vector<const vpImage<unsigned char> *> >::iterator it1 = pyramid.begin();
       it1 != pyramid.end(); ++it1) {
    if (it1->second.size() > 0) {
      it1->second[0] = NULL;
      for (size_t i = 1; i < it1->second.size(); i++) {
        if (it1->second[i] != NULL) {
          delete it1->second[i];
          it1->second[i] = NULL;
        }
      }
      it1->second.clear();
    }
  }
}

void vpMbEdgeMultiTracker::computeProjectionError()
{
  if (computeProjError) {
    double rawTotalProjectionError = 0.0;
    unsigned int nbTotalFeaturesUsed = 0;
    for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
         it != m_mapOfEdgeTrackers.end(); ++it) {
      double curProjError = it->second->getProjectionError();
      unsigned int nbFeaturesUsed = it->second->nbFeaturesForProjErrorComputation;

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

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

void vpMbEdgeMultiTracker::computeVVS(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                                      const unsigned int lvl)
{
  computeVVSInit();
  unsigned int nbrow = m_error_edgeMulti.getRows();

  unsigned int iter = 0;
  // Parametre pour la premiere phase d'asservissement
  bool reloop = true;

  bool isoJoIdentity_ = isoJoIdentity; // Backup since it can be modified if L is not full rank

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

  //  std::cout << "\n\n\ncMo used before the first phase=\n" << cMo <<
  //  std::endl;

  /*** First phase ***/

  vpMbEdgeTracker *edge;
  while (reloop == true && iter < 10) {
    for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
         it != m_mapOfEdgeTrackers.end(); ++it) {
      it->second->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo;
    }

    double count = 0;
    reloop = false;

    unsigned int start_idx = 0;
    for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
         it != m_mapOfEdgeTrackers.end(); ++it) {
      double count_tmp = 0.0;
      edge = it->second;
      edge->computeVVSFirstPhase(*mapOfImages[it->first], iter, count_tmp, lvl);
      count += count_tmp;

      m_L_edgeMulti.insert(edge->m_L_edge * mapOfVelocityTwist[it->first], start_idx, 0);
      m_factor.insert(start_idx, edge->m_factor);
      m_w_edgeMulti.insert(start_idx, edge->m_w_edge);
      m_error_edgeMulti.insert(start_idx, edge->m_error_edge);

      start_idx += edge->m_error_edge.getRows();
    }

    count = count / (double)nbrow;
    if (count < 0.85) {
      reloop = true;
    }

    computeVVSFirstPhasePoseEstimation(iter, isoJoIdentity_);

    iter++;
  }

  //  std::cout << "\n\t First minimization in " << iter << " iteration give
  //  as initial cMo: \n" << cMo << std::endl; std::cout << "Residual=" <<
  //  m_error.sum() / m_error.size() << std::endl;

  /*** Second phase ***/

  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
       it != m_mapOfEdgeTrackers.end(); ++it) {
    edge = it->second;
    edge->m_w_edge = 1;
  }

  iter = 0;

  vpHomogeneousMatrix cMoPrev;
  vpColVector W_true(nbrow);
  vpMatrix L_true;
  vpMatrix LVJ_true;

  double mu = m_initialMu;
  vpColVector m_error_prev;
  vpColVector m_w_prev;

  double residu_1 = 1e3;
  double r = 1e3 - 1;

  // For computeVVSPoseEstimation
  vpColVector LTR;
  vpColVector v;
  vpMatrix LTL;

  // while ( ((int)((residu_1 - r)*1e8) != 0 )  && (iter<30))
  while (std::fabs((residu_1 - r) * 1e8) > std::numeric_limits<double>::epsilon() && (iter < m_maxIter)) {
    computeVVSInteractionMatrixAndResidu(mapOfImages, mapOfVelocityTwist);

    bool reStartFromLastIncrement = false;
    computeVVSCheckLevenbergMarquardt(iter, m_error_edgeMulti, m_error_prev, cMoPrev, mu, reStartFromLastIncrement,
                                      &m_w_prev);

    if (!reStartFromLastIncrement) {
      computeVVSWeights();

      L_true = m_L_edgeMulti;
      vpVelocityTwistMatrix cVo;

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

      double wi = 0.0, eri = 0.0;
      double num = 0.0, den = 0.0;
      if ((iter == 0) || m_computeInteraction) {
        for (unsigned int i = 0; i < nbrow; i++) {
          wi = m_w_edgeMulti[i] * m_factor[i];
          W_true[i] = wi;
          eri = m_error_edgeMulti[i];
          num += wi * vpMath::sqr(eri);
          den += wi;

          m_weightedError_edgeMulti[i] = wi * eri;

          for (unsigned int j = 0; j < 6; j++) {
            m_L_edgeMulti[i][j] = wi * m_L_edgeMulti[i][j];
          }
        }
      } else {
        for (unsigned int i = 0; i < nbrow; i++) {
          wi = m_w_edgeMulti[i] * m_factor[i];
          W_true[i] = wi;
          eri = m_error_edgeMulti[i];
          num += wi * vpMath::sqr(eri);
          den += wi;

          m_weightedError_edgeMulti[i] = wi * eri;
        }
      }

      residu_1 = r;
      r = sqrt(num / den); // Le critere d'arret prend en compte le poids

      computeVVSPoseEstimation(isoJoIdentity_, iter, m_L_edgeMulti, LTL, m_weightedError_edgeMulti, m_error_edgeMulti,
                               m_error_prev, LTR, mu, v, &m_w_edgeMulti, &m_w_prev);

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

    iter++;
  }

  // std::cout << "VVS estimate pose cMo:\n" << cMo << std::endl;

  computeCovarianceMatrixVVS(isoJoIdentity_, W_true, cMoPrev, L_true, LVJ_true, m_error_edgeMulti);

  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
       it != m_mapOfEdgeTrackers.end(); ++it) {
    edge = it->second;
    edge->updateMovingEdgeWeights();
  }
}

void vpMbEdgeMultiTracker::computeVVSFirstPhasePoseEstimation(const unsigned int iter, bool &isoJoIdentity_)
{
  unsigned int nerror = m_weightedError_edgeMulti.getRows();

  double wi, eri;
  if ((iter == 0) || m_computeInteraction) {
    for (unsigned int i = 0; i < nerror; i++) {
      wi = m_w_edgeMulti[i] * m_factor[i];
      eri = m_error_edgeMulti[i];

      m_weightedError_edgeMulti[i] = wi * eri;

      for (unsigned int j = 0; j < 6; j++) {
        m_L_edgeMulti[i][j] = wi * m_L_edgeMulti[i][j];
      }
    }
  } else {
    for (unsigned int i = 0; i < nerror; i++) {
      wi = m_w_edgeMulti[i] * m_factor[i];
      eri = m_error_edgeMulti[i];

      m_weightedError_edgeMulti[i] = wi * eri;
    }
  }

  vpVelocityTwistMatrix cVo;

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

    vpMatrix K; // kernel
    unsigned int rank = (m_L_edgeMulti * cVo).kernel(K);
    if (rank == 0) {
      throw vpException(vpException::fatalError, "Rank=0, cannot estimate the pose !");
    }
    if (rank != 6) {
      vpMatrix I; // Identity
      I.eye(6);
      oJo = I - K.AtA();

      isoJoIdentity_ = false;
    }
  }

  vpColVector v;
  vpMatrix LTL;
  vpColVector LTR;

  if (isoJoIdentity_) {
    LTL = m_L_edgeMulti.AtA();
    computeJTR(m_L_edgeMulti, m_weightedError_edgeMulti, LTR);
    v = -0.7 * LTL.pseudoInverse(LTL.getRows() * std::numeric_limits<double>::epsilon()) * LTR;
  } else {
    cVo.buildFrom(cMo);
    vpMatrix LVJ = (m_L_edgeMulti * cVo * oJo);
    vpMatrix LVJTLVJ = (LVJ).AtA();
    vpColVector LVJTR;
    computeJTR(LVJ, m_weightedError_edgeMulti, LVJTR);
    v = -0.7 * LVJTLVJ.pseudoInverse(LVJTLVJ.getRows() * std::numeric_limits<double>::epsilon()) * LVJTR;
    v = cVo * v;
  }

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

void vpMbEdgeMultiTracker::computeVVSInit()
{
  unsigned int nbrow = 0;

  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
       it != m_mapOfEdgeTrackers.end(); ++it) {
    vpMbEdgeTracker *edge = it->second;

    try {
      edge->computeVVSInit();
      nbrow += edge->m_error_edge.getRows();
    } catch (...) {
    }
  }

  if (nbrow < 4) {
    throw vpTrackingException(vpTrackingException::notEnoughPointError,
                              "No data found to compute the interaction matrix...");
  }

  // Initialize with correct size
  m_L_edgeMulti.resize(nbrow, 6, false, false);
  m_w_edgeMulti.resize(nbrow, false);
  m_error_edgeMulti.resize(nbrow, false);
  m_weightedError_edgeMulti.resize(nbrow, false);
  m_factor.resize(nbrow, false);
}

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

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

  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
       it != m_mapOfEdgeTrackers.end(); ++it) {
    vpMbEdgeTracker *edge = it->second;
    edge->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo;

    edge->computeVVSInteractionMatrixAndResidu(*mapOfImages[it->first]);

    m_L_edgeMulti.insert(edge->m_L_edge * mapOfVelocityTwist[it->first], start_idx, 0);
    m_error_edgeMulti.insert(start_idx, edge->m_error_edge);

    start_idx += edge->m_error_edge.getRows();
  }
}

void vpMbEdgeMultiTracker::computeVVSWeights()
{
  unsigned int start_idx = 0;

  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
       it != m_mapOfEdgeTrackers.end(); ++it) {
    vpMbEdgeTracker *edge = it->second;

    // Compute weights
    edge->computeVVSWeights();

    m_w_edgeMulti.insert(start_idx, edge->m_w_edge);
    start_idx += edge->m_w_edge.getRows();
  }
}

void vpMbEdgeMultiTracker::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, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName);
  if (it != m_mapOfEdgeTrackers.end()) {
    it->second->display(I, cMo_, cam_, col, thickness, displayFullModel);
  } else {
    std::cerr << "Cannot find reference camera: " << m_referenceCameraName << " !" << std::endl;
  }
}

void vpMbEdgeMultiTracker::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo_,
                                   const vpCameraParameters &cam_, const vpColor &col, const unsigned int thickness,
                                   const bool displayFullModel)
{

  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName);
  if (it != m_mapOfEdgeTrackers.end()) {
    it->second->display(I, cMo_, cam_, col, thickness, displayFullModel);
  } else {
    std::cerr << "Cannot find reference camera: " << m_referenceCameraName << " !" << std::endl;
  }
}

void vpMbEdgeMultiTracker::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 &col,
                                   const unsigned int thickness, const bool displayFullModel)
{

  if (m_mapOfEdgeTrackers.size() == 2) {
    std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
    it->second->display(I1, c1Mo, cam1, col, thickness, displayFullModel);
    ++it;

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

void vpMbEdgeMultiTracker::display(const vpImage<vpRGBa> &I1, const vpImage<vpRGBa> &I2,
                                   const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
                                   const vpCameraParameters &cam1, const vpCameraParameters &cam2, const vpColor &col,
                                   const unsigned int thickness, const bool displayFullModel)
{

  if (m_mapOfEdgeTrackers.size() == 2) {
    std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
    it->second->display(I1, c1Mo, cam1, col, thickness, displayFullModel);
    ++it;

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

void vpMbEdgeMultiTracker::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, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.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_edge != m_mapOfEdgeTrackers.end() && it_camPose != mapOfCameraPoses.end() &&
        it_cam != mapOfCameraParameters.end()) {
      it_edge->second->display(*it_img->second, it_camPose->second, it_cam->second, col, thickness, displayFullModel);
    } else {
      std::cerr << "Missing elements !" << std::endl;
    }
  }
}

void vpMbEdgeMultiTracker::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, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.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_edge != m_mapOfEdgeTrackers.end() && it_camPose != mapOfCameraPoses.end() &&
        it_cam != mapOfCameraParameters.end()) {
      it_edge->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> vpMbEdgeMultiTracker::getCameraNames() const
{
  std::vector<std::string> cameraNames;

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

  return cameraNames;
}

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

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

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

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

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

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

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

  return vpMbTracker::getClipping();
}

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

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

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

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

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

  return mapOfFaces;
}

void vpMbEdgeMultiTracker::getLcircle(std::list<vpMbtDistanceCircle *> &circlesList, const unsigned int level) const
{
  std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);

  if (it_edge != m_mapOfEdgeTrackers.end()) {
    it_edge->second->getLcircle(circlesList, level);
  } else {
    std::cerr << "Cannot find reference camera: " << m_referenceCameraName << " !" << std::endl;
  }
}

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

void vpMbEdgeMultiTracker::getLcylinder(std::list<vpMbtDistanceCylinder *> &cylindersList,
                                        const unsigned int level) const
{
  std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);

  if (it_edge != m_mapOfEdgeTrackers.end()) {
    it_edge->second->getLcylinder(cylindersList, level);
  } else {
    std::cerr << "Cannot find reference camera: " << m_referenceCameraName << " !" << std::endl;
  }
}

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

void vpMbEdgeMultiTracker::getLline(std::list<vpMbtDistanceLine *> &linesList, const unsigned int level) const
{
  std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);

  if (it_edge != m_mapOfEdgeTrackers.end()) {
    it_edge->second->getLline(linesList, level);
  } else {
    std::cerr << "Cannot find reference camera: " << m_referenceCameraName << " !" << std::endl;
  }
}

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

void vpMbEdgeMultiTracker::getMovingEdge(vpMe &p_me) const
{
  std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);
  if (it_edge != m_mapOfEdgeTrackers.end()) {
    it_edge->second->getMovingEdge(p_me);
  } else {
    std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist !" << std::endl;
  }
}

vpMe vpMbEdgeMultiTracker::getMovingEdge() const
{
  std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);
  vpMe me_tmp;
  if (it_edge != m_mapOfEdgeTrackers.end()) {
    it_edge->second->getMovingEdge(me_tmp);
  } else {
    std::cerr << "The reference camera: " << m_referenceCameraName << " does not exist !" << std::endl;
  }

  return me_tmp;
}

void vpMbEdgeMultiTracker::getMovingEdge(const std::string &cameraName, vpMe &p_me) const
{
  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(cameraName);
  if (it != m_mapOfEdgeTrackers.end()) {
    it->second->getMovingEdge(p_me);
  } else {
    std::cerr << "The camera: " << cameraName << " does not exist !" << std::endl;
  }
}

vpMe vpMbEdgeMultiTracker::getMovingEdge(const std::string &cameraName) const
{
  vpMe me_tmp;
  getMovingEdge(cameraName, me_tmp);
  return me_tmp;
}

unsigned int vpMbEdgeMultiTracker::getNbPoints(const unsigned int level) const
{
  std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);

  unsigned int nbGoodPoints = 0;
  if (it_edge != m_mapOfEdgeTrackers.end()) {

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

  return nbGoodPoints;
}

unsigned int vpMbEdgeMultiTracker::getNbPoints(const std::string &cameraName, const unsigned int level) const
{
  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(cameraName);
  if (it != m_mapOfEdgeTrackers.end()) {
    return it->second->getNbPoints(level);
  } else {
    std::cerr << "The camera: " << cameraName << " does not exist !" << std::endl;
  }

  return 0;
}

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

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

unsigned int vpMbEdgeMultiTracker::getNbPolygon(const std::string &cameraName) const
{
  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(cameraName);
  if (it != m_mapOfEdgeTrackers.end()) {
    return it->second->getNbPolygon();
  }

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

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

  return mapOfNbPolygons;
}

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

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

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

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

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

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

#ifdef VISP_HAVE_MODULE_GUI

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

void vpMbEdgeMultiTracker::initClick(const vpImage<unsigned char> &I, const std::string &initFile,
                                     const bool displayHelp)
{
  if (m_mapOfEdgeTrackers.empty()) {
    throw vpException(vpTrackingException::initializationError, "There is no camera !");
  } else if (m_mapOfEdgeTrackers.size() > 1) {
    throw vpException(vpTrackingException::initializationError, "There is more than one camera !");
  } else {
    // Get the vpMbEdgeTracker object for the reference camera name
    std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName);
    if (it != m_mapOfEdgeTrackers.end()) {
      it->second->initClick(I, initFile, displayHelp);
      it->second->getPose(cMo);
    } else {
      std::stringstream ss;
      ss << "Cannot initClick as the reference camera: " << m_referenceCameraName << " does not exist !";
      throw vpException(vpTrackingException::initializationError, ss.str());
    }
  }
}

void vpMbEdgeMultiTracker::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_mapOfEdgeTrackers.size() == 2) {
    std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.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);
    }

    ++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);
    }
  } else {
    std::stringstream ss;
    ss << "Cannot init click ! Require two cameras but there are " << m_mapOfEdgeTrackers.size() << " cameras !";
    throw vpException(vpTrackingException::initializationError, ss.str());
  }
}

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

    if (it_img != mapOfImages.end()) {
      // Init click on reference camera
      it_edge->second->initClick(*it_img->second, initFile, displayHelp);

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

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

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

void vpMbEdgeMultiTracker::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, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.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_edge != m_mapOfEdgeTrackers.end() && it_img != mapOfImages.end() && it_initFile != mapOfInitFiles.end()) {
    // InitClick for the reference camera
    it_edge->second->initClick(*it_img->second, it_initFile->second, displayHelp);

    // Get reference camera pose
    it_edge->second->getPose(cMo);
  } else {
    throw vpException(vpTrackingException::initializationError, "Cannot initClick for the reference camera !");
  }

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

  // InitClick for all the cameras that have an initFile
  for (it_edge = m_mapOfEdgeTrackers.begin(); it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) {
    if (it_edge->first != m_referenceCameraName) {
      it_img = mapOfImages.find(it_edge->first);
      it_initFile = mapOfInitFiles.find(it_edge->first);

      if (it_img != mapOfImages.end() && it_initFile != mapOfInitFiles.end()) {
        it_edge->second->initClick(*it_img->second, it_initFile->second, displayHelp);
      } else {
        vectorOfMissingCameraPoses.push_back(it_edge->first);
      }
    }
  }

  // SetPose 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_mapOfEdgeTrackers[*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::initializationError, ss.str());
    }
  }
}
#endif //#ifdef VISP_HAVE_MODULE_GUI

void vpMbEdgeMultiTracker::initFromPose(const vpImage<unsigned char> &I, const std::string &initFile)
{
  // Monocular case only !
  if (m_mapOfEdgeTrackers.size() > 1) {
    throw vpException(vpTrackingException::initializationError,
                      "This method 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);

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

  it_ref->second->cMo = cMo;
  it_ref->second->init(I);
}

void vpMbEdgeMultiTracker::initFromPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo_)
{
  if (m_mapOfEdgeTrackers.size() != 1) {
    std::stringstream ss;
    ss << "This method requires exactly one camera, there are " << m_mapOfEdgeTrackers.size() << " cameras !";
    throw vpException(vpTrackingException::initializationError, ss.str());
  }

  this->cMo = cMo_;

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

  it_ref->second->cMo = cMo;
  it_ref->second->init(I);
}

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

void vpMbEdgeMultiTracker::initFromPose(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
                                        const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
                                        const bool firstCameraIsReference)
{
  // For Edge, initFromPose has the same behavior than setPose
  // So, for convenience we call setPose
  vpMbEdgeMultiTracker::setPose(I1, I2, c1Mo, c2Mo, firstCameraIsReference);
}

void vpMbEdgeMultiTracker::initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                                        const vpHomogeneousMatrix &cMo_)
{
  // For Edge, initFromPose has the same behavior than setPose
  // So, for convenience we call setPose
  vpMbEdgeMultiTracker::setPose(mapOfImages, cMo_);
}

void vpMbEdgeMultiTracker::initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                                        const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
{
  // For Edge, initFromPose has the same behavior than setPose
  // So, for convenience we call setPose
  vpMbEdgeMultiTracker::setPose(mapOfImages, mapOfCameraPoses);
}

void vpMbEdgeMultiTracker::initPyramid(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                                       std::map<std::string, std::vector<const vpImage<unsigned char> *> > &pyramid)
{
  for (std::map<std::string, const vpImage<unsigned char> *>::const_iterator it = mapOfImages.begin();
       it != mapOfImages.end(); ++it) {
    pyramid[it->first].resize(scales.size());

    vpMbEdgeTracker::initPyramid(*it->second, pyramid[it->first]);
  }
}

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

    // Set Moving Edge parameters
    this->me = it->second->getMovingEdge();

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

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

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

      // Set Moving Edge parameters
      this->me = it->second->getMovingEdge();

      // 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 Moving Edge parameters
      this->me = it->second->getMovingEdge();

      // 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_mapOfEdgeTrackers.size() << " cameras !";
    throw vpException(vpTrackingException::initializationError, ss.str());
  }
}

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

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

    // Set Moving Edge parameters
    this->me = it->second->getMovingEdge();

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

void vpMbEdgeMultiTracker::loadModel(const std::string &modelFile, const bool verbose)
{
  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
       it != m_mapOfEdgeTrackers.end(); ++it) {
    it->second->loadModel(modelFile, verbose);
  }

  modelInitialised = true;
}

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

  std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);
  if (it_edge != m_mapOfEdgeTrackers.end()) {
    it_edge->second->reInitModel(I, cad_name, cMo_, verbose);

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

    modelInitialised = true;
  }
}

void vpMbEdgeMultiTracker::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_mapOfEdgeTrackers.size() == 2) {
    std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.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);
    }
  } else {
    throw vpException(vpTrackingException::fatalError, "This method requires exactly two cameras !");
  }
}

void vpMbEdgeMultiTracker::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, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.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_edge != m_mapOfEdgeTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
    it_edge->second->reInitModel(*it_img->second, cad_name, it_camPose->second, verbose);
    modelInitialised = true;

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

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

      if (it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
        it_edge->second->reInitModel(*it_img->second, cad_name, it_camPose->second, verbose);
      } else {
        vectorOfMissingCameras.push_back(it_edge->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_mapOfEdgeTrackers[*it]->reInitModel(*it_img->second, cad_name, cCurrentMo, verbose);
    }
  }
}

void vpMbEdgeMultiTracker::resetTracker()
{
  this->cMo.eye();

  // Reset all internal trackers
  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
       it != m_mapOfEdgeTrackers.end(); ++it) {
    it->second->resetTracker();
  }

  useScanLine = false;

#ifdef VISP_HAVE_OGRE
  useOgre = false;
#endif

  m_computeInteraction = true;
  //  nline = 0; //Not used in vpMbEdgeMultiTracker class
  //  ncylinder = 0; //Not used in vpMbEdgeMultiTracker class
  m_lambda = 1.0;
  //  nbvisiblepolygone = 0; //Not used in vpMbEdgeMultiTracker class
  percentageGdPt = 0.4;

  angleAppears = vpMath::rad(89);
  angleDisappears = vpMath::rad(89);
  clippingFlag = vpPolygon3D::NO_CLIPPING;

  m_optimizationMethod = vpMbTracker::GAUSS_NEWTON_OPT;

  // reinitialization of the scales.
  this->setScales(scales);
}

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

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

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

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

void vpMbEdgeMultiTracker::setCameraParameters(const vpCameraParameters &camera)
{
  if (m_mapOfEdgeTrackers.empty()) {
    throw vpException(vpTrackingException::fatalError, "There is no camera !");
  } else if (m_mapOfEdgeTrackers.size() > 1) {
    throw vpException(vpTrackingException::fatalError, "There is more than one camera !");
  } else {
    std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName);
    if (it != m_mapOfEdgeTrackers.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::initializationError, ss.str());
    }
  }
}

void vpMbEdgeMultiTracker::setCameraParameters(const vpCameraParameters &camera1, const vpCameraParameters &camera2,
                                               const bool firstCameraIsReference)
{
  if (m_mapOfEdgeTrackers.empty()) {
    throw vpException(vpTrackingException::fatalError, "There is no camera !");
  } else if (m_mapOfEdgeTrackers.size() == 2) {
    std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.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_mapOfEdgeTrackers.size() << " cameras !";
    throw vpException(vpTrackingException::initializationError, ss.str());
  }
}

void vpMbEdgeMultiTracker::setCameraParameters(const std::string &cameraName, const vpCameraParameters &camera)
{
  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(cameraName);
  if (it != m_mapOfEdgeTrackers.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::initializationError, ss.str());
  }
}

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

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

void vpMbEdgeMultiTracker::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::initializationError, ss.str());
  }
}

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

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

  m_mapOfCameraTransformationMatrix = mapOfTransformationMatrix;
}

void vpMbEdgeMultiTracker::setClipping(const unsigned int &flags)
{
  // Set clipping for vpMbEdgeMultiTracker class
  vpMbTracker::setClipping(flags);

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

void vpMbEdgeMultiTracker::setClipping(const std::string &cameraName, const unsigned int &flags)
{
  // Set clipping for the given camera, do not change the clipping for
  // vpMbEdgeMultiTracker class
  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(cameraName);
  if (it != m_mapOfEdgeTrackers.end()) {
    it->second->setClipping(flags);
  } else {
    std::cerr << "Camera: " << cameraName << " does not exist !" << std::endl;
  }
}

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

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

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

  displayFeatures = displayF;
}

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

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

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

void vpMbEdgeMultiTracker::setGoodMovingEdgesRatioThreshold(const double threshold)
{
  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
       it != m_mapOfEdgeTrackers.end(); ++it) {
    it->second->setGoodMovingEdgesRatioThreshold(threshold);
  }

  percentageGdPt = threshold;
}

#ifdef VISP_HAVE_OGRE

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

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

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

void vpMbEdgeMultiTracker::setLod(const bool useLod, const std::string &cameraName, const std::string &name)
{
  std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(cameraName);

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

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

void vpMbEdgeMultiTracker::setMinLineLengthThresh(const double minLineLengthThresh, const std::string &cameraName,
                                                  const std::string &name)
{
  std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(cameraName);

  if (it_edge != m_mapOfEdgeTrackers.end()) {
    it_edge->second->setMinLineLengthThresh(minLineLengthThresh, name);
  } else {
    std::cerr << "The camera: " << cameraName << " cannot be found !" << std::endl;
  }
}

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

void vpMbEdgeMultiTracker::setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &cameraName,
                                                   const std::string &name)
{
  std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(cameraName);

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

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

void vpMbEdgeMultiTracker::setMovingEdge(const std::string &cameraName, const vpMe &me)
{
  std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(cameraName);
  if (it != m_mapOfEdgeTrackers.end()) {
    it->second->setMovingEdge(me);
  } else {
    std::cerr << "Camera: " << cameraName << " does not exist !" << std::endl;
  }
}

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

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

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

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

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

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

  useOgre = v;
}

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

  m_optimizationMethod = opt;
}

void vpMbEdgeMultiTracker::setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo_)
{
  if (m_mapOfEdgeTrackers.size() == 1) {
    std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName);
    if (it != m_mapOfEdgeTrackers.end()) {
      it->second->setPose(I, cMo_);
      this->cMo = cMo_;
    } 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 vpMbEdgeMultiTracker::setPose(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
                                   const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
                                   const bool firstCameraIsReference)
{
  if (m_mapOfEdgeTrackers.size() == 2) {
    std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
    it->second->setPose(I1, c1Mo);

    ++it;

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

    if (firstCameraIsReference) {
      this->cMo = c1Mo;
    } else {
      this->cMo = c2Mo;
    }
  } else {
    std::stringstream ss;
    ss << "This method requires 2 cameras but there are " << m_mapOfEdgeTrackers.size() << " cameras !";
    throw vpException(vpTrackingException::fatalError, ss.str());
  }
}

void vpMbEdgeMultiTracker::setPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                                   const vpHomogeneousMatrix &cMo_)
{
  std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);
  if (it_edge != m_mapOfEdgeTrackers.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_edge->second->setPose(*it_img->second, cMo_);

      // Set the reference cMo
      cMo = cMo_;

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

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

void vpMbEdgeMultiTracker::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, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.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_edge != m_mapOfEdgeTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
    it_edge->second->setPose(*it_img->second, it_camPose->second);
    it_edge->second->getPose(cMo);
  } else {
    throw vpException(vpTrackingException::fatalError, "Cannot set pose for the reference camera !");
  }

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

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

      if (it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
        // Set pose
        it_edge->second->setPose(*it_img->second, it_camPose->second);
      } else {
        vectorOfMissingCameraPoses.push_back(it_edge->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_mapOfEdgeTrackers[*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());
    }
  }
}

void vpMbEdgeMultiTracker::setProjectionErrorComputation(const bool &flag)
{
  // Set the flag in the current class
  vpMbTracker::setProjectionErrorComputation(flag);

  // Set the flag for each camera
  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
       it != m_mapOfEdgeTrackers.end(); ++it) {
    it->second->setProjectionErrorComputation(flag);
  }
}

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

void vpMbEdgeMultiTracker::setScales(const std::vector<bool> &scale)
{
  vpMbEdgeTracker::setScales(scale);
  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
       it != m_mapOfEdgeTrackers.end(); ++it) {
    it->second->setScales(scale);
  }
}

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

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

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

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

  if (it != m_mapOfEdgeTrackers.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());
  }

  if (computeProjError) {
    // Use the projection error computed by the single vpMbEdgeTracker in
    // m_mapOfEdgeTrackers
    projectionError = it->second->getProjectionError();
  }
}

void vpMbEdgeMultiTracker::track(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2)
{
  // Track with the special case of stereo cameras
  if (m_mapOfEdgeTrackers.size() == 2) {
    std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.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_mapOfEdgeTrackers.size() << " cameras !";
    throw vpException(vpTrackingException::fatalError, ss.str());
  }
}

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

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

  initPyramid(mapOfImages, m_mapOfPyramidalImages);

  unsigned int lvl = (unsigned int)scales.size();
  do {
    lvl--;

    projectionError = 90.0;

    if (scales[lvl]) {
      vpHomogeneousMatrix cMo_1 = cMo;
      try {
        downScale(lvl);
        for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it1 = m_mapOfEdgeTrackers.begin();
             it1 != m_mapOfEdgeTrackers.end(); ++it1) {
          // Downscale for each camera
          it1->second->downScale(lvl);

          // Track moving edges
          try {
            it1->second->trackMovingEdge(*m_mapOfPyramidalImages[it1->first][lvl]);
          } catch (...) {
            vpTRACE("Error in moving edge tracking");
            throw;
          }
        }

        try {
          std::map<std::string, const vpImage<unsigned char> *> mapOfPyramidImages;
          for (std::map<std::string, std::vector<const vpImage<unsigned char> *> >::const_iterator it =
                   m_mapOfPyramidalImages.begin();
               it != m_mapOfPyramidalImages.end(); ++it) {
            mapOfPyramidImages[it->first] = it->second[lvl];
          }

          computeVVS(mapOfPyramidImages, lvl);
        } catch (...) {
          covarianceMatrix = -1;
          throw; // throw the original exception
        }

        // Test tracking failed only if all testTracking failed
        bool isOneTestTrackingOk = false;
        for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
             it != m_mapOfEdgeTrackers.end(); ++it) {
          // Set the camera pose
          it->second->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo;

          try {
            it->second->testTracking();
            isOneTestTrackingOk = true;
          } catch (/*vpException &e*/...) {
            //      throw e;
          }
        }

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

        if (displayFeatures) {
          for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
               it != m_mapOfEdgeTrackers.end(); ++it) {
            it->second->displayFeaturesOnImage(*mapOfImages[it->first], lvl);
          }
        }

        // Looking for new visible face
        bool newvisibleface = false;
        for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
             it != m_mapOfEdgeTrackers.end(); ++it) {
          it->second->visibleFace(*mapOfImages[it->first], it->second->cMo, newvisibleface);
        }

        if (useScanLine) {
          for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
               it != m_mapOfEdgeTrackers.end(); ++it) {
            it->second->faces.computeClippedPolygons(it->second->cMo, it->second->cam);
            it->second->faces.computeScanLineRender(it->second->cam, mapOfImages[it->first]->getWidth(),
                                                    mapOfImages[it->first]->getHeight());
          }
        }

        try {
          for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
               it != m_mapOfEdgeTrackers.end(); ++it) {
            it->second->updateMovingEdge(*mapOfImages[it->first]);
          }
        } catch (...) {
          throw; // throw the original exception
        }

        for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
             it != m_mapOfEdgeTrackers.end(); ++it) {
          it->second->initMovingEdge(*mapOfImages[it->first], it->second->cMo);

          // Reinit the moving edge for the lines which need it.
          it->second->reinitMovingEdge(*mapOfImages[it->first], it->second->cMo);

          if (computeProjError) {
            // Compute the projection error
            it->second->computeProjectionError(*mapOfImages[it->first]);
          }
        }

        computeProjectionError();

        upScale(lvl);
        for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
             it != m_mapOfEdgeTrackers.end(); ++it) {
          it->second->upScale(lvl);
        }
      } catch (vpException &e) {
        if (lvl != 0) {
          cMo = cMo_1;
          reInitLevel(lvl);
          upScale(lvl);

          for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
               it != m_mapOfEdgeTrackers.end(); ++it) {
            it->second->cMo = cMo_1;
            it->second->reInitLevel(lvl);
            it->second->upScale(lvl);
          }
        } else {
          upScale(lvl);
          for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
               it != m_mapOfEdgeTrackers.end(); ++it) {
            it->second->upScale(lvl);
          }
          throw(e);
        }
      }
    }
  } while (lvl != 0);

  cleanPyramid(m_mapOfPyramidalImages);
}