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
 * ("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 edge tracker with multiple cameras.
 *
 * Authors:
 * Souriya Trinh
 *
 *****************************************************************************/

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


vpMbEdgeMultiTracker::vpMbEdgeMultiTracker() : m_mapOfCameraTransformationMatrix(), m_mapOfEdgeTrackers(),
    m_mapOfPyramidalImages(), m_referenceCameraName("Camera") {
  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") {

  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") {

  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) {
  //Number of moving edges
  unsigned int nbrow = 0;
  //unsigned int nberrors_lines = 0;
  //unsigned int nberrors_cylinders = 0;
  //unsigned int nberrors_circles = 0;

  std::vector<FeatureType> indexOfFeatures;
  std::map<std::string, unsigned int> mapOfNumberOfRows;
  std::map<std::string, unsigned int> mapOfNumberOfLines;
  std::map<std::string, unsigned int> mapOfNumberOfCylinders;
  std::map<std::string, unsigned int> mapOfNumberOfCircles;

  for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it1 = m_mapOfEdgeTrackers.begin();
      it1 != m_mapOfEdgeTrackers.end(); ++it1) {
    unsigned int nrows = 0;
    unsigned int nlines = 0;
    unsigned int ncylinders = 0;
    unsigned int ncircles = 0;

    nrows = it1->second->initMbtTracking(nlines, ncylinders, ncircles);

    mapOfNumberOfRows[it1->first] = nrows;
    mapOfNumberOfLines[it1->first] = nlines;
    mapOfNumberOfCylinders[it1->first] = ncylinders;
    mapOfNumberOfCircles[it1->first] = ncircles;

    nbrow += nrows;
    //nberrors_lines += nlines;
    //nberrors_cylinders += ncylinders;
    //nberrors_circles += ncircles;

    for(unsigned int i = 0; i < nlines; i++) {
      indexOfFeatures.push_back(LINE);
    }

    for(unsigned int i = 0; i < ncylinders; i++) {
      indexOfFeatures.push_back(CYLINDER);
    }

    for(unsigned int i = 0; i < ncircles; i++) {
      indexOfFeatures.push_back(CIRCLE);
    }
  }

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

  vpMatrix L;

  // compute the error vector
  m_error.resize(nbrow);
  unsigned int nerror = m_error.getRows();
  vpColVector v;

//  double limite = 3; //Une limite de 3 pixels
//  limite = limite / cam.get_px(); //Transformation limite pixel en limite metre.
  unsigned int iter = 0;
  vpColVector weighted_error;
  vpColVector factor;
  std::map<std::string, vpColVector> mapOfFactors;

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

  while(reloop == true && iter < 10)
  {
    m_w.resize(0);
    m_error.resize(0);

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

    if(iter == 0)
    {
      weighted_error.resize(nerror);

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

        it->second->m_error.resize(mapOfNumberOfRows[it->first]);

        mapOfFactors[it->first].resize(mapOfNumberOfRows[it->first]);
        mapOfFactors[it->first] = 1;
      }
    }

    double count = 0;
    reloop = false;

    L = vpMatrix();
    factor = vpColVector();


    for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
        it != m_mapOfEdgeTrackers.end(); ++it) {
      vpMatrix L_tmp(mapOfNumberOfRows[it->first], 6);

      double count_tmp = 0.0;
      it->second->computeVVSFirstPhase(*mapOfImages[it->first], iter, L_tmp, mapOfFactors[it->first], count_tmp,
          it->second->m_error, it->second->m_w, lvl);
      count += count_tmp;

      L_tmp = L_tmp*mapOfVelocityTwist[it->first];

      L.stack(L_tmp);
      factor.stack(mapOfFactors[it->first]);
      m_w.stack(it->second->m_w);
      m_error.stack(it->second->m_error);
    }

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

    computeVVSFirstPhasePoseEstimation(nerror, iter, factor, weighted_error, L, 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 ***/

  //Variables for individual weights
  std::map<std::string, vpRobust> mapOfRobustLines;
  std::map<std::string, vpRobust> mapOfRobustCylinders;
  std::map<std::string, vpRobust> mapOfRobustCircles;
  //Init map of robust
  for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
      it != m_mapOfEdgeTrackers.end(); ++it) {
    mapOfRobustLines[it->first] = vpRobust(mapOfNumberOfLines[it->first]);
    mapOfRobustLines[it->first].setIteration(0);

    mapOfRobustCylinders[it->first] = vpRobust(mapOfNumberOfCylinders[it->first]);
    mapOfRobustCylinders[it->first].setIteration(0);

    mapOfRobustCircles[it->first] = vpRobust(mapOfNumberOfCircles[it->first]);
    mapOfRobustCircles[it->first].setIteration(0);
  }

  vpColVector w_lines;
  vpColVector w_cylinders;
  vpColVector w_circles;

  std::map<std::string, vpColVector> mapOfWeightLines;
  std::map<std::string, vpColVector> mapOfWeightCylinders;
  std::map<std::string, vpColVector> mapOfWeightCircles;

  iter = 0;

  vpColVector error_lines;
  vpColVector error_cylinders;
  vpColVector error_circles;

  vpHomogeneousMatrix cMoPrev;
  vpColVector W_true;
  vpMatrix L_true;
  vpMatrix LVJ_true;

  double mu = 0.01;
  vpColVector m_error_prev(nbrow);
  vpColVector m_w_prev(nbrow);

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

  //while ( ((int)((residu_1 - r)*1e8) != 0 )  && (iter<30))
  while(std::fabs((residu_1 - r)*1e8) > std::numeric_limits<double>::epsilon() && (iter<30))
  {
    L.resize(0,0);
    m_error.resize(0);

    error_lines.resize(0);
    error_cylinders.resize(0);
    error_circles.resize(0);

    std::map<std::string, vpColVector> mapOfErrorLines;
    std::map<std::string, vpColVector> mapOfErrorCylinders;
    std::map<std::string, vpColVector> mapOfErrorCircles;

    for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
        it != m_mapOfEdgeTrackers.end(); ++it) {
      vpMatrix L_tmp(mapOfNumberOfRows[it->first], 6);
      vpColVector error_lines_tmp(mapOfNumberOfLines[it->first]);
      vpColVector error_cylinders_tmp(mapOfNumberOfCylinders[it->first]);
      vpColVector error_circles_tmp(mapOfNumberOfCircles[it->first]);

      it->second->cMo = m_mapOfCameraTransformationMatrix[it->first]*cMo;

      vpColVector error_tmp;
      error_tmp.resize(mapOfNumberOfRows[it->first]);

      it->second->computeVVSSecondPhase(*mapOfImages[it->first], L_tmp, error_lines_tmp,
          error_cylinders_tmp, error_circles_tmp, error_tmp, lvl);
      L_tmp = L_tmp*mapOfVelocityTwist[it->first];

      L.stack(L_tmp);
      m_error.stack(error_tmp);

      error_lines.stack(error_lines_tmp);
      error_cylinders.stack(error_cylinders_tmp);
      error_circles.stack(error_circles_tmp);

      mapOfErrorLines[it->first] = error_lines_tmp;
      mapOfErrorCylinders[it->first] = error_cylinders_tmp;
      mapOfErrorCircles[it->first] = error_circles_tmp;
    }

    bool reStartFromLastIncrement = false;
    computeVVSSecondPhaseCheckLevenbergMarquardt(iter, nbrow, m_error_prev, m_w_prev, cMoPrev, mu, reStartFromLastIncrement);

    if(!reStartFromLastIncrement) {
      w_lines.resize(0);
      w_cylinders.resize(0);
      w_circles.resize(0);

      computeVVSSecondPhaseWeights(iter, nerror, weighted_error, w_lines, w_cylinders, w_circles, mapOfNumberOfLines,
          mapOfNumberOfCylinders, mapOfNumberOfCircles, mapOfWeightLines, mapOfWeightCylinders, mapOfWeightCircles,
          mapOfErrorLines, mapOfErrorCylinders, mapOfErrorCircles, mapOfRobustLines, mapOfRobustCylinders,
          mapOfRobustCircles, 2.0);

      if(iter == 0) {
        m_w.resize(nerror);
        m_w = 1;
      }

      for(unsigned int cpt = 0, cpt_lines = 0, cpt_cylinders = 0, cpt_circles = 0; cpt < nbrow; cpt++) {
        switch(indexOfFeatures[cpt]) {
        case LINE:
          m_w[cpt] = w_lines[cpt_lines];
          cpt_lines++;
          break;

        case CYLINDER:
          m_w[cpt] = w_cylinders[cpt_cylinders];
          cpt_cylinders++;
          break;

        case CIRCLE:
          m_w[cpt] = w_circles[cpt_circles];
          cpt_circles++;
          break;

        default:
          std::cerr << "Problem with feature type !" << std::endl;
          break;
        }
      }

      computeVVSSecondPhasePoseEstimation(nerror, L, L_true, LVJ_true, W_true, factor, iter, isoJoIdentity_,
          weighted_error, mu, m_error_prev, m_w_prev, cMoPrev, residu_1, r);
    }

    iter++;
  }

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

  if(computeCovariance){
    vpMatrix D;
    D.diag(W_true);

    // Note that here the covariance is computed on cMoPrev for time computation efficiency
    if(isoJoIdentity_){
        covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev, m_error, L_true, D);
    }
    else{
        covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev, m_error, LVJ_true, D);
    }
  }

  unsigned int cpt = 0;
  for(std::map<std::string, unsigned int>::const_iterator it = mapOfNumberOfRows.begin(); it != mapOfNumberOfRows.end(); ++it) {
    for(unsigned int i = 0; i < mapOfNumberOfRows[it->first]; i++) {
      m_mapOfEdgeTrackers[it->first]->m_w[i] = m_w[i+cpt];
    }
    m_mapOfEdgeTrackers[it->first]->updateMovingEdgeWeights();
    cpt += mapOfNumberOfRows[it->first];
  }
}

void vpMbEdgeMultiTracker::computeVVSSecondPhaseWeights(const unsigned int iter, const unsigned int nerror,
    vpColVector &weighted_error, vpColVector &w_lines, vpColVector &w_cylinders, vpColVector &w_circles,
    std::map<std::string, unsigned int> &mapOfNumberOfLines,
    std::map<std::string, unsigned int> &mapOfNumberOfCylinders, std::map<std::string, unsigned int> &mapOfNumberOfCircles,
    std::map<std::string, vpColVector> &mapOfWeightLines, std::map<std::string, vpColVector> &mapOfWeightCylinders,
    std::map<std::string, vpColVector> &mapOfWeightCircles, std::map<std::string, vpColVector> &mapOfErrorLines,
    std::map<std::string, vpColVector> &mapOfErrorCylinders, std::map<std::string, vpColVector> &mapOfErrorCircles,
    std::map<std::string, vpRobust> &mapOfRobustLines, std::map<std::string, vpRobust> &mapOfRobustCylinders,
    std::map<std::string, vpRobust> &mapOfRobustCircles, double threshold) {
  if(iter == 0)
  {
    weighted_error.resize(nerror);

    //Init weight size
    for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
        it != m_mapOfEdgeTrackers.end(); ++it) {
      mapOfWeightLines[it->first].resize(mapOfNumberOfLines[it->first]);
      mapOfWeightLines[it->first] = 1;

      mapOfWeightCylinders[it->first].resize(mapOfNumberOfCylinders[it->first]);
      mapOfWeightCylinders[it->first] = 1;

      mapOfWeightCircles[it->first].resize(mapOfNumberOfCircles[it->first]);
      mapOfWeightCircles[it->first] = 1;
    }

    //Set threshold for each robust
    for(std::map<std::string, vpRobust>::iterator it = mapOfRobustLines.begin(); it != mapOfRobustLines.end(); ++it) {
      vpCameraParameters current_cam;
      m_mapOfEdgeTrackers[it->first]->getCameraParameters(current_cam);
      it->second.setThreshold(threshold / current_cam.get_px());
    }

    for(std::map<std::string, vpRobust>::iterator it = mapOfRobustCylinders.begin(); it != mapOfRobustCylinders.end(); ++it) {
      vpCameraParameters current_cam;
      m_mapOfEdgeTrackers[it->first]->getCameraParameters(current_cam);
      it->second.setThreshold(threshold / current_cam.get_px());
    }

    for(std::map<std::string, vpRobust>::iterator it = mapOfRobustCircles.begin(); it != mapOfRobustCircles.end(); ++it) {
      vpCameraParameters current_cam;
      m_mapOfEdgeTrackers[it->first]->getCameraParameters(current_cam);
      it->second.setThreshold( vpMath::sqr(threshold / current_cam.get_px()) );
    }

    //Compute weights
    for(std::map<std::string, vpColVector>::const_iterator it = mapOfErrorLines.begin();
        it != mapOfErrorLines.end(); ++it) {
      if(mapOfNumberOfLines[it->first] > 0) {
        mapOfRobustLines[it->first].MEstimator(vpRobust::TUKEY, it->second, mapOfWeightLines[it->first]);

        //Stack weights
        w_lines.stack(mapOfWeightLines[it->first]);
      }
    }

    for(std::map<std::string, vpColVector>::const_iterator it = mapOfErrorCylinders.begin();
        it != mapOfErrorCylinders.end(); ++it) {
      if(mapOfNumberOfCylinders[it->first] > 0) {
        mapOfRobustCylinders[it->first].MEstimator(vpRobust::TUKEY, it->second, mapOfWeightCylinders[it->first]);

        //Stack weights
        w_cylinders.stack(mapOfWeightCylinders[it->first]);
      }
    }

    for(std::map<std::string, vpColVector>::const_iterator it = mapOfErrorCircles.begin();
        it != mapOfErrorCircles.end(); ++it) {
      if(mapOfNumberOfCircles[it->first] > 0) {
        mapOfRobustCircles[it->first].MEstimator(vpRobust::TUKEY, it->second, mapOfWeightCircles[it->first]);

        //Stack weights
        w_circles.stack(mapOfWeightCircles[it->first]);
      }
    }
  }
  else
  {
    //Set iteration for each robust
    for(std::map<std::string, vpRobust>::iterator it = mapOfRobustLines.begin(); it != mapOfRobustLines.end(); ++it) {
      it->second.setIteration(iter);
    }

    for(std::map<std::string, vpRobust>::iterator it = mapOfRobustCylinders.begin(); it != mapOfRobustCylinders.end(); ++it) {
      it->second.setIteration(iter);
    }

    for(std::map<std::string, vpRobust>::iterator it = mapOfRobustCircles.begin(); it != mapOfRobustCircles.end(); ++it) {
      it->second.setIteration(iter);
    }

    //Compute weights
    for(std::map<std::string, vpColVector>::const_iterator it = mapOfErrorLines.begin();
        it != mapOfErrorLines.end(); ++it) {
      if(mapOfNumberOfLines[it->first] > 0) {
        mapOfRobustLines[it->first].MEstimator(vpRobust::TUKEY, it->second, mapOfWeightLines[it->first]);

        //Stack weights
        w_lines.stack(mapOfWeightLines[it->first]);
      }
    }

    for(std::map<std::string, vpColVector>::const_iterator it = mapOfErrorCylinders.begin();
        it != mapOfErrorCylinders.end(); ++it) {
      if(mapOfNumberOfCylinders[it->first] > 0) {
        mapOfRobustCylinders[it->first].MEstimator(vpRobust::TUKEY, it->second, mapOfWeightCylinders[it->first]);

        //Stack weights
        w_cylinders.stack(mapOfWeightCylinders[it->first]);
      }
    }

    for(std::map<std::string, vpColVector>::const_iterator it = mapOfErrorCircles.begin();
        it != mapOfErrorCircles.end(); ++it) {
      if(mapOfNumberOfCircles[it->first] > 0) {
        mapOfRobustCircles[it->first].MEstimator(vpRobust::TUKEY, it->second, mapOfWeightCircles[it->first]);

        //Stack weights
        w_circles.stack(mapOfWeightCircles[it->first]);
      }
    }
  }
}

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

  compute_interaction = 1;
//  nline = 0; //Not used in vpMbEdgeMultiTracker class
//  ncylinder = 0; //Not used in vpMbEdgeMultiTracker class
  lambda = 1;
//  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);
}