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

#ifndef _vpPose_h_
#define _vpPose_h_

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

#include <list>
#include <math.h>
#include <vector>
#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
#include <atomic>
#endif

#include <visp3/core/vpUniRand.h>

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 RANSAC_FILTER_FLAGS {
    NO_FILTER,
    PREFILTER_DEGENERATE_POINTS, 
    CHECK_DEGENERATE_POINTS      
  };

  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;
  RANSAC_FILTER_FLAGS ransacFlag;
  std::vector<vpPoint> listOfPoints;
  bool useParallelRansac;
  int nbParallelRansacThreads;
  double vvsEpsilon;

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

    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:
#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
    std::atomic<bool> &m_abort;
#endif
    std::vector<unsigned int> m_best_consensus;
    bool m_checkDegeneratePoints;
    vpHomogeneousMatrix m_cMo;
    bool m_foundSolution;
    bool (*m_func)(const vpHomogeneousMatrix &);
    std::vector<vpPoint> m_listOfUniquePoints;
    unsigned int m_nbInliers;
    int m_ransacMaxTrials;
    unsigned int m_ransacNbInlierConsensus;
    double m_ransacThreshold;
    vpUniRand m_uniRand;

    bool poseRansacImpl();
  };

protected:
  double computeResidualDementhon(const vpHomogeneousMatrix &cMo);

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

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

  bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool (*func)(const 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 poseDementhonPlan(vpHomogeneousMatrix &cMo);
  void poseDementhonNonPlan(vpHomogeneousMatrix &cMo);
  void poseLagrangePlan(vpHomogeneousMatrix &cMo);
  void poseLagrangeNonPlan(vpHomogeneousMatrix &cMo);
  void poseLowe(vpHomogeneousMatrix &cMo);
  bool poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneousMatrix &) = NULL);
  void poseVirtualVSrobust(vpHomogeneousMatrix &cMo);
  void poseVirtualVS(vpHomogeneousMatrix &cMo);
  void printPoint();
  void setDistanceToPlaneForCoplanarityTest(double d);
  void setLambda(double a) { lambda = a; }
  void setVvsEpsilon(const double eps)
  {
    if (eps >= 0) {
      vvsEpsilon = eps;
    } else {
      throw vpException(vpException::badValue, "Epsilon value must be >= 0.");
    }
  }
  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 > std::numeric_limits<double>::epsilon()) {
      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 setRansacFilterFlag(const RANSAC_FILTER_FLAGS &flag) { ransacFlag = flag; }

  inline int getNbParallelRansacThreads() const { return nbParallelRansacThreads; }

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

  inline bool getUseParallelRansac() const { return useParallelRansac; }

  inline void setUseParallelRansac(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, bool useParallelRansac=true, unsigned int nthreads=0,
                        bool (*func)(const vpHomogeneousMatrix &) = NULL);

  static bool computePlanarObjectPoseFromRGBD(const vpImage<float> &depthMap, const std::vector<vpImagePoint> &corners,
                                              const vpCameraParameters &colorIntrinsics, const std::vector<vpPoint> &point3d, vpHomogeneousMatrix &cMo,
                                              double *confidence_index = NULL);

#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)

  vp_deprecated void init();
#endif
};

#endif