vpMbTracker.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:
 * Generic model based tracker. This class declares the methods to implement
 *in order to have a model based tracker.
 *
 * Authors:
 * Romain Tallonneau
 * Aurelien Yol
 *
 *****************************************************************************/

#ifndef vpMbTracker_hh
#define vpMbTracker_hh

#include <algorithm>
#include <cctype>
#include <fstream>
#include <functional> // std::not1
#include <locale>
#include <map>
#include <sstream>
#include <string>
#include <vector>

#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpColVector.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImagePoint.h>
#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpPoint.h>
#include <visp3/core/vpPolygon.h>
#include <visp3/core/vpRGBa.h>
#include <visp3/core/vpRobust.h>
#include <visp3/mbt/vpMbHiddenFaces.h>
#include <visp3/mbt/vpMbtPolygon.h>

#include <visp3/mbt/vpMbtDistanceCircle.h>
#include <visp3/mbt/vpMbtDistanceCylinder.h>
#include <visp3/mbt/vpMbtDistanceLine.h>

#ifdef VISP_HAVE_COIN3D
// Work around to avoid type redefinition int8_t with Coin
// #if defined(_WIN32) && defined(VISP_HAVE_OGRE) && (_MSC_VER >= 1600) //
// Visual Studio 2010
//   #define HAVE_INT8_T 1
// #endif

// Inventor includes
#include <Inventor/VRMLnodes/SoVRMLGroup.h>
#include <Inventor/VRMLnodes/SoVRMLIndexedFaceSet.h>
#include <Inventor/VRMLnodes/SoVRMLIndexedLineSet.h>
#endif

class VISP_EXPORT vpMbTracker
{
public:
  typedef enum { GAUSS_NEWTON_OPT = 0, LEVENBERG_MARQUARDT_OPT = 1 } vpMbtOptimizationMethod;

protected:
  vpCameraParameters cam;
  vpHomogeneousMatrix cMo;
  vpMatrix oJo;
  bool isoJoIdentity;
  std::string modelFileName;
  bool modelInitialised;
  std::string poseSavingFilename;
  bool computeCovariance;
  vpMatrix covarianceMatrix;
  bool computeProjError;
  double projectionError;
  bool displayFeatures;
  vpMbtOptimizationMethod m_optimizationMethod;

  vpMbHiddenFaces<vpMbtPolygon> faces;
  double angleAppears;
  double angleDisappears;
  double distNearClip;
  double distFarClip;
  unsigned int clippingFlag;
  bool useOgre;
  bool ogreShowConfigDialog;
  bool useScanLine;
  unsigned int nbPoints;
  unsigned int nbLines;
  unsigned int nbPolygonLines;
  unsigned int nbPolygonPoints;
  unsigned int nbCylinders;
  unsigned int nbCircles;
  bool useLodGeneral;
  bool applyLodSettingInConfig;
  double minLineLengthThresholdGeneral;
  double minPolygonAreaThresholdGeneral;
  std::map<std::string, std::string> mapOfParameterNames;
  bool m_computeInteraction;
  double m_lambda;
  unsigned int m_maxIter;
  double m_stopCriteriaEpsilon;
  double m_initialMu;

  std::vector<vpMbtDistanceLine *> m_projectionErrorLines;
  std::vector<vpMbtDistanceCylinder *> m_projectionErrorCylinders;
  std::vector<vpMbtDistanceCircle *> m_projectionErrorCircles;
  vpMbHiddenFaces<vpMbtPolygon> m_projectionErrorFaces;
  bool m_projectionErrorOgreShowConfigDialog;
  vpMe m_projectionErrorMe;
  unsigned int m_projectionErrorKernelSize;
  vpMatrix m_SobelX;
  vpMatrix m_SobelY;
  bool m_projectionErrorDisplay;
  unsigned int m_projectionErrorDisplayLength;
  unsigned int m_projectionErrorDisplayThickness;
  vpCameraParameters m_projectionErrorCam;
  const vpImage<bool> *m_mask;

public:
  vpMbTracker();
  virtual ~vpMbTracker();

  virtual double computeCurrentProjectionError(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &_cMo,
                                               const vpCameraParameters &_cam);

  virtual inline double getAngleAppear() const { return angleAppears; }

  virtual inline double getAngleDisappear() const { return angleDisappears; }

  virtual void getCameraParameters(vpCameraParameters &camera) const { camera = this->cam; }

  virtual inline unsigned int getClipping() const { return clippingFlag; }

  virtual vpMatrix getCovarianceMatrix() const
  {
    if (!computeCovariance) {
      //      vpTRACE("Warning : The covariance matrix has not been computed.
      //      See setCovarianceComputation() to do it.");
      std::cerr << "Warning : The covariance matrix has not been computed. "
                   "See setCovarianceComputation() to do it."
                << std::endl;
    }

    return covarianceMatrix;
  }

  virtual inline double getInitialMu() const { return m_initialMu; }

  virtual inline double getLambda() const { return m_lambda; }

  virtual inline unsigned int getMaxIter() const { return m_maxIter; }

  virtual double getProjectionError() const { return projectionError; }

  virtual vpColVector getEstimatedDoF() const;

  virtual vpColVector getError() const = 0;

  virtual inline vpMbHiddenFaces<vpMbtPolygon> &getFaces() { return faces; }

  virtual inline double getFarClippingDistance() const { return distFarClip; }

  virtual vpColVector getRobustWeights() const = 0;

  virtual inline unsigned int getNbPolygon() const { return static_cast<unsigned int>(faces.size()); }

  virtual inline double getNearClippingDistance() const { return distNearClip; }

  virtual inline vpMbtOptimizationMethod getOptimizationMethod() const { return m_optimizationMethod; }

  virtual inline vpMbtPolygon *getPolygon(const unsigned int index)
  {
    if (index >= static_cast<unsigned int>(faces.size())) {
      throw vpException(vpException::dimensionError, "index out of range");
    }

    return faces[index];
  }

  virtual std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > >
  getPolygonFaces(const bool orderPolygons = true, const bool useVisibility = true, const bool clipPolygon = false);

  virtual inline void getPose(vpHomogeneousMatrix &cMo_) const { cMo_ = this->cMo; }

  virtual inline vpHomogeneousMatrix getPose() const { return this->cMo; }

  virtual inline double getStopCriteriaEpsilon() const { return m_stopCriteriaEpsilon; }

// Intializer

#ifdef VISP_HAVE_MODULE_GUI
  virtual void initClick(const vpImage<unsigned char> &I, const std::string &initFile, const bool displayHelp = false,
                         const vpHomogeneousMatrix &T=vpHomogeneousMatrix());
  virtual void initClick(const vpImage<unsigned char> &I, const std::vector<vpPoint> &points3D_list,
                         const std::string &displayFile = "");
#endif

  virtual void initFromPoints(const vpImage<unsigned char> &I, const std::string &initFile);
  virtual void initFromPoints(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &points2D_list,
                              const std::vector<vpPoint> &points3D_list);

  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 loadModel(const std::string &modelFile, const bool verbose = false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix());

  virtual inline void setAngleAppear(const double &a) { angleAppears = a; }

  virtual inline void setAngleDisappear(const double &a) { angleDisappears = a; }

  virtual void setCameraParameters(const vpCameraParameters &camera) { this->cam = camera; }

  virtual void setClipping(const unsigned int &flags);

  virtual void setCovarianceComputation(const bool &flag) { computeCovariance = flag; }

  virtual void setDisplayFeatures(const bool displayF) { displayFeatures = displayF; }

  virtual void setEstimatedDoF(const vpColVector &v);

  virtual void setFarClippingDistance(const double &dist);

  virtual inline void setInitialMu(const double mu) { m_initialMu = mu; }

  virtual inline void setLambda(const double gain) { m_lambda = gain; }

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

  virtual inline void setMaxIter(const unsigned int max) { m_maxIter = max; }

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

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

  virtual void setNearClippingDistance(const double &dist);

  virtual inline void setOptimizationMethod(const vpMbtOptimizationMethod &opt) { m_optimizationMethod = opt; }

  void setProjectionErrorMovingEdge(const vpMe &me);

  void setProjectionErrorKernelSize(const unsigned int &size);

  virtual void setMask(const vpImage<bool> &mask) { m_mask = &mask; }

  virtual inline void setStopCriteriaEpsilon(const double eps) { m_stopCriteriaEpsilon = eps; }

  virtual void setProjectionErrorComputation(const bool &flag) { computeProjError = flag; }

  virtual void setProjectionErrorDisplay(const bool display) { m_projectionErrorDisplay = display; }

  virtual void setProjectionErrorDisplayArrowLength(const unsigned int length) { m_projectionErrorDisplayLength = length; }

  virtual void setProjectionErrorDisplayArrowThickness(const unsigned int thickness) { m_projectionErrorDisplayThickness = thickness; }

  virtual void setScanLineVisibilityTest(const bool &v) { useScanLine = v; }

  virtual void setOgreVisibilityTest(const bool &v);

  void savePose(const std::string &filename) const;

#ifdef VISP_HAVE_OGRE

  virtual void setGoodNbRayCastingAttemptsRatio(const double &ratio) { faces.setGoodNbRayCastingAttemptsRatio(ratio); }
  virtual void setNbRayCastingAttemptsForVisibility(const unsigned int &attempts)
  {
    faces.setNbRayCastingAttemptsForVisibility(attempts);
  }
#endif

  inline virtual void setOgreShowConfigDialog(const bool showConfigDialog) { ogreShowConfigDialog = showConfigDialog; }

  inline void setPoseSavingFilename(const std::string &filename) { poseSavingFilename = filename; }

  /* PURE VIRTUAL METHODS */

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

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

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

  virtual void resetTracker() = 0;

  virtual void setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cdMo) = 0;

  virtual void testTracking() = 0;

  virtual void track(const vpImage<unsigned char> &I) = 0;

protected:
  void addPolygon(const std::vector<vpPoint> &corners, const int idFace = -1, const std::string &polygonName = "",
                  const bool useLod = false, const double minPolygonAreaThreshold = 2500.0,
                  const double minLineLengthThreshold = 50.0);
  void addPolygon(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius, const int idFace = -1,
                  const std::string &polygonName = "", const bool useLod = false,
                  const double minPolygonAreaThreshold = 2500.0);
  void addPolygon(const vpPoint &p1, const vpPoint &p2, const int idFace = -1, const std::string &polygonName = "",
                  const bool useLod = false, const double minLineLengthThreshold = 50);
  void addPolygon(const std::vector<std::vector<vpPoint> > &listFaces, const int idFace = -1,
                  const std::string &polygonName = "", const bool useLod = false,
                  const double minLineLengthThreshold = 50);

  void addProjectionErrorCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, int idFace = -1,
                                const std::string &name = "");
  void addProjectionErrorCylinder(const vpPoint &P1, const vpPoint &P2, const double r, int idFace = -1, const std::string &name = "");
  void addProjectionErrorLine(vpPoint &p1, vpPoint &p2, int polygon = -1, std::string name = "");

  void addProjectionErrorPolygon(const std::vector<vpPoint> &corners, const int idFace = -1, const std::string &polygonName = "",
                                 const bool useLod = false, const double minPolygonAreaThreshold = 2500.0,
                                 const double minLineLengthThreshold = 50.0);
  void addProjectionErrorPolygon(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius, const int idFace = -1,
                                 const std::string &polygonName = "", const bool useLod = false,
                                 const double minPolygonAreaThreshold = 2500.0);
  void addProjectionErrorPolygon(const vpPoint &p1, const vpPoint &p2, const int idFace = -1, const std::string &polygonName = "",
                                 const bool useLod = false, const double minLineLengthThreshold = 50);
  void addProjectionErrorPolygon(const std::vector<std::vector<vpPoint> > &listFaces, const int idFace = -1,
                                 const std::string &polygonName = "", const bool useLod = false,
                                 const double minLineLengthThreshold = 50);

  void createCylinderBBox(const vpPoint &p1, const vpPoint &p2, const double &radius,
                          std::vector<std::vector<vpPoint> > &listFaces);

  virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true,
                                          const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true,
                                          const vpMatrix &LVJ_true, const vpColVector &error);

  void computeJTR(const vpMatrix &J, const vpColVector &R, vpColVector &JTR) const;

  double computeProjectionErrorImpl(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &_cMo,
                                    const vpCameraParameters &_cam, unsigned int &nbFeatures);

  virtual void computeVVSCheckLevenbergMarquardt(const unsigned int iter, vpColVector &error,
                                                 const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev,
                                                 double &mu, bool &reStartFromLastIncrement,
                                                 vpColVector *const w = NULL, const vpColVector *const m_w_prev = NULL);
  virtual void computeVVSInit() = 0;
  virtual void computeVVSInteractionMatrixAndResidu() = 0;
  virtual void computeVVSPoseEstimation(const bool isoJoIdentity_, const unsigned int iter, vpMatrix &L, vpMatrix &LTL,
                                        vpColVector &R, const vpColVector &error, vpColVector &error_prev,
                                        vpColVector &LTR, double &mu, vpColVector &v, const vpColVector *const w = NULL,
                                        vpColVector *const m_w_prev = NULL);
  virtual void computeVVSWeights(vpRobust &robust, const vpColVector &error, vpColVector &w);

#ifdef VISP_HAVE_COIN3D
  virtual void extractGroup(SoVRMLGroup *sceneGraphVRML2, vpHomogeneousMatrix &transform, int &idFace);
  virtual void extractFaces(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace,
                            const std::string &polygonName = "");
  virtual void extractLines(SoVRMLIndexedLineSet *line_set, int &idFace, const std::string &polygonName = "");
  virtual void extractCylinders(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace,
                                const std::string &polygonName = "");
#endif

  vpPoint getGravityCenter(const std::vector<vpPoint> &_pts) const;

  virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius,
                          const int idFace = 0, const std::string &name = "") = 0;
  virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace = 0,
                            const std::string &name = "") = 0;

  virtual void initFaceFromCorners(vpMbtPolygon &polygon) = 0;
  virtual void initFaceFromLines(vpMbtPolygon &polygon) = 0;

  void initProjectionErrorCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius,
                                 const int idFace = 0, const std::string &name = "");
  void initProjectionErrorCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace = 0,
                                   const std::string &name = "");
  void initProjectionErrorFaceFromCorners(vpMbtPolygon &polygon);
  void initProjectionErrorFaceFromLines(vpMbtPolygon &polygon);

  virtual void loadVRMLModel(const std::string &modelFile);
  virtual void loadCAOModel(const std::string &modelFile, std::vector<std::string> &vectorOfModelFilename,
                            int &startIdFace, const bool verbose = false, const bool parent = true,
                            const vpHomogeneousMatrix &T=vpHomogeneousMatrix());

  void projectionErrorInitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &_cMo);
  void projectionErrorResetMovingEdges();
  void projectionErrorVisibleFace(const vpImage<unsigned char> &_I, const vpHomogeneousMatrix &_cMo);

  void removeComment(std::ifstream &fileId);

  inline bool parseBoolean(std::string &input)
  {
    std::transform(input.begin(), input.end(), input.begin(), ::tolower);
    std::istringstream is(input);
    bool b;
    // Parse string to boolean either in the textual representation
    // (True/False)  or in numeric representation (1/0)
    is >> (input.size() > 1 ? std::boolalpha : std::noboolalpha) >> b;
    return b;
  }

  std::map<std::string, std::string> parseParameters(std::string &endLine);

  inline std::string &ltrim(std::string &s) const
  {
    s.erase(s.begin(), std::find_if(s.begin(), s.end(), std::not1(std::ptr_fun<int, int>(std::isspace))));
    return s;
  }

  inline std::string &rtrim(std::string &s) const
  {
    s.erase(std::find_if(s.rbegin(), s.rend(), std::not1(std::ptr_fun<int, int>(std::isspace))).base(), s.end());
    return s;
  }

  inline std::string &trim(std::string &s) const { return ltrim(rtrim(s)); }

  bool samePoint(const vpPoint &P1, const vpPoint &P2) const;
};

#endif