vpMbTracker.h

/****************************************************************************
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * ("GPL") version 2 as published by the Free Software Foundation.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See http://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 <vector>
#include <string>
#include <sstream>
#include <fstream>
#include <map>
#include <algorithm>
#include <cctype>
#include <locale>
#include <functional>   // std::not1


#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImagePoint.h>
#include <visp3/core/vpColVector.h>
#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpRGBa.h>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpPoint.h>
#include <visp3/mbt/vpMbtPolygon.h>
#include <visp3/mbt/vpMbHiddenFaces.h>
#include <visp3/core/vpPolygon.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;
  vpColVector m_w;
  vpColVector m_error;
  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;

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

  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.");
    
    return covarianceMatrix; 
  }

  virtual double getProjectionError() const { return projectionError; }

  virtual vpColVector getEstimatedDoF() const;

  virtual vpColVector getError() const {
    return m_error;
  }

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

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

  virtual vpColVector getRobustWeights() const {
    return m_w;
  }

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

  // Intializer

#ifdef VISP_HAVE_MODULE_GUI
  virtual void initClick( const vpImage<unsigned char>& I, const std::string& initFile, const bool displayHelp = false );
  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 char *modelFile, const bool verbose=false);
  virtual void loadModel(const std::string &modelFile, const bool verbose=false);

  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 void setLod(const bool useLod, const std::string &name="");

  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; }
    
  virtual void setProjectionErrorComputation(const bool &flag) { computeProjError = flag; }

  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

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

  inline 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)=0;

  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 createCylinderBBox(const vpPoint& p1, const vpPoint &p2, const double &radius, std::vector<std::vector<vpPoint> > &listFaces);

  void computeJTR(const vpMatrix& J, const vpColVector& R, vpColVector& JTR) const;
  
#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;

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

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


#endif