vpPose.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:
 * Pose computation.
 *
 * Authors:
 * Eric Marchand
 * Francois Chaumette
 * Aurelien Yol
 *
 *****************************************************************************/

#ifndef vpPOSE_HH
#define vpPOSE_HH

#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/vision/vpHomography.h>
#include <visp3/core/vpPoint.h>
#include <visp3/core/vpRGBa.h>
#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
#  include <visp3/core/vpList.h>
#endif
#include <visp3/core/vpThread.h>

#include <math.h>
#include <list>
#include <vector>

class VISP_EXPORT vpPose
{  
public:
  typedef enum
    {
      LAGRANGE         , 
      DEMENTHON        , 
      LOWE             , 
      RANSAC           , 
      LAGRANGE_LOWE    , 
      DEMENTHON_LOWE   , 
      VIRTUAL_VS       , 
      DEMENTHON_VIRTUAL_VS, 
      LAGRANGE_VIRTUAL_VS   
    } vpPoseMethodType;

  enum FILTERING_RANSAC_FLAGS {
    PREFILTER_DUPLICATE_POINTS        = 0x1,  
    PREFILTER_ALMOST_DUPLICATE_POINTS = 0x2,  
    PREFILTER_DEGENERATE_POINTS       = 0x4,  
    CHECK_DEGENERATE_POINTS           = 0x8   
  };

  unsigned int npt ;             
  std::list<vpPoint> listP ;     

  double residual ;     

protected :
  double lambda ;

private:
  int vvsIterMax ; 
  std::vector<vpPoint> c3d ;
  bool computeCovariance;
  vpMatrix covarianceMatrix;
  
  unsigned int ransacNbInlierConsensus;
  int ransacMaxTrials;
  std::vector<vpPoint> ransacInliers;
  std::vector<unsigned int> ransacInlierIndex;
  double ransacThreshold;
  double distanceToPlaneForCoplanarityTest;
  int ransacFlags;
  std::vector<vpPoint> listOfPoints;
  bool useParallelRansac;
  int nbParallelRansacThreads;


  //For parallel RANSAC
  class RansacFunctor {
  public:
    RansacFunctor(const vpHomogeneousMatrix &cMo_,
                  const unsigned int ransacNbInlierConsensus_, const int ransacMaxTrials_,
                  const double ransacThreshold_, const unsigned int initial_seed_,
                  const bool checkDegeneratePoints_, const std::vector<vpPoint> &listOfUniquePoints_,
                  bool (*func_)(vpHomogeneousMatrix *)) :
      m_best_consensus(), m_checkDegeneratePoints(checkDegeneratePoints_), m_cMo(cMo_), m_foundSolution(false),
      m_func(func_), m_initial_seed(initial_seed_), m_listOfUniquePoints(listOfUniquePoints_), m_nbInliers(0),
      m_ransacMaxTrials(ransacMaxTrials_), m_ransacNbInlierConsensus(ransacNbInlierConsensus_), m_ransacThreshold(ransacThreshold_) {
    }

    RansacFunctor() :
      m_best_consensus(),m_checkDegeneratePoints(false), m_cMo(), m_foundSolution(false), m_func(NULL),
      m_initial_seed(0), m_listOfUniquePoints(), m_nbInliers(0), m_ransacMaxTrials(), m_ransacNbInlierConsensus(),
      m_ransacThreshold() {
    }

    void operator()() {
      m_foundSolution = poseRansacImpl();
    }

    // Access the return value.
    bool getResult() const {
      return m_foundSolution;
    }

    std::vector<unsigned int> getBestConsensus() const {
      return m_best_consensus;
    }

    vpHomogeneousMatrix getEstimatedPose() const {
      return m_cMo;
    }

    unsigned int getNbInliers() const {
      return m_nbInliers;
    }

  private:
    std::vector<unsigned int> m_best_consensus;
    bool m_checkDegeneratePoints;
    vpHomogeneousMatrix m_cMo;
    bool m_foundSolution;
    bool (*m_func)(vpHomogeneousMatrix *);
    unsigned int m_initial_seed;
    std::vector<vpPoint> m_listOfUniquePoints;
    unsigned int m_nbInliers;
    int m_ransacMaxTrials;
    unsigned int m_ransacNbInlierConsensus;
    double m_ransacThreshold;

    bool poseRansacImpl();
  };

#if defined(VISP_HAVE_PTHREAD) || (defined(_WIN32) && !defined(WINRT_8_0))
  static vpThread::Return poseRansacImplThread(vpThread::Args arg);
#endif


protected:
  double computeResidualDementhon(const vpHomogeneousMatrix &cMo) ;

  // method used in poseDementhonPlan()
  int calculArbreDementhon(vpMatrix &b, vpColVector &U, vpHomogeneousMatrix &cMo) ;

public:
  vpPose() ;
  virtual ~vpPose() ;
  void addPoint(const vpPoint& P) ;
  void addPoints(const std::vector<vpPoint>& lP);
  void clearPoint() ;

  bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool (*func)(vpHomogeneousMatrix *)=NULL) ;
  double computeResidual(const vpHomogeneousMatrix &cMo) const ;
  bool coplanar(int &coplanar_plane_type) ;
  void displayModel(vpImage<unsigned char> &I,
                    vpCameraParameters &cam,
                    vpColor col=vpColor::none) ;
  void displayModel(vpImage<vpRGBa> &I,
                    vpCameraParameters &cam,
                    vpColor col=vpColor::none) ;
  void init() ;
  void poseDementhonPlan(vpHomogeneousMatrix &cMo) ;
  void poseDementhonNonPlan(vpHomogeneousMatrix &cMo) ;
  void poseLagrangePlan(vpHomogeneousMatrix &cMo, const int coplanar_plane_type=0) ;
  void poseLagrangeNonPlan(vpHomogeneousMatrix &cMo) ;
  void poseLowe(vpHomogeneousMatrix & cMo) ;
  bool poseRansac(vpHomogeneousMatrix & cMo, bool (*func)(vpHomogeneousMatrix *)=NULL) ;
  void poseVirtualVSrobust(vpHomogeneousMatrix & cMo) ;
  void poseVirtualVS(vpHomogeneousMatrix & cMo) ;
  void printPoint() ; 
  void setDistanceToPlaneForCoplanarityTest(double d) ;
  void setLambda(double a) { lambda = a ; }
  void setVvsIterMax(int nb) { vvsIterMax = nb ; }
  
  void setRansacNbInliersToReachConsensus(const unsigned int &nbC){ ransacNbInlierConsensus = nbC; }
  void setRansacThreshold(const double &t) {
    //Test whether or not t is > 0
    if(t > 0) {
      ransacThreshold = t;
    } else {
      throw vpException(vpException::badValue, "The Ransac threshold must be positive as we deal with distance.");
    }
  }
  void setRansacMaxTrials(const int &rM){ ransacMaxTrials = rM; }
  unsigned int getRansacNbInliers() const { return (unsigned int) ransacInliers.size(); }
  std::vector<unsigned int> getRansacInlierIndex() const{ return ransacInlierIndex; }
  std::vector<vpPoint> getRansacInliers() const{ return ransacInliers; }
  
  void setCovarianceComputation(const bool& flag) { computeCovariance = flag; }
  
  vpMatrix getCovarianceMatrix() const { 
    if(!computeCovariance)
      vpTRACE("Warning : The covariance matrix has not been computed. See setCovarianceComputation() to do it.");
    
    return covarianceMatrix; 
  }

  inline void setRansacFilterFlags(const int flags) {
    ransacFlags = flags;
  }

  inline int getNbParallelRansacThreads() const {
    return nbParallelRansacThreads;
  }

  inline void setNbParallelRansacThreads(const int nb) {
    nbParallelRansacThreads = nb;
  }

  inline bool getUseParallelRansac() const {
    return useParallelRansac;
  }

  inline void setUseParallelRansac(const bool use) {
    useParallelRansac = use;
  }

  std::vector<vpPoint> getPoints() const {
    std::vector<vpPoint> vectorOfPoints(listP.begin(), listP.end());
    return vectorOfPoints;
  }

  static void display(vpImage<unsigned char> &I, vpHomogeneousMatrix &cMo,
                      vpCameraParameters &cam, double size,
                      vpColor col=vpColor::none) ;
  static void display(vpImage<vpRGBa> &I, vpHomogeneousMatrix &cMo,
                      vpCameraParameters &cam, double size,
                      vpColor col=vpColor::none) ;
  static double poseFromRectangle(vpPoint &p1,vpPoint &p2,
                                  vpPoint &p3,vpPoint &p4,
                                  double lx, vpCameraParameters & cam,
                                  vpHomogeneousMatrix & cMo) ;

  static int computeRansacIterations(double probability, double epsilon,
                                     const int sampleSize=4, int maxIterations=2000);
                     
  static void findMatch(std::vector<vpPoint> &p2D, 
                     std::vector<vpPoint> &p3D, 
                     const unsigned int &numberOfInlierToReachAConsensus,
                     const double &threshold,
                     unsigned int &ninliers,
                     std::vector<vpPoint> &listInliers,
                     vpHomogeneousMatrix &cMo,
                     const int &maxNbTrials = 10000);
} ;


#endif


/*
 * Local variables:
 * c-basic-offset: 2
 * End:
 */