vpMbEdgeMultiTracker.h

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

#ifndef _vpMbEdgeMultiTracker_h_
#define _vpMbEdgeMultiTracker_h_

#include <visp3/core/vpConfig.h>

#include <iostream>
#include <vector>

#include <visp3/mbt/vpMbEdgeTracker.h>

#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)

class VISP_EXPORT vp_deprecated vpMbEdgeMultiTracker : public vpMbEdgeTracker
{
protected:
  std::map<std::string, vpHomogeneousMatrix> m_mapOfCameraTransformationMatrix;

  std::map<std::string, vpMbEdgeTracker *> m_mapOfEdgeTrackers;

  std::map<std::string, std::vector<const vpImage<unsigned char> *> > m_mapOfPyramidalImages;

  std::string m_referenceCameraName;
  vpMatrix m_L_edgeMulti;
  vpColVector m_error_edgeMulti;
  vpColVector m_w_edgeMulti;
  vpColVector m_weightedError_edgeMulti;

public:
  // Default constructor <==> equivalent to vpMbEdgeTracker
  vpMbEdgeMultiTracker();
  // Constructor with a specified number of cameras, camera names are
  // generated
  explicit vpMbEdgeMultiTracker(unsigned int nbCameras);
  // Constructor with a specified list of camera names
  explicit vpMbEdgeMultiTracker(const std::vector<std::string> &cameraNames);

  virtual ~vpMbEdgeMultiTracker();

  virtual void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
                       const vpColor &col, unsigned int thickness = 1, bool displayFullModel = false);

  virtual void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
                       const vpColor &col, unsigned int thickness = 1, bool displayFullModel = false);

  virtual void 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, unsigned int thickness = 1,
                       bool displayFullModel = false);

  virtual void display(const vpImage<vpRGBa> &I1, const vpImage<vpRGBa> &I2, const vpHomogeneousMatrix &c1Mo,
                       const vpHomogeneousMatrix &c2Mo, const vpCameraParameters &cam1, const vpCameraParameters &cam2,
                       const vpColor &col, unsigned int thickness = 1, bool displayFullModel = false);

  virtual void 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 = 1, bool displayFullModel = false);

  virtual void 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 = 1, bool displayFullModel = false);

  virtual std::vector<std::vector<double> > getModelForDisplay(unsigned int, unsigned int,
                                                               const vpHomogeneousMatrix &,
                                                               const vpCameraParameters &,
                                                               bool =false)
  {
    std::cerr << "Not implemented. Deprecated class." << std::endl;
    return std::vector<std::vector<double> >();
  }

  virtual std::vector<std::string> getCameraNames() const;

  virtual void getCameraParameters(vpCameraParameters &camera) const;
  virtual void getCameraParameters(vpCameraParameters &cam1, vpCameraParameters &cam2) const;
  virtual void getCameraParameters(const std::string &cameraName, vpCameraParameters &camera) const;
  virtual void getCameraParameters(std::map<std::string, vpCameraParameters> &mapOfCameraParameters) const;

  using vpMbTracker::getClipping;
  virtual unsigned int getClipping(const std::string &cameraName) const;

  virtual vpMbHiddenFaces<vpMbtPolygon> &getFaces();
  virtual vpMbHiddenFaces<vpMbtPolygon> &getFaces(const std::string &cameraName);
  virtual std::map<std::string, vpMbHiddenFaces<vpMbtPolygon> > getFaces() const;

  void getLcircle(std::list<vpMbtDistanceCircle *> &circlesList, unsigned int level = 0) const;
  virtual void getLcircle(const std::string &cameraName, std::list<vpMbtDistanceCircle *> &circlesList,
                          unsigned int level = 0) const;

  void getLcylinder(std::list<vpMbtDistanceCylinder *> &cylindersList, unsigned int level = 0) const;
  virtual void getLcylinder(const std::string &cameraName, std::list<vpMbtDistanceCylinder *> &cylindersList,
                            unsigned int level = 0) const;

  void getLline(std::list<vpMbtDistanceLine *> &linesList, unsigned int level = 0) const;
  virtual void getLline(const std::string &cameraName, std::list<vpMbtDistanceLine *> &linesList,
                        unsigned int level = 0) const;

  virtual void getMovingEdge(vpMe &p_me) const;
  virtual vpMe getMovingEdge() const;
  virtual void getMovingEdge(const std::string &cameraName, vpMe &p_me) const;
  virtual vpMe getMovingEdge(const std::string &cameraName) const;

  virtual unsigned int getNbPoints(unsigned int level = 0) const;
  virtual unsigned int getNbPoints(const std::string &cameraName, unsigned int level = 0) const;

  virtual unsigned int getNbPolygon() const;
  virtual unsigned int getNbPolygon(const std::string &cameraName) const;
  virtual std::map<std::string, unsigned int> getMultiNbPolygon() const;

  inline unsigned int getNumberOfCameras() const { return (unsigned int)m_mapOfEdgeTrackers.size(); }

  using vpMbTracker::getPose;
  virtual void getPose(vpHomogeneousMatrix &c1Mo, vpHomogeneousMatrix &c2Mo) const;
  virtual void getPose(const std::string &cameraName, vpHomogeneousMatrix &cMo) const;
  virtual void getPose(std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses) const;

  virtual inline vpColVector getError() const { return m_error_edgeMulti; }

  virtual inline vpColVector getRobustWeights() const { return m_w_edgeMulti; }

  void init(const vpImage<unsigned char> &I);

#ifdef VISP_HAVE_MODULE_GUI
  using vpMbTracker::initClick;
  virtual void initClick(const vpImage<unsigned char> &I, const std::vector<vpPoint> &points3D_list,
                         const std::string &displayFile = "");

  virtual void initClick(const vpImage<unsigned char> &I, const std::string &initFile, bool displayHelp = false,
                         const vpHomogeneousMatrix &T=vpHomogeneousMatrix());

  virtual void initClick(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
                         const std::string &initFile1, const std::string &initFile2, bool displayHelp = false,
                         bool firstCameraIsReference = true);

  virtual void initClick(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                         const std::string &initFile, bool displayHelp = false);

  virtual void initClick(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                         const std::map<std::string, std::string> &mapOfInitFiles, bool displayHelp = false);
#endif

  using vpMbTracker::initFromPose;
  virtual void initFromPose(const vpImage<unsigned char> &I, const std::string &initFile);
  virtual void initFromPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
  virtual void initFromPose(const vpImage<unsigned char> &I, const vpPoseVector &cPo);

  virtual void initFromPose(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
                            const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
                            bool firstCameraIsReference = true);
  virtual void initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                            const vpHomogeneousMatrix &cMo);
  virtual void initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                            const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses);

  virtual void loadConfigFile(const std::string &configFile);

  virtual void loadConfigFile(const std::string &configFile1, const std::string &configFile2,
                              bool firstCameraIsReference = true);

  virtual void loadConfigFile(const std::map<std::string, std::string> &mapOfConfigFiles);

  virtual void loadModel(const std::string &modelFile, bool verbose = false,
                         const vpHomogeneousMatrix &T=vpHomogeneousMatrix());

  virtual void reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
                           const vpHomogeneousMatrix &cMo, bool verbose = false,
                           const vpHomogeneousMatrix &T=vpHomogeneousMatrix());
  virtual void reInitModel(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
                           const std::string &cad_name, const vpHomogeneousMatrix &c1Mo,
                           const vpHomogeneousMatrix &c2Mo, bool verbose = false,
                           bool firstCameraIsReference = true);
  virtual void 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 = false);

  virtual void resetTracker();

  virtual void setAngleAppear(const double &a);
  virtual void setAngleDisappear(const double &a);

  virtual void setCameraParameters(const vpCameraParameters &camera);

  virtual void setCameraParameters(const vpCameraParameters &camera1, const vpCameraParameters &camera2,
                                   bool firstCameraIsReference = true);

  virtual void setCameraParameters(const std::string &cameraName, const vpCameraParameters &camera);

  virtual void setCameraParameters(const std::map<std::string, vpCameraParameters> &mapOfCameraParameters);

  virtual void setCameraTransformationMatrix(const std::string &cameraName,
                                             const vpHomogeneousMatrix &cameraTransformationMatrix);

  virtual void
  setCameraTransformationMatrix(const std::map<std::string, vpHomogeneousMatrix> &mapOfTransformationMatrix);

  virtual void setClipping(const unsigned int &flags);
  virtual void setClipping(const std::string &cameraName, const unsigned int &flags);

  virtual void setCovarianceComputation(const bool &flag);

  virtual void setDisplayFeatures(bool displayF);

  virtual void setFarClippingDistance(const double &dist);
  virtual void setFarClippingDistance(const std::string &cameraName, const double &dist);

  virtual void setGoodMovingEdgesRatioThreshold(double threshold);

#ifdef VISP_HAVE_OGRE

  void setGoodNbRayCastingAttemptsRatio(const double &ratio);

  void setNbRayCastingAttemptsForVisibility(const unsigned int &attempts);
#endif

  virtual void setLod(bool useLod, const std::string &name = "");
  virtual void setLod(bool useLod, const std::string &cameraName, const std::string &name);

  virtual void setMinLineLengthThresh(double minLineLengthThresh, const std::string &name = "");
  virtual void setMinLineLengthThresh(double minLineLengthThresh, const std::string &cameraName,
                                      const std::string &name);

  virtual void setMinPolygonAreaThresh(double minPolygonAreaThresh, const std::string &name = "");
  virtual void setMinPolygonAreaThresh(double minPolygonAreaThresh, const std::string &cameraName,
                                       const std::string &name);

  virtual void setMovingEdge(const vpMe &moving_edge);
  virtual void setMovingEdge(const std::string &cameraName, const vpMe &moving_edge);

  virtual void setNearClippingDistance(const double &dist);
  virtual void setNearClippingDistance(const std::string &cameraName, const double &dist);

  virtual void setOgreShowConfigDialog(bool showConfigDialog);

  virtual void setOgreVisibilityTest(const bool &v);

  virtual void setOptimizationMethod(const vpMbtOptimizationMethod &opt);

  virtual void setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
  virtual void setPose(const vpImage<vpRGBa> &I_color, const vpHomogeneousMatrix &cMo);

  virtual void setPose(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
                       const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
                       bool firstCameraIsReference = true);

  virtual void setPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                       const vpHomogeneousMatrix &cMo);

  virtual void setPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                       const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses);

  virtual void setProjectionErrorComputation(const bool &flag);

  virtual void setReferenceCameraName(const std::string &referenceCameraName);

  virtual void setScales(const std::vector<bool> &scales);

  virtual void setScanLineVisibilityTest(const bool &v);

  virtual void setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking);

  using vpMbEdgeTracker::track;
  virtual void track(const vpImage<unsigned char> &I);
  virtual void track(const vpImage<vpRGBa> &I_color);
  virtual void track(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2);
  virtual void track(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages);

protected:
  enum FeatureType { LINE, CYLINDER, CIRCLE };

  virtual void cleanPyramid(std::map<std::string, std::vector<const vpImage<unsigned char> *> > &pyramid);

  virtual void computeProjectionError();

  virtual void computeVVS(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages, unsigned int lvl);
  virtual void computeVVSFirstPhasePoseEstimation(unsigned int iter, bool &isoJoIdentity_);
  virtual void computeVVSInit();
  virtual void computeVVSInteractionMatrixAndResidu();
  using vpMbEdgeTracker::computeVVSInteractionMatrixAndResidu;
  virtual void computeVVSInteractionMatrixAndResidu(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                                                    std::map<std::string, vpVelocityTwistMatrix> &mapOfVelocityTwist);
  virtual void computeVVSWeights();
  using vpMbTracker::computeVVSWeights;

  virtual void initPyramid(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
                           std::map<std::string, std::vector<const vpImage<unsigned char> *> > &pyramid);
};

#endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
#endif //_vpMbEdgeMultiTracker_h_