vpMbEdgeKltMultiTracker.cpp

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

#include <visp3/core/vpConfig.h>

#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)

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

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

vpMbEdgeKltMultiTracker::vpMbEdgeKltMultiTracker()
  : vpMbEdgeMultiTracker(), vpMbKltMultiTracker(), m_factorKLT(0.65), m_factorMBT(0.35), m_thresholdKLT(2.),
    m_thresholdMBT(2.), m_mapOfCameraTransformationMatrix(), m_referenceCameraName("Camera"), m_nbrow(0),
    m_L_hybridMulti(), m_error_hybridMulti(), m_w_hybridMulti(), m_weightedError_hybridMulti()
{
  // Add default camera transformation matrix
  m_mapOfCameraTransformationMatrix["Camera"] = vpHomogeneousMatrix();

  m_lambda = 0.8;
  m_maxIter = 200;
}

vpMbEdgeKltMultiTracker::vpMbEdgeKltMultiTracker(unsigned int nbCameras)
  : vpMbEdgeMultiTracker(nbCameras), vpMbKltMultiTracker(nbCameras), m_factorKLT(0.65), m_factorMBT(0.35),
    m_thresholdKLT(2.), m_thresholdMBT(2.), m_mapOfCameraTransformationMatrix(), m_referenceCameraName("Camera"),
    m_nbrow(0), m_L_hybridMulti(), m_error_hybridMulti(), m_w_hybridMulti(), m_weightedError_hybridMulti()
{

  if (nbCameras == 0) {
    throw vpException(vpTrackingException::fatalError, "Cannot construct a vpMbkltMultiTracker with no camera !");
  } else if (nbCameras == 1) {
    // Add default camera transformation matrix
    m_mapOfCameraTransformationMatrix["Camera"] = vpHomogeneousMatrix();
  } else if (nbCameras == 2) {
    // Add default camera transformation matrix
    m_mapOfCameraTransformationMatrix["Camera1"] = vpHomogeneousMatrix();
    m_mapOfCameraTransformationMatrix["Camera2"] = vpHomogeneousMatrix();

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

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

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

  m_lambda = 0.8;
  m_maxIter = 200;
}

vpMbEdgeKltMultiTracker::vpMbEdgeKltMultiTracker(const std::vector<std::string> &cameraNames)
  : vpMbEdgeMultiTracker(cameraNames), vpMbKltMultiTracker(cameraNames), m_factorKLT(0.65), m_factorMBT(0.35),
    m_thresholdKLT(2.), m_thresholdMBT(2.), m_mapOfCameraTransformationMatrix(), m_referenceCameraName(cameraNames.front()),
    m_nbrow(0), m_L_hybridMulti(), m_error_hybridMulti(), m_w_hybridMulti(), m_weightedError_hybridMulti()
{
  m_lambda = 0.8;
  m_maxIter = 200;
}

vpMbEdgeKltMultiTracker::~vpMbEdgeKltMultiTracker() {}

void vpMbEdgeKltMultiTracker::computeVVS(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                                         unsigned int lvl)
{

  m_nbrow = initMbtTracking(mapOfImages, lvl);

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

  double factorMBT = m_factorMBT;
  double factorKLT = m_factorKLT;
  if (m_nbrow < 4) {
    factorKLT = 1.;
    std::cerr << "There are less than 4 KLT features, set factorKLT = 1. !" << std::endl;
  }

  if (m_nbInfos < 4) {
    factorMBT = 1.;
    std::cerr << "There are less than 4 moving edges, set factorMBT = 1. !" << std::endl;
    m_nbInfos = 0;
  }

  computeVVSInit();

  vpHomogeneousMatrix cMoPrev;
  vpHomogeneousMatrix ctTc0_Prev;
  // Error vector for MBT + KLT for the previous iteration
  vpColVector m_error_prev;
  // Weighting vector for MBT + KLT for the previous iteration
  vpColVector m_w_prev;
  double mu = m_initialMu;

  // Create the map of VelocityTwistMatrices
  std::map<std::string, vpVelocityTwistMatrix> mapOfVelocityTwist;
  for (std::map<std::string, vpHomogeneousMatrix>::const_iterator it = m_mapOfCameraTransformationMatrix.begin();
       it != m_mapOfCameraTransformationMatrix.end(); ++it) {
    vpVelocityTwistMatrix cVo;
    cVo.buildFrom(it->second);
    mapOfVelocityTwist[it->first] = cVo;
  }

  // Variables used in the minimization process
  double residu = 0;
  double residu_1 = -1;
  unsigned int iter = 0;

  vpMatrix L_true;
  vpMatrix LVJ_true;

  vpColVector v;

  vpMatrix LTL;
  vpColVector LTR;

  while (((int)((residu - residu_1) * 1e8) != 0) && (iter < m_maxIter)) {
    computeVVSInteractionMatrixAndResidu(mapOfImages, mapOfVelocityTwist);

    bool reStartFromLastIncrement = false;
    computeVVSCheckLevenbergMarquardt(iter, m_error_hybridMulti, m_error_prev, cMoPrev, mu, reStartFromLastIncrement,
                                      &m_w_prev);
    if (reStartFromLastIncrement) {
      ctTc0 = ctTc0_Prev;
    }

    if (!reStartFromLastIncrement) {
      computeVVSWeights();

      // Set weight for m_w with the good weighting between MBT and KLT
      for (unsigned int cpt = 0; cpt < m_nbrow + 2 * m_nbInfos; cpt++) {
        if (cpt < m_nbrow) {
          m_w_hybridMulti[cpt] = (m_w_hybridMulti[cpt] * m_factor[cpt]) * factorMBT;
        } else {
          m_w_hybridMulti[cpt] *= factorKLT;
        }
      }

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

      residu_1 = residu;
      residu = 0;
      double num = 0;
      double den = 0;

      for (unsigned int i = 0; i < m_weightedError_hybridMulti.getRows(); i++) {
        num += m_w_hybridMulti[i] * vpMath::sqr(m_error_hybridMulti[i]);
        den += m_w_hybridMulti[i];

        m_weightedError_hybridMulti[i] = m_error_hybridMulti[i] * m_w_hybridMulti[i];
        if (m_computeInteraction) {
          for (unsigned int j = 0; j < 6; j++) {
            m_L_hybridMulti[i][j] *= m_w_hybridMulti[i];
          }
        }
      }

      residu = sqrt(num / den);

      computeVVSPoseEstimation(isoJoIdentity, iter, m_L_hybridMulti, LTL, m_weightedError_hybridMulti,
                               m_error_hybridMulti, m_error_prev, LTR, mu, v, &m_w_hybridMulti, &m_w_prev);

      cMoPrev = m_cMo;
      ctTc0_Prev = ctTc0;
      ctTc0 = vpExponentialMap::direct(v).inverse() * ctTc0;
      m_cMo = ctTc0 * c0Mo;
    }

    iter++;
  }

  computeCovarianceMatrixVVS(isoJoIdentity, m_w_hybridMulti, cMoPrev, L_true, LVJ_true, m_error_hybridMulti);
}

void vpMbEdgeKltMultiTracker::computeVVSInit()
{
  unsigned int totalNbRows = 2 * m_nbInfos + m_nbrow;

  m_L_hybridMulti.resize(totalNbRows, 6, false, false);
  m_error_hybridMulti.resize(totalNbRows, false);

  m_weightedError_hybridMulti.resize(totalNbRows, false);
  m_w_hybridMulti.resize(totalNbRows, false);
  m_w_hybridMulti = 1;

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

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

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

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

      // Set the corresponding cMo for the current camera
      it->second->m_cMo = m_mapOfCameraTransformationMatrix[it->first] * m_cMo;

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

      // Stack interaction matrix for MBT
      m_L_hybridMulti.insert(edge->m_L_edge * mapOfVelocityTwist[it->first], startIdx, 0);
      // Stack residual for MBT
      m_error_hybridMulti.insert(startIdx, edge->m_error_edge);

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

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

    if (klt->m_nbInfos > 0) {

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

      // Stack residual and interaction matrix
      m_error_hybridMulti.insert(startIdx, klt->m_error_klt);
      m_L_hybridMulti.insert(klt->m_L_klt * mapOfVelocityTwist[it->first], startIdx, 0);

      startIdx += 2 * klt->m_nbInfos;
    }
  }
}

void vpMbEdgeKltMultiTracker::computeVVSWeights()
{
  unsigned int startIdx = 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_hybridMulti.insert(startIdx, edge->m_w_edge);
    startIdx += edge->m_w_edge.getRows();
  }

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

    // Compute weights
    klt->computeVVSWeights(klt->m_robust_klt, klt->m_error_klt, klt->m_w_klt);

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

void vpMbEdgeKltMultiTracker::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo,
                                      const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
                                      bool displayFullModel)
{
  vpMbEdgeMultiTracker::display(I, cMo, cam, col, thickness, displayFullModel);

  // Display only features for KLT trackers
  for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
       it_klt != m_mapOfKltTrackers.end(); ++it_klt) {

    for (std::list<vpMbtDistanceKltPoints *>::const_iterator it_pts = it_klt->second->kltPolygons.begin();
         it_pts != it_klt->second->kltPolygons.end(); ++it_pts) {
      vpMbtDistanceKltPoints *kltpoly = *it_pts;
      if (displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) {
        kltpoly->displayPrimitive(I);
      }
    }

    for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it_cyl = it_klt->second->kltCylinders.begin();
         it_cyl != it_klt->second->kltCylinders.end(); ++it_cyl) {
      vpMbtDistanceKltCylinder *kltPolyCylinder = *it_cyl;
      if (displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
        kltPolyCylinder->displayPrimitive(I);
    }
  }
}

void vpMbEdgeKltMultiTracker::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
                                      const vpCameraParameters &cam, const vpColor &color,
                                      unsigned int thickness, bool displayFullModel)
{
  vpMbEdgeMultiTracker::display(I, cMo, cam, color, thickness, displayFullModel);
  //  vpMbKltMultiTracker::display(I, cMo_, cam_, color, thickness,
  //  displayFullModel);

  // Display only features for KLT trackers
  for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
       it_klt != m_mapOfKltTrackers.end(); ++it_klt) {

    for (std::list<vpMbtDistanceKltPoints *>::const_iterator it_pts = it_klt->second->kltPolygons.begin();
         it_pts != it_klt->second->kltPolygons.end(); ++it_pts) {
      vpMbtDistanceKltPoints *kltpoly = *it_pts;
      if (displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) {
        kltpoly->displayPrimitive(I);
      }
    }

    for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it_cyl = it_klt->second->kltCylinders.begin();
         it_cyl != it_klt->second->kltCylinders.end(); ++it_cyl) {
      vpMbtDistanceKltCylinder *kltPolyCylinder = *it_cyl;
      if (displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
        kltPolyCylinder->displayPrimitive(I);
    }
  }
}

void vpMbEdgeKltMultiTracker::display(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
                                      const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
                                      const vpCameraParameters &cam1, const vpCameraParameters &cam2,
                                      const vpColor &color, unsigned int thickness, bool displayFullModel)
{
  vpMbEdgeMultiTracker::display(I1, I2, c1Mo, c2Mo, cam1, cam2, color, thickness, displayFullModel);
  //  vpMbKltMultiTracker::display(I1, I2, c1Mo, c2Mo, cam1, cam2, color,
  //  thickness, displayFullModel);

  // Display only features for KLT trackers
  bool first = true;
  for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
       it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
    for (std::list<vpMbtDistanceKltPoints *>::const_iterator it_pts = it_klt->second->kltPolygons.begin();
         it_pts != it_klt->second->kltPolygons.end(); ++it_pts) {
      vpMbtDistanceKltPoints *kltpoly = *it_pts;
      if (displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) {
        if (first) {
          kltpoly->displayPrimitive(I1);
        } else {
          kltpoly->displayPrimitive(I2);
        }
      }
    }

    for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it_cyl = it_klt->second->kltCylinders.begin();
         it_cyl != it_klt->second->kltCylinders.end(); ++it_cyl) {
      vpMbtDistanceKltCylinder *kltPolyCylinder = *it_cyl;
      if (displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) {
        if (first) {
          kltPolyCylinder->displayPrimitive(I1);
        } else {
          kltPolyCylinder->displayPrimitive(I2);
        }
      }
    }

    first = false;
  }
}

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

  // Display only features for KLT trackers (not the model)
  bool first = true;
  for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
       it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
    for (std::list<vpMbtDistanceKltPoints *>::const_iterator it_pts = it_klt->second->kltPolygons.begin();
         it_pts != it_klt->second->kltPolygons.end(); ++it_pts) {
      vpMbtDistanceKltPoints *kltpoly = *it_pts;
      if (displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) {
        if (first) {
          kltpoly->displayPrimitive(I1);
        } else {
          kltpoly->displayPrimitive(I2);
        }
      }
    }

    for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it_cyl = it_klt->second->kltCylinders.begin();
         it_cyl != it_klt->second->kltCylinders.end(); ++it_cyl) {
      vpMbtDistanceKltCylinder *kltPolyCylinder = *it_cyl;
      if (displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) {
        if (first) {
          kltPolyCylinder->displayPrimitive(I1);
        } else {
          kltPolyCylinder->displayPrimitive(I2);
        }
      }
    }

    first = false;
  }
}

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

  // Display only features for KLT trackers (not the model)
  for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
       it_klt != m_mapOfKltTrackers.end(); ++it_klt) {

    std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.find(it_klt->first);
    if (it_img != mapOfImages.end()) {
      for (std::list<vpMbtDistanceKltPoints *>::const_iterator it_pts = it_klt->second->kltPolygons.begin();
           it_pts != it_klt->second->kltPolygons.end(); ++it_pts) {
        vpMbtDistanceKltPoints *kltpoly = *it_pts;
        if (displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) {
          kltpoly->displayPrimitive(*(it_img->second));
        }
      }

      for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it_cyl = it_klt->second->kltCylinders.begin();
           it_cyl != it_klt->second->kltCylinders.end(); ++it_cyl) {
        vpMbtDistanceKltCylinder *kltPolyCylinder = *it_cyl;
        if (displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
          kltPolyCylinder->displayPrimitive(*(it_img->second));
      }
    }
  }
}

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

  // Display only features for KLT trackers  (not the model)
  for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
       it_klt != m_mapOfKltTrackers.end(); ++it_klt) {

    std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img = mapOfImages.find(it_klt->first);
    if (it_img != mapOfImages.end()) {
      for (std::list<vpMbtDistanceKltPoints *>::const_iterator it_pts = it_klt->second->kltPolygons.begin();
           it_pts != it_klt->second->kltPolygons.end(); ++it_pts) {
        vpMbtDistanceKltPoints *kltpoly = *it_pts;
        if (displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) {
          kltpoly->displayPrimitive(*(it_img->second));
        }
      }

      for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it_cyl = it_klt->second->kltCylinders.begin();
           it_cyl != it_klt->second->kltCylinders.end(); ++it_cyl) {
        vpMbtDistanceKltCylinder *kltPolyCylinder = *it_cyl;
        if (displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
          kltPolyCylinder->displayPrimitive(*(it_img->second));
      }
    }
  }
}

std::vector<std::string> vpMbEdgeKltMultiTracker::getCameraNames() const
{
  return vpMbEdgeMultiTracker::getCameraNames();
}

void vpMbEdgeKltMultiTracker::getCameraParameters(vpCameraParameters &cam) const { cam = m_cam; }

void vpMbEdgeKltMultiTracker::getCameraParameters(vpCameraParameters &cam1, vpCameraParameters &cam2) const
{
  // We could use either the vpMbEdgeMultiTracker or vpMbKltMultiTracker class
  vpMbEdgeMultiTracker::getCameraParameters(cam1, cam2);
}

void vpMbEdgeKltMultiTracker::getCameraParameters(const std::string &cameraName, vpCameraParameters &camera) const
{
  // We could use either the vpMbEdgeMultiTracker or vpMbKltMultiTracker class
  vpMbEdgeMultiTracker::getCameraParameters(cameraName, camera);
}

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

  // We could use either the vpMbEdgeMultiTracker or vpMbKltMultiTracker class
  vpMbEdgeMultiTracker::getCameraParameters(mapOfCameraParameters);
}

unsigned int vpMbEdgeKltMultiTracker::getClipping(const std::string &cameraName) const
{
  std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(cameraName);
  if (it != m_mapOfKltTrackers.end()) {
    // Return the clipping flags for m_mapOfKltTrackers as it should be the
    // same for the  same camera in m_mapOfEdgeTrackers
    return it->second->getClipping();
  } else {
    std::cerr << "Cannot find camera: " << cameraName << std::endl;
  }

  return vpMbTracker::getClipping();
}

vpMbHiddenFaces<vpMbtPolygon> &vpMbEdgeKltMultiTracker::getFaces()
{
  std::cerr << "Return the wrong faces reference !" << std::endl;
  std::cerr << "Use vpMbEdgeKltMultiTracker::getEdgeFaces or "
               "vpMbEdgeKltMultiTracker::getKltFaces instead !"
            << std::endl;

  return faces;
}

vpMbHiddenFaces<vpMbtPolygon> &vpMbEdgeKltMultiTracker::getEdgeFaces(const std::string &cameraName)
{
  return vpMbEdgeMultiTracker::getFaces(cameraName);
}

std::map<std::string, vpMbHiddenFaces<vpMbtPolygon> > vpMbEdgeKltMultiTracker::getEdgeFaces() const
{
  return vpMbEdgeMultiTracker::getFaces();
}

vpMbHiddenFaces<vpMbtPolygon> &vpMbEdgeKltMultiTracker::getKltFaces(const std::string &cameraName)
{
  return vpMbKltMultiTracker::getFaces(cameraName);
}

std::map<std::string, vpMbHiddenFaces<vpMbtPolygon> > vpMbEdgeKltMultiTracker::getKltFaces() const
{
  return vpMbKltMultiTracker::getFaces();
}

unsigned int vpMbEdgeKltMultiTracker::getNbPolygon() const
{
  std::cerr << "Use vpMbEdgeKltMultiTracker::getEdgeMultiNbPolygon or "
               "vpMbEdgeKltMultiTracker::getKltMultiNbPolygon instead !"
            << std::endl;
  return 0;
}

std::map<std::string, unsigned int> vpMbEdgeKltMultiTracker::getEdgeMultiNbPolygon() const
{
  return vpMbEdgeMultiTracker::getMultiNbPolygon();
}

std::map<std::string, unsigned int> vpMbEdgeKltMultiTracker::getKltMultiNbPolygon() const
{
  return vpMbKltMultiTracker::getMultiNbPolygon();
}

void vpMbEdgeKltMultiTracker::getPose(vpHomogeneousMatrix &c1Mo, vpHomogeneousMatrix &c2Mo) const
{
  // We could use either the vpMbEdgeMultiTracker or vpMbKltMultiTracker class
  vpMbEdgeMultiTracker::getPose(c1Mo, c2Mo);
}

void vpMbEdgeKltMultiTracker::getPose(const std::string &cameraName, vpHomogeneousMatrix &cMo) const
{
  // We could use either the vpMbEdgeMultiTracker or vpMbKltMultiTracker class
  vpMbEdgeMultiTracker::getPose(cameraName, cMo);
}

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

  // We could use either the vpMbEdgeMultiTracker or vpMbKltMultiTracker class
  vpMbEdgeMultiTracker::getPose(mapOfCameraPoses);
}

void vpMbEdgeKltMultiTracker::init(const vpImage<unsigned char> & /*I*/)
{
  if (!modelInitialised) {
    throw vpException(vpTrackingException::initializationError, "model not initialized");
  }
}

#ifdef VISP_HAVE_MODULE_GUI

void vpMbEdgeKltMultiTracker::initClick(const vpImage<unsigned char> &I, const std::vector<vpPoint> &points3D_list,
                                        const std::string &displayFile)
{
  // Cannot use directly set pose for KLT as it is different than for the edge
  // case  It moves the KLT points instead of detecting new KLT points
  vpMbKltMultiTracker::initClick(I, points3D_list, displayFile);

  // Set pose for Edge (setPose or initFromPose is equivalent with
  // vpMbEdgeTracker)  And it avoids to click a second time
  vpMbEdgeMultiTracker::setPose(I, m_cMo);
}

void vpMbEdgeKltMultiTracker::initClick(const vpImage<unsigned char> &I, const std::string &initFile,
                                        bool displayHelp, const vpHomogeneousMatrix &T)
{
  // Cannot use directly set pose for KLT as it is different than for the edge
  // case  It moves the KLT points instead of detecting new KLT points
  vpMbKltMultiTracker::initClick(I, initFile, displayHelp, T);

  // Set pose for Edge (setPose or initFromPose is equivalent with
  // vpMbEdgeTracker)  And it avoids to click a second time
  vpMbEdgeMultiTracker::setPose(I, m_cMo);
}

void vpMbEdgeKltMultiTracker::initClick(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
                                        const std::string &initFile1, const std::string &initFile2,
                                        bool displayHelp, bool firstCameraIsReference)
{
  vpMbKltMultiTracker::initClick(I1, I2, initFile1, initFile2, displayHelp, firstCameraIsReference);

  // Get c2Mo
  vpHomogeneousMatrix c2Mo;
  std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
  if (firstCameraIsReference) {
    ++it_klt;
    it_klt->second->getPose(c2Mo);
  } else {
    it_klt->second->getPose(c2Mo);
  }

  // Set pose for Edge (setPose or initFromPose is equivalent with
  // vpMbEdgeTracker)
  vpMbEdgeMultiTracker::setPose(I1, I2, m_cMo, c2Mo, firstCameraIsReference);
}

void vpMbEdgeKltMultiTracker::initClick(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                                        const std::string &initFile, bool displayHelp)
{
  vpMbKltMultiTracker::initClick(mapOfImages, initFile, displayHelp);

  // Get pose for all the cameras
  std::map<std::string, vpHomogeneousMatrix> mapOfCameraPoses;
  vpMbKltMultiTracker::getPose(mapOfCameraPoses);

  // Set pose for Edge for all the cameras (setPose or initFromPose is
  // equivalent with vpMbEdgeTracker)  And it avoids to click a second time
  vpMbEdgeMultiTracker::setPose(mapOfImages, mapOfCameraPoses);
}

void vpMbEdgeKltMultiTracker::initClick(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                                        const std::map<std::string, std::string> &mapOfInitFiles,
                                        bool displayHelp)
{
  vpMbKltMultiTracker::initClick(mapOfImages, mapOfInitFiles, displayHelp);

  // Get pose for all the cameras
  std::map<std::string, vpHomogeneousMatrix> mapOfCameraPoses;
  vpMbKltMultiTracker::getPose(mapOfCameraPoses);

  // Set pose for Edge for all the cameras (setPose or initFromPose is
  // equivalent with vpMbEdgeTracker)  And it avoids to click a second time
  vpMbEdgeMultiTracker::setPose(mapOfImages, mapOfCameraPoses);
}
#endif //#ifdef VISP_HAVE_MODULE_GUI

void vpMbEdgeKltMultiTracker::initCircle(const vpPoint &, const vpPoint &, const vpPoint &, const double, const int,
                                         const std::string &)
{
  std::cerr << "The method initCircle is not used in vpMbEdgeKltMultiTracker !" << std::endl;
}

void vpMbEdgeKltMultiTracker::initCylinder(const vpPoint &, const vpPoint &, const double, const int,
                                           const std::string &)
{
  std::cerr << "The method initCylinder is not used in vpMbEdgeKltMultiTracker !" << std::endl;
}

void vpMbEdgeKltMultiTracker::initFaceFromCorners(vpMbtPolygon &)
{
  std::cerr << "The method initFaceFromCorners is not used in "
               "vpMbEdgeKltMultiTracker !"
            << std::endl;
}

void vpMbEdgeKltMultiTracker::initFaceFromLines(vpMbtPolygon &)
{
  std::cerr << "The method initFaceFromLines is not used in "
               "vpMbEdgeKltMultiTracker !"
            << std::endl;
}

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

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

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

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

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

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

  m_cMo.buildFrom(init_pos);
  vpMbEdgeMultiTracker::initFromPose(I, m_cMo);
  vpMbKltMultiTracker::initFromPose(I, m_cMo);
}

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

  m_cMo = cMo;
  vpMbEdgeMultiTracker::initFromPose(I, m_cMo);
  vpMbKltMultiTracker::initFromPose(I, m_cMo);
}

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

void vpMbEdgeKltMultiTracker::initFromPose(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
                                           const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
                                           bool firstCameraIsReference)
{
  vpMbEdgeMultiTracker::initFromPose(I1, I2, c1Mo, c2Mo, firstCameraIsReference);
  vpMbKltMultiTracker::initFromPose(I1, I2, c1Mo, c2Mo, firstCameraIsReference);
}

void vpMbEdgeKltMultiTracker::initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                                           const vpHomogeneousMatrix &cMo)
{
  vpMbEdgeMultiTracker::initFromPose(mapOfImages, cMo);
  vpMbKltMultiTracker::initFromPose(mapOfImages, cMo);
}

void vpMbEdgeKltMultiTracker::initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                                           const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
{
  vpMbEdgeMultiTracker::initFromPose(mapOfImages, mapOfCameraPoses);
  vpMbKltMultiTracker::initFromPose(mapOfImages, mapOfCameraPoses);
}

unsigned int
vpMbEdgeKltMultiTracker::initMbtTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                                         unsigned int lvl)
{
  vpMbEdgeTracker *edge = NULL;
  unsigned int nbrows = 0;

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

    try {
      edge->computeVVSInit();

      unsigned int nbrow = edge->m_error_edge.getRows();
      nbrows += nbrow;

      // Set the corresponding cMo for each camera
      // Used in computeVVSFirstPhaseFactor with computeInteractionMatrixError
      edge->m_cMo = m_mapOfCameraTransformationMatrix[it->first] * m_cMo;

      edge->computeVVSFirstPhaseFactor(*mapOfImages[it->first], lvl);
      m_factor.stack(edge->m_factor);
    } catch (...) {
      edge->m_L_edge.resize(0, 6, false, false);
      edge->m_error_edge.resize(0, false);

      edge->m_weightedError_edge.resize(0, false);
      edge->m_w_edge.resize(0, false);
      edge->m_factor.resize(0, false);

      edge->m_robustLines.resize(0);
      edge->m_robustCylinders.resize(0);
      edge->m_robustCircles.resize(0);

      edge->m_wLines.resize(0, false);
      edge->m_wCylinders.resize(0, false);
      edge->m_wCircles.resize(0, false);

      edge->m_errorLines.resize(0, false);
      edge->m_errorCylinders.resize(0, false);
      edge->m_errorCircles.resize(0, false);
    }
  }

  return nbrows;
}

void vpMbEdgeKltMultiTracker::loadConfigFile(const std::string &configFile)
{
  vpMbEdgeMultiTracker::loadConfigFile(configFile);
  vpMbKltMultiTracker::loadConfigFile(configFile);
}

void vpMbEdgeKltMultiTracker::loadConfigFile(const std::string &configFile1, const std::string &configFile2,
                                             bool firstCameraIsReference)
{
  vpMbEdgeMultiTracker::loadConfigFile(configFile1, configFile2, firstCameraIsReference);
  vpMbKltMultiTracker::loadConfigFile(configFile1, configFile2, firstCameraIsReference);
}

void vpMbEdgeKltMultiTracker::loadConfigFile(const std::map<std::string, std::string> &mapOfConfigFiles)
{
  vpMbEdgeMultiTracker::loadConfigFile(mapOfConfigFiles);
  vpMbKltMultiTracker::loadConfigFile(mapOfConfigFiles);
}

void vpMbEdgeKltMultiTracker::loadModel(const std::string &modelFile, bool verbose,
                                        const vpHomogeneousMatrix &T)
{
  vpMbEdgeMultiTracker::loadModel(modelFile, verbose, T);
  vpMbKltMultiTracker::loadModel(modelFile, verbose, T);

  modelInitialised = true;
}

void vpMbEdgeKltMultiTracker::postTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages)
{
  // MBT
  vpMbEdgeTracker *edge = NULL;
  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
       it != m_mapOfEdgeTrackers.end(); ++it) {
    edge = it->second;

    edge->updateMovingEdgeWeights();

    if (displayFeatures) {
      edge->m_featuresToBeDisplayedEdge = edge->getFeaturesForDisplayEdge();
    }
  }

  // KLT
  vpMbKltMultiTracker::postTracking(mapOfImages);

  // 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) {
    edge = it->second;
    edge->visibleFace(*mapOfImages[it->first], it->second->m_cMo, newvisibleface);
  }

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

      edge->faces.computeClippedPolygons(it->second->m_cMo, it->second->m_cam);
      edge->faces.computeScanLineRender(it->second->m_cam, mapOfImages[it->first]->getWidth(),
                                        mapOfImages[it->first]->getHeight());
    }
  }

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

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

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

    if (computeProjError) {
      edge->computeProjectionError(*mapOfImages[it->first]);
    }
  }
}

void vpMbEdgeKltMultiTracker::reinit(/*const vpImage<unsigned char>& I */)
{
  //  vpMbEdgeMultiTracker::reinit();
  vpMbKltMultiTracker::reinit();
}

void vpMbEdgeKltMultiTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
                                          const vpHomogeneousMatrix &cMo, bool verbose,
                                          const vpHomogeneousMatrix &T)
{
  vpMbEdgeMultiTracker::reInitModel(I, cad_name, cMo, verbose, T);
  vpMbKltMultiTracker::reInitModel(I, cad_name, cMo, verbose, T);
}

void vpMbEdgeKltMultiTracker::reInitModel(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
                                          const std::string &cad_name, const vpHomogeneousMatrix &c1Mo,
                                          const vpHomogeneousMatrix &c2Mo, bool verbose,
                                          bool firstCameraIsReference)
{
  vpMbEdgeMultiTracker::reInitModel(I1, I2, cad_name, c1Mo, c2Mo, verbose, firstCameraIsReference);
  vpMbKltMultiTracker::reInitModel(I1, I2, cad_name, c1Mo, c2Mo, verbose, firstCameraIsReference);
}

void vpMbEdgeKltMultiTracker::reInitModel(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                                          const std::string &cad_name,
                                          const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
                                          bool verbose)
{
  vpMbEdgeMultiTracker::reInitModel(mapOfImages, cad_name, mapOfCameraPoses, verbose);
  vpMbKltMultiTracker::reInitModel(mapOfImages, cad_name, mapOfCameraPoses, verbose);
}

void vpMbEdgeKltMultiTracker::resetTracker()
{
  vpMbEdgeMultiTracker::resetTracker();
  vpMbKltMultiTracker::resetTracker();
}

void vpMbEdgeKltMultiTracker::setAngleAppear(const double &a)
{
  vpMbEdgeMultiTracker::setAngleAppear(a);
  vpMbKltMultiTracker::setAngleAppear(a);
}

void vpMbEdgeKltMultiTracker::setAngleDisappear(const double &a)
{
  vpMbEdgeMultiTracker::setAngleDisappear(a);
  vpMbKltMultiTracker::setAngleDisappear(a);
}

void vpMbEdgeKltMultiTracker::setCameraParameters(const vpCameraParameters &cam)
{
  vpMbEdgeMultiTracker::setCameraParameters(cam);
  vpMbKltMultiTracker::setCameraParameters(cam);

  m_cam = cam;
}

void vpMbEdgeKltMultiTracker::setCameraParameters(const vpCameraParameters &camera1, const vpCameraParameters &camera2,
                                                  bool firstCameraIsReference)
{
  vpMbEdgeMultiTracker::setCameraParameters(camera1, camera2, firstCameraIsReference);
  vpMbKltMultiTracker::setCameraParameters(camera1, camera2, firstCameraIsReference);

  if (firstCameraIsReference) {
    m_cam = camera1;
  } else {
    m_cam = camera2;
  }
}

void vpMbEdgeKltMultiTracker::setCameraParameters(const std::string &cameraName, const vpCameraParameters &cam)
{
  vpMbEdgeMultiTracker::setCameraParameters(cameraName, cam);
  vpMbKltMultiTracker::setCameraParameters(cameraName, cam);

  if (cameraName == m_referenceCameraName) {
    m_cam = cam;
  }
}

void vpMbEdgeKltMultiTracker::setCameraParameters(
    const std::map<std::string, vpCameraParameters> &mapOfCameraParameters)
{
  vpMbEdgeMultiTracker::setCameraParameters(mapOfCameraParameters);
  vpMbKltMultiTracker::setCameraParameters(mapOfCameraParameters);

  for (std::map<std::string, vpCameraParameters>::const_iterator it = mapOfCameraParameters.begin();
       it != mapOfCameraParameters.end(); ++it) {
    if (it->first == m_referenceCameraName) {
      m_cam = it->second;
    }
  }
}

void vpMbEdgeKltMultiTracker::setCameraTransformationMatrix(const std::string &cameraName,
                                                            const vpHomogeneousMatrix &cameraTransformationMatrix)
{
  vpMbEdgeMultiTracker::setCameraTransformationMatrix(cameraName, cameraTransformationMatrix);
  vpMbKltMultiTracker::setCameraTransformationMatrix(cameraName, cameraTransformationMatrix);

  std::map<std::string, vpHomogeneousMatrix>::iterator it = m_mapOfCameraTransformationMatrix.find(cameraName);
  if (it != m_mapOfCameraTransformationMatrix.end()) {
    it->second = cameraTransformationMatrix;
  } else {
    std::cerr << "Cannot find camera: " << cameraName << " !" << std::endl;
  }
}

void vpMbEdgeKltMultiTracker::setCameraTransformationMatrix(
    const std::map<std::string, vpHomogeneousMatrix> &mapOfTransformationMatrix)
{
  vpMbEdgeMultiTracker::setCameraTransformationMatrix(mapOfTransformationMatrix);
  vpMbKltMultiTracker::setCameraTransformationMatrix(mapOfTransformationMatrix);

  m_mapOfCameraTransformationMatrix = mapOfTransformationMatrix;
}

void vpMbEdgeKltMultiTracker::setClipping(const unsigned int &flags)
{
  vpMbEdgeMultiTracker::setClipping(flags);
  vpMbKltMultiTracker::setClipping(flags);
}

void vpMbEdgeKltMultiTracker::setClipping(const std::string &cameraName, const unsigned int &flags)
{
  // Here, we do not change the general clipping flag
  vpMbEdgeMultiTracker::setClipping(cameraName, flags);
  vpMbKltMultiTracker::setClipping(cameraName, flags);
}

void vpMbEdgeKltMultiTracker::setCovarianceComputation(const bool &flag)
{
  vpMbEdgeMultiTracker::setCovarianceComputation(flag);
  vpMbKltMultiTracker::setCovarianceComputation(flag);
}

void vpMbEdgeKltMultiTracker::setDisplayFeatures(bool displayF)
{
  vpMbEdgeMultiTracker::setDisplayFeatures(displayF);
  vpMbKltMultiTracker::setDisplayFeatures(displayF);
}

void vpMbEdgeKltMultiTracker::setFarClippingDistance(const double &dist)
{
  vpMbEdgeMultiTracker::setFarClippingDistance(dist);
  vpMbKltMultiTracker::setFarClippingDistance(dist);
}

void vpMbEdgeKltMultiTracker::setFarClippingDistance(const std::string &cameraName, const double &dist)
{
  vpMbEdgeMultiTracker::setFarClippingDistance(cameraName, dist);
  vpMbKltMultiTracker::setFarClippingDistance(cameraName, dist);
}

#ifdef VISP_HAVE_OGRE

void vpMbEdgeKltMultiTracker::setGoodNbRayCastingAttemptsRatio(const double &ratio)
{
  vpMbEdgeMultiTracker::setGoodNbRayCastingAttemptsRatio(ratio);
  vpMbKltMultiTracker::setGoodNbRayCastingAttemptsRatio(ratio);
}

void vpMbEdgeKltMultiTracker::setNbRayCastingAttemptsForVisibility(const unsigned int &attempts)
{
  vpMbEdgeMultiTracker::setNbRayCastingAttemptsForVisibility(attempts);
  vpMbKltMultiTracker::setNbRayCastingAttemptsForVisibility(attempts);
}
#endif

void vpMbEdgeKltMultiTracker::setLod(bool useLod, const std::string &name)
{
  vpMbEdgeMultiTracker::setLod(useLod, name);
  vpMbKltMultiTracker::setLod(useLod, name);
}

void vpMbEdgeKltMultiTracker::setLod(bool useLod, const std::string &cameraName, const std::string &name)
{
  vpMbEdgeMultiTracker::setLod(useLod, cameraName, name);
  vpMbKltMultiTracker::setLod(useLod, cameraName, name);
}

void vpMbEdgeKltMultiTracker::setMinLineLengthThresh(double minLineLengthThresh, const std::string &name)
{
  vpMbEdgeMultiTracker::setMinLineLengthThresh(minLineLengthThresh, name);
}

void vpMbEdgeKltMultiTracker::setMinLineLengthThresh(double minLineLengthThresh, const std::string &cameraName,
                                                     const std::string &name)
{
  vpMbEdgeMultiTracker::setMinLineLengthThresh(minLineLengthThresh, cameraName, name);
}

void vpMbEdgeKltMultiTracker::setMinPolygonAreaThresh(double minPolygonAreaThresh, const std::string &name)
{
  vpMbEdgeMultiTracker::setMinPolygonAreaThresh(minPolygonAreaThresh, name);
  vpMbKltMultiTracker::setMinPolygonAreaThresh(minPolygonAreaThresh, name);
}

void vpMbEdgeKltMultiTracker::setMinPolygonAreaThresh(double minPolygonAreaThresh, const std::string &cameraName,
                                                      const std::string &name)
{
  vpMbEdgeMultiTracker::setMinPolygonAreaThresh(minPolygonAreaThresh, cameraName, name);
  vpMbKltMultiTracker::setMinPolygonAreaThresh(minPolygonAreaThresh, cameraName, name);
}

void vpMbEdgeKltMultiTracker::setNearClippingDistance(const double &dist)
{
  vpMbEdgeMultiTracker::setNearClippingDistance(dist);
  vpMbKltMultiTracker::setNearClippingDistance(dist);
}

void vpMbEdgeKltMultiTracker::setNearClippingDistance(const std::string &cameraName, const double &dist)
{
  vpMbEdgeMultiTracker::setNearClippingDistance(cameraName, dist);
  vpMbKltMultiTracker::setNearClippingDistance(cameraName, dist);
}

void vpMbEdgeKltMultiTracker::setOgreShowConfigDialog(bool showConfigDialog)
{
  vpMbEdgeMultiTracker::setOgreShowConfigDialog(showConfigDialog);
  vpMbKltMultiTracker::setOgreShowConfigDialog(showConfigDialog);
}

void vpMbEdgeKltMultiTracker::setOgreVisibilityTest(const bool &v)
{
  // Edge
  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 Edge MBT Hybrid (" + it->first + ")");
  }
#endif

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

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

  useOgre = v;
}

void vpMbEdgeKltMultiTracker::setOptimizationMethod(const vpMbtOptimizationMethod &opt)
{
  vpMbEdgeMultiTracker::setOptimizationMethod(opt);
  vpMbKltMultiTracker::setOptimizationMethod(opt);
}

void vpMbEdgeKltMultiTracker::setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
{
  if (m_mapOfEdgeTrackers.size() != 1 || m_mapOfKltTrackers.size() != 1) {
    std::cerr << "This method requires only 1 camera !" << std::endl;
  } else {
    std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);
    std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);
    if (it_edge != m_mapOfEdgeTrackers.end() && it_klt != m_mapOfKltTrackers.end()) {
      it_edge->second->setPose(I, cMo);
      it_klt->second->setPose(I, cMo);

      m_cMo = cMo;
      c0Mo = m_cMo;
      ctTc0.eye();
    } else {
      std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << " !" << std::endl;
    }
  }
}

void vpMbEdgeKltMultiTracker::setPose(const vpImage<vpRGBa> &I_color, const vpHomogeneousMatrix &cMo)
{
  if (m_mapOfEdgeTrackers.size() != 1 || m_mapOfKltTrackers.size() != 1) {
    std::cerr << "This method requires only 1 camera !" << std::endl;
  } else {
    std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);
    std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);
    if (it_edge != m_mapOfEdgeTrackers.end() && it_klt != m_mapOfKltTrackers.end()) {
      vpImageConvert::convert(I_color, m_I);
      it_edge->second->setPose(m_I, cMo);
      it_klt->second->setPose(m_I, cMo);

      m_cMo = cMo;
      c0Mo = m_cMo;
      ctTc0.eye();
    } else {
      std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << " !" << std::endl;
    }
  }
}

void vpMbEdgeKltMultiTracker::setPose(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
                                      const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
                                      bool firstCameraIsReference)
{
  vpMbEdgeMultiTracker::setPose(I1, I2, c1Mo, c2Mo, firstCameraIsReference);
  vpMbKltMultiTracker::setPose(I1, I2, c1Mo, c2Mo, firstCameraIsReference);
}

void vpMbEdgeKltMultiTracker::setPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                                      const vpHomogeneousMatrix &cMo)
{
  vpMbEdgeMultiTracker::setPose(mapOfImages, cMo);
  vpMbKltMultiTracker::setPose(mapOfImages, cMo);
}

void vpMbEdgeKltMultiTracker::setPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                                      const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
{
  vpMbEdgeMultiTracker::setPose(mapOfImages, mapOfCameraPoses);
  vpMbKltMultiTracker::setPose(mapOfImages, mapOfCameraPoses);
}

void vpMbEdgeKltMultiTracker::setProjectionErrorComputation(const bool &flag)
{
  // Set the general flag for 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 vpMbEdgeKltMultiTracker::setReferenceCameraName(const std::string &referenceCameraName)
{
  vpMbEdgeMultiTracker::setReferenceCameraName(referenceCameraName);
  vpMbKltMultiTracker::setReferenceCameraName(referenceCameraName);
  m_referenceCameraName = referenceCameraName;
}

void vpMbEdgeKltMultiTracker::setScanLineVisibilityTest(const bool &v)
{
  vpMbEdgeMultiTracker::setScanLineVisibilityTest(v);
  vpMbKltMultiTracker::setScanLineVisibilityTest(v);
}

void vpMbEdgeKltMultiTracker::setThresholdAcceptation(double th)
{
  vpMbKltMultiTracker::setThresholdAcceptation(th);
}

void vpMbEdgeKltMultiTracker::testTracking()
{
  std::cerr << "The method vpMbEdgeKltMultiTracker::testTracking is not used !" << std::endl;
}

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

  if (it_mbt != m_mapOfEdgeTrackers.end() && it_klt != m_mapOfKltTrackers.end()) {
    std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
    mapOfImages[m_referenceCameraName] = &I;
    track(mapOfImages);
  } else {
    std::stringstream ss;
    ss << "The reference camera: " << m_referenceCameraName << " does not exist !";
    throw vpException(vpTrackingException::fatalError, ss.str().c_str());
  }

  // Set the projection error from the single camera
  if (computeProjError) {
    projectionError = it_mbt->second->getProjectionError();
  }
}

void vpMbEdgeKltMultiTracker::track(const vpImage<vpRGBa> &)
{
  std::cout << "Not supported interface, this class is deprecated." << std::endl;
}

void vpMbEdgeKltMultiTracker::track(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2)
{
  if (m_mapOfEdgeTrackers.size() == 2 && m_mapOfKltTrackers.size() == 2) {
    std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
    std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
    // Assume that the first image is the first name in alphabetic order
    mapOfImages[it->first] = &I1;
    ++it;

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

void vpMbEdgeKltMultiTracker::track(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages)
{
  // Reset the projectionError
  projectionError = 90.0;

  // Check if there is an image for each camera
  // mbt
  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 for edge trackers !");
    }
  }

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

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

  try {
    vpMbKltMultiTracker::preTracking(mapOfImages);
  } catch (...) {
  }

  // MBT: track moving edges
  trackMovingEdges(mapOfImages);

  computeVVS(mapOfImages);

  postTracking(mapOfImages);

  if (computeProjError) {
    vpMbEdgeMultiTracker::computeProjectionError();
  }
}

void vpMbEdgeKltMultiTracker::trackMovingEdges(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages)
{
  for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
       it != m_mapOfEdgeTrackers.end(); ++it) {
    vpMbEdgeTracker *edge = it->second;
    // Track moving edges
    try {
      edge->trackMovingEdge(*mapOfImages[it->first]);
    } catch (...) {
      std::cerr << "Error in moving edge tracking" << std::endl;
      throw;
    }
  }
}

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