vpMbTracker.h

/*
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2025 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See https://visp.inria.fr 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.
 */

#ifndef vpMbTracker_hh
#define vpMbTracker_hh
 
#include <map>
#include <string>
#include <vector>
 
#include <visp3/core/vpConfig.h>
#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
 
BEGIN_VISP_NAMESPACE
class VISP_EXPORT vpMbTracker
{
public:
  typedef enum { GAUSS_NEWTON_OPT = 0, LEVENBERG_MARQUARDT_OPT = 1 } vpMbtOptimizationMethod;
 
protected:
  vpCameraParameters m_cam;
  vpHomogeneousMatrix m_cMo;
  vpMatrix oJo;
  bool m_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 m_nbInitPoints;
  unsigned int m_maxInitPoints;
  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;
  vpImage<unsigned char> m_I;
  bool m_sodb_init_called;
  vpUniRand m_rand;
 
public:
  vpMbTracker();
  vpMbTracker(const vpMbTracker &tracker);
  virtual ~vpMbTracker();
  vpMbTracker &operator=(const vpMbTracker &tracker);

  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 &cam) const { cam = m_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(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(bool orderPolygons = true, bool useVisibility = true, bool clipPolygon = false);

  virtual inline void getPose(vpHomogeneousMatrix &cMo) const { cMo = m_cMo; }

  virtual inline vpHomogeneousMatrix getPose() const { return m_cMo; }
 
  virtual inline double getStopCriteriaEpsilon() const { return m_stopCriteriaEpsilon; }
 
  // initializer
 
#ifdef VISP_HAVE_MODULE_GUI
  virtual void initClick(const vpImage<unsigned char> &I, const std::string &initFile, bool displayHelp = false,
                         const vpHomogeneousMatrix &T = vpHomogeneousMatrix());
  virtual void initClick(const vpImage<vpRGBa> &I_color, const std::string &initFile, 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 = "");
  virtual void initClick(const vpImage<vpRGBa> &I_color, 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<vpRGBa> &I_color, 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 initFromPoints(const vpImage<vpRGBa> &I_color, 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<vpRGBa> &I_color, const std::string &initFile);
 
  virtual void initFromPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
  virtual void initFromPose(const vpImage<vpRGBa> &I_color, const vpHomogeneousMatrix &cMo);
 
  virtual void initFromPose(const vpImage<unsigned char> &I, const vpPoseVector &cPo);
  virtual void initFromPose(const vpImage<vpRGBa> &I_color, const vpPoseVector &cPo);
 
  virtual void loadModel(const std::string &modelFile, bool verbose = false,
                         const vpHomogeneousMatrix &od_M_o = vpHomogeneousMatrix());

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

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

  virtual void setCameraParameters(const vpCameraParameters &cam) { m_cam = cam; }
 
  virtual void setClipping(const unsigned int &flags);

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

  virtual void setDisplayFeatures(bool displayF) { displayFeatures = displayF; }
 
  virtual void setEstimatedDoF(const vpColVector &v);
 
  virtual void setFarClippingDistance(const double &dist);

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

  virtual inline void setLambda(double gain) { m_lambda = gain; }
 
  virtual void setLod(bool useLod, const std::string &name = "");

  virtual inline void setMaxIter(unsigned int max) { m_maxIter = max; }
 
  virtual void setMinLineLengthThresh(double minLineLengthThresh, const std::string &name = "");
 
  virtual void setMinPolygonAreaThresh(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(bool display) { m_projectionErrorDisplay = display; }

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

  virtual void setProjectionErrorDisplayArrowThickness(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(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, unsigned int thickness = 1, bool displayFullModel = false) = 0;
  virtual void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
                       const vpColor &col, unsigned int thickness = 1, bool displayFullModel = false) = 0;
 
  virtual std::vector<std::vector<double> > getModelForDisplay(unsigned int width, unsigned int height,
                                                               const vpHomogeneousMatrix &cMo,
                                                               const vpCameraParameters &cam,
                                                               bool displayFullModel = false) = 0;

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

  virtual void loadConfigFile(const std::string &configFile, bool verbose = true);

  virtual void resetTracker() = 0;

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

  virtual void setPose(const vpImage<vpRGBa> &I_color, const vpHomogeneousMatrix &cdMo) = 0;

  virtual void testTracking() = 0;

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

  virtual void track(const vpImage<vpRGBa> &I) = 0;
 
protected:
  void addPolygon(const std::vector<vpPoint> &corners, int idFace = -1, const std::string &polygonName = "",
                  bool useLod = false, double minPolygonAreaThreshold = 2500.0, double minLineLengthThreshold = 50.0);
  void addPolygon(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace = -1,
                  const std::string &polygonName = "", bool useLod = false, double minPolygonAreaThreshold = 2500.0);
  void addPolygon(const vpPoint &p1, const vpPoint &p2, int idFace = -1, const std::string &polygonName = "",
                  bool useLod = false, double minLineLengthThreshold = 50);
  void addPolygon(const std::vector<std::vector<vpPoint> > &listFaces, int idFace = -1,
                  const std::string &polygonName = "", bool useLod = false, double minLineLengthThreshold = 50);
 
  void addProjectionErrorCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, double r, int idFace = -1,
                                const std::string &name = "");
  void addProjectionErrorCylinder(const vpPoint &P1, const vpPoint &P2, 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, int idFace = -1,
                                 const std::string &polygonName = "", bool useLod = false,
                                 double minPolygonAreaThreshold = 2500.0, const double minLineLengthThreshold = 50.0);
  void addProjectionErrorPolygon(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius,
                                 int idFace = -1, const std::string &polygonName = "", bool useLod = false,
                                 double minPolygonAreaThreshold = 2500.0);
  void addProjectionErrorPolygon(const vpPoint &p1, const vpPoint &p2, int idFace = -1,
                                 const std::string &polygonName = "", bool useLod = false,
                                 double minLineLengthThreshold = 50);
  void addProjectionErrorPolygon(const std::vector<std::vector<vpPoint> > &listFaces, int idFace = -1,
                                 const std::string &polygonName = "", bool useLod = false,
                                 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(unsigned int iter, vpColVector &error, const vpColVector &m_error_prev,
                                                 const vpHomogeneousMatrix &cMoPrev, double &mu,
                                                 bool &reStartFromLastIncrement, vpColVector *const w = nullptr,
                                                 const vpColVector *const m_w_prev = nullptr);
  virtual void computeVVSInit() = 0;
  virtual void computeVVSInteractionMatrixAndResidu() = 0;
  virtual void computeVVSPoseEstimation(const bool isoJoIdentity, 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 = nullptr,
                                        vpColVector *const m_w_prev = nullptr);
  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, double radius, int idFace = 0,
                          const std::string &name = "") = 0;
 
#ifdef VISP_HAVE_MODULE_GUI
  virtual void initClick(const vpImage<unsigned char> *const I, const vpImage<vpRGBa> *const I_color,
                         const std::string &initFile, bool displayHelp = false,
                         const vpHomogeneousMatrix &od_M_o = vpHomogeneousMatrix());
 
  virtual void initClick(const vpImage<unsigned char> *const I, const vpImage<vpRGBa> *const I_color,
                         const std::vector<vpPoint> &points3D_list, const std::string &displayFile = "");
#endif
 
  virtual void initFromPoints(const vpImage<unsigned char> *const I, const vpImage<vpRGBa> *const I_color,
                              const std::string &initFile);
 
  virtual void initFromPoints(const vpImage<unsigned char> *const I, const vpImage<vpRGBa> *const I_color,
                              const std::vector<vpImagePoint> &points2D_list,
                              const std::vector<vpPoint> &points3D_list);
 
  virtual void initFromPose(const vpImage<unsigned char> *const I, const vpImage<vpRGBa> *const I_color,
                            const std::string &initFile);

  virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, 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, double radius, int idFace = 0,
                                 const std::string &name = "");
  void initProjectionErrorCylinder(const vpPoint &p1, const vpPoint &p2, double radius, 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, bool verbose = false, bool parent = true,
                            const vpHomogeneousMatrix &T = vpHomogeneousMatrix());
  void loadInitFile(const std::string &initFile, std::vector<std::string> &vectorOfInitFilename,
                    bool parent, const vpHomogeneousMatrix &T, std::vector<vpPoint> &P);
  void projectionErrorInitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &_cMo);
  void projectionErrorResetMovingEdges();
  void projectionErrorVisibleFace(unsigned int width, unsigned int height, const vpHomogeneousMatrix &_cMo);
 
  void removeCommentsAndEmptyLines(std::ifstream &fileId);
 
  std::map<std::string, std::string> parseParameters(std::string &endLine);
 
  bool samePoint(const vpPoint &P1, const vpPoint &P2) const;
};
END_VISP_NAMESPACE
#endif