Visual Servoing Platform  version 3.6.1 under development (2024-07-18)
vpPose.h
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Pose computation.
32  */
33 
39 #ifndef VP_POSE_H
40 #define VP_POSE_H
41 
42 #include <visp3/core/vpCameraParameters.h>
43 #include <visp3/core/vpHomogeneousMatrix.h>
44 #include <visp3/core/vpPixelMeterConversion.h>
45 #include <visp3/core/vpPlane.h>
46 #include <visp3/core/vpPoint.h>
47 #include <visp3/core/vpRGBa.h>
48 
49 #include <list>
50 #include <math.h>
51 #include <vector>
52 
53 // Check if std:c++17 or higher.
54 // Here we cannot use (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) in the declaration of the class
55 #if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
56 #include <map>
57 #include <optional>
58 #endif
59 
60 #include <visp3/core/vpUniRand.h>
61 
63 
76 class VISP_EXPORT vpPose
77 {
78 public:
80  typedef enum
81  {
84  LOWE,
88  LAGRANGE_LOWE,
90  DEMENTHON_LOWE,
92  VIRTUAL_VS,
94  DEMENTHON_VIRTUAL_VS,
96  LAGRANGE_VIRTUAL_VS,
98  DEMENTHON_LAGRANGE_VIRTUAL_VS,
101  } vpPoseMethodType;
102 
107  {
110  CHECK_DEGENERATE_POINTS
111  };
112 
113  unsigned int npt;
114  std::list<vpPoint> listP;
115 
116  double residual;
117 
118 public:
119  // Typedef a function that checks if a pose is valid.
120  typedef bool (*FuncCheckValidityPose)(const vpHomogeneousMatrix &);
121 
125  vpPose();
126 
130  VP_EXPLICIT vpPose(const std::vector<vpPoint> &lP);
131 
135  virtual ~vpPose();
136 
145  void addPoint(const vpPoint &P);
146 
155  void addPoints(const std::vector<vpPoint> &lP);
156 
160  void clearPoint();
161 
186  bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, FuncCheckValidityPose func = nullptr);
187 
197  bool computePoseDementhonLagrangeVVS(vpHomogeneousMatrix &cMo);
198 
212  double computeResidual(const vpHomogeneousMatrix &cMo) const;
213 
226  double computeResidual(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam) const;
227 
242  double computeResidual(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, vpColVector &squaredResidual) const;
243 
259  bool coplanar(int &coplanar_plane_type, double *p_a = nullptr, double *p_b = nullptr, double *p_c = nullptr, double *p_d = nullptr);
260 
265  void displayModel(vpImage<unsigned char> &I, vpCameraParameters &cam, vpColor col = vpColor::none);
266 
271  void displayModel(vpImage<vpRGBa> &I, vpCameraParameters &cam, vpColor col = vpColor::none);
272 
278  void poseDementhonPlan(vpHomogeneousMatrix &cMo);
279 
285  void poseDementhonNonPlan(vpHomogeneousMatrix &cMo);
286 
297  void poseLagrangePlan(vpHomogeneousMatrix &cMo, bool *p_isPlan = nullptr, double *p_a = nullptr, double *p_b = nullptr,
298  double *p_c = nullptr, double *p_d = nullptr);
299 
305  void poseLagrangeNonPlan(vpHomogeneousMatrix &cMo);
306 
314  void poseLowe(vpHomogeneousMatrix &cMo);
315 
329  bool poseRansac(vpHomogeneousMatrix &cMo, FuncCheckValidityPose func = nullptr);
330 
337  void poseVirtualVSrobust(vpHomogeneousMatrix &cMo);
338 
344  void poseVirtualVS(vpHomogeneousMatrix &cMo);
345 
349  void printPoint();
350 
354  void setDementhonSvThreshold(const double &svThresh);
355 
359  void setDistToPlaneForCoplanTest(double d);
360 
364  void setLambda(double lambda) { m_lambda = lambda; }
365 
369  void setVvsEpsilon(const double eps)
370  {
371  if (eps >= 0) {
372  vvsEpsilon = eps;
373  }
374  else {
375  throw vpException(vpException::badValue, "Epsilon value must be >= 0.");
376  }
377  }
378 
382  void setVvsIterMax(int nb) { vvsIterMax = nb; }
383 
387  void setRansacNbInliersToReachConsensus(const unsigned int &nbC) { ransacNbInlierConsensus = nbC; }
388 
392  void setRansacThreshold(const double &t)
393  {
394  // Test whether or not t is > 0
395  if (t > std::numeric_limits<double>::epsilon()) {
396  ransacThreshold = t;
397  }
398  else {
399  throw vpException(vpException::badValue, "The Ransac threshold must be positive as we deal with distance.");
400  }
401  }
402 
406  void setRansacMaxTrials(const int &rM) { ransacMaxTrials = rM; }
407 
411  unsigned int getRansacNbInliers() const { return static_cast<unsigned int>(ransacInliers.size()); }
412 
416  std::vector<unsigned int> getRansacInlierIndex() const { return ransacInlierIndex; }
417 
421  std::vector<vpPoint> getRansacInliers() const { return ransacInliers; }
422 
429  void setCovarianceComputation(const bool &flag) { computeCovariance = flag; }
430 
441  {
442  if (!computeCovariance) {
443  std::cout << "Warning: The covariance matrix has not been computed. See setCovarianceComputation() to do it." << std::endl;
444  }
445  return covarianceMatrix;
446  }
447 
459  inline void setRansacFilterFlag(const RANSAC_FILTER_FLAGS &flag) { ransacFlag = flag; }
460 
466  inline int getNbParallelRansacThreads() const { return nbParallelRansacThreads; }
467 
476  inline void setNbParallelRansacThreads(int nb) { nbParallelRansacThreads = nb; }
477 
483  inline bool getUseParallelRansac() const { return useParallelRansac; }
484 
490  inline void setUseParallelRansac(bool use) { useParallelRansac = use; }
491 
497  std::vector<vpPoint> getPoints() const
498  {
499  std::vector<vpPoint> vectorOfPoints(listP.begin(), listP.end());
500  return vectorOfPoints;
501  }
502 
523  static bool computePlanarObjectPoseFromRGBD(const vpImage<float> &depthMap, const std::vector<vpImagePoint> &corners,
524  const vpCameraParameters &colorIntrinsics,
525  const std::vector<vpPoint> &point3d, vpHomogeneousMatrix &cMo,
526  double *confidence_index = nullptr);
527 
560  static bool computePlanarObjectPoseFromRGBD(const vpImage<float> &depthMap,
561  const std::vector<std::vector<vpImagePoint> > &corners,
562  const vpCameraParameters &colorIntrinsics,
563  const std::vector<std::vector<vpPoint> > &point3d,
564  vpHomogeneousMatrix &cMo, double *confidence_index = nullptr,
565  bool coplanar_points = true);
566 
586  static int computeRansacIterations(double probability, double epsilon, const int sampleSize = 4,
587  int maxIterations = 2000);
588 
599  static void display(vpImage<unsigned char> &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size,
600  vpColor col = vpColor::none);
601 
612  static void display(vpImage<vpRGBa> &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size,
613  vpColor col = vpColor::none);
614 
645  static void findMatch(std::vector<vpPoint> &p2D, std::vector<vpPoint> &p3D,
646  const unsigned int &numberOfInlierToReachAConsensus, const double &threshold,
647  unsigned int &ninliers, std::vector<vpPoint> &listInliers, vpHomogeneousMatrix &cMo,
648  const int &maxNbTrials = 10000, bool useParallelRansac = true, unsigned int nthreads = 0,
649  FuncCheckValidityPose func = nullptr);
650 
651 #ifdef VISP_HAVE_HOMOGRAPHY
671  static double poseFromRectangle(vpPoint &p1, vpPoint &p2, vpPoint &p3, vpPoint &p4, double lx,
673 #endif
674 
675  // Check if std:c++17 or higher.
676  // Here we cannot use (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) in the declaration of the class
677 #if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
678 
693  template <typename DataId>
694  static std::optional<vpHomogeneousMatrix> computePlanarObjectPoseWithAtLeast3Points(
695  const vpPlane &plane_in_camera_frame, const std::map<DataId, vpPoint> &pts,
696  const std::map<DataId, vpImagePoint> &ips, const vpCameraParameters &camera_intrinsics,
697  std::optional<vpHomogeneousMatrix> cMo_init = std::nullopt, bool enable_vvs = true)
698  {
699  if (cMo_init && (!enable_vvs)) {
700  throw(vpException(
702  "It doesn't make sense to use an initialized pose without enabling VVS to compute the pose from 4 points"));
703  }
704 
705  // Check if detection and model fit
706  // - The next line produces an internal compiler error with Visual Studio 2017:
707  // modules\vision\include\visp3/vision/vpPose.h(404): fatal error C1001: An internal error has occurred in the
708  // compiler.
709  // To work around this problem, try simplifying or changing the program near the locations listed above.
710  // Please choose the Technical Support command on the Visual C++
711  // Help menu, or open the Technical Support help file for more information
712  // - Note that the next line builds with Visual Studio 2022.
713  // for ([[maybe_unused]] const auto &[ip_id, _] : ips) {
714  for (const auto &[ip_id, ip_unused] : ips) {
715  (void)ip_unused;
716  if (pts.find(ip_id) == end(pts)) {
718  "Cannot compute pose with points and image points which do not have the same IDs"));
719  }
720  }
721 
722  std::vector<vpPoint> P {}, Q {};
723  // The next line in C++17 produces a build error with Visual Studio 2017, that's why we
724  // use rather C++11 to loop through std::map
725  // for (auto [pt_id, pt] : pts) {
726  for (const auto &pt_map : pts) {
727  if (ips.find(pt_map.first) != end(ips)) {
728  double x = 0, y = 0;
729  vpPoint pt = pt_map.second;
730  vpPixelMeterConversion::convertPoint(camera_intrinsics, ips.at(pt_map.first), x, y);
731  const auto Z = plane_in_camera_frame.computeZ(x, y);
732 
733  pt.set_x(x);
734  pt.set_y(y);
735  pt.set_Z(Z);
736 
737  Q.push_back(pt);
738  P.emplace_back(x * Z, y * Z, Z);
739  }
740  }
741 
742  if (Q.size() < 3) {
743  return std::nullopt;
744  }
745 
746  auto cMo = cMo_init.value_or(vpHomogeneousMatrix::compute3d3dTransformation(P, Q));
747  if (!cMo.isValid()) {
748  return std::nullopt;
749  }
750 
751  return enable_vvs ? vpPose::poseVirtualVSWithDepth(Q, cMo).value_or(cMo) : cMo;
752  }
753 
763  static std::optional<vpHomogeneousMatrix> poseVirtualVSWithDepth(const std::vector<vpPoint> &points,
764  const vpHomogeneousMatrix &cMo);
765 #endif
766 
767 protected:
768  double m_lambda;
770 
779  double computeResidualDementhon(const vpHomogeneousMatrix &cMo);
780 
785  int calculArbreDementhon(vpMatrix &b, vpColVector &U, vpHomogeneousMatrix &cMo);
786 
787 private:
788  void callLagrangePose(vpHomogeneousMatrix &cMo);
789 
791  int vvsIterMax;
793  std::vector<vpPoint> c3d;
795  bool computeCovariance;
797  vpMatrix covarianceMatrix;
800  unsigned int ransacNbInlierConsensus;
802  int ransacMaxTrials;
804  std::vector<vpPoint> ransacInliers;
806  std::vector<unsigned int> ransacInlierIndex;
808  double ransacThreshold;
811  double distToPlaneForCoplanarityTest;
813  RANSAC_FILTER_FLAGS ransacFlag;
816  std::vector<vpPoint> listOfPoints;
818  bool useParallelRansac;
820  int nbParallelRansacThreads;
823  double vvsEpsilon;
824 
828  class vpRansacFunctor
829  {
830  public:
834  vpRansacFunctor(const vpHomogeneousMatrix &cMo_, unsigned int ransacNbInlierConsensus_, const int ransacMaxTrials_,
835  double ransacThreshold_, unsigned int initial_seed_, bool checkDegeneratePoints_,
836  const std::vector<vpPoint> &listOfUniquePoints_, FuncCheckValidityPose func_)
837  : m_best_consensus(), m_checkDegeneratePoints(checkDegeneratePoints_), m_cMo(cMo_), m_foundSolution(false),
838  m_func(func_), m_listOfUniquePoints(listOfUniquePoints_), m_nbInliers(0), m_ransacMaxTrials(ransacMaxTrials_),
839  m_ransacNbInlierConsensus(ransacNbInlierConsensus_), m_ransacThreshold(ransacThreshold_),
840  m_uniRand(initial_seed_)
841  { }
842 
846  void operator()() { m_foundSolution = poseRansacImpl(); }
847 
851  bool getResult() const { return m_foundSolution; }
852 
856  std::vector<unsigned int> getBestConsensus() const { return m_best_consensus; }
857 
861  vpHomogeneousMatrix getEstimatedPose() const { return m_cMo; }
862 
866  unsigned int getNbInliers() const { return m_nbInliers; }
867 
868  private:
869  std::vector<unsigned int> m_best_consensus;
870  bool m_checkDegeneratePoints;
871  vpHomogeneousMatrix m_cMo;
872  bool m_foundSolution;
873  FuncCheckValidityPose m_func;
874  std::vector<vpPoint> m_listOfUniquePoints;
875  unsigned int m_nbInliers;
876  int m_ransacMaxTrials;
877  unsigned int m_ransacNbInlierConsensus;
878  double m_ransacThreshold;
879  vpUniRand m_uniRand;
880 
885  bool poseRansacImpl();
886  };
887 };
888 
889 END_VISP_NAMESPACE
890 
891 #endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:157
static const vpColor none
Definition: vpColor.h:229
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:73
@ fatalError
Fatal error.
Definition: vpException.h:72
Implementation of an homogeneous matrix and operations on such kind of matrices.
static vpHomogeneousMatrix compute3d3dTransformation(const std::vector< vpPoint > &p, const std::vector< vpPoint > &q)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:57
double computeZ(double x, double y) const
Definition: vpPlane.cpp:245
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:79
void set_x(double x)
Set the point x coordinate in the image plane.
Definition: vpPoint.cpp:464
void set_Z(double cZ)
Set the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:450
void set_y(double y)
Set the point y coordinate in the image plane.
Definition: vpPoint.cpp:466
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:77
void setRansacMaxTrials(const int &rM)
Definition: vpPose.h:406
double m_lambda
Parameters use for the virtual visual servoing approach.
Definition: vpPose.h:768
void setRansacNbInliersToReachConsensus(const unsigned int &nbC)
Definition: vpPose.h:387
vpMatrix getCovarianceMatrix() const
Definition: vpPose.h:440
bool getUseParallelRansac() const
Definition: vpPose.h:483
static std::optional< vpHomogeneousMatrix > computePlanarObjectPoseWithAtLeast3Points(const vpPlane &plane_in_camera_frame, const std::map< DataId, vpPoint > &pts, const std::map< DataId, vpImagePoint > &ips, const vpCameraParameters &camera_intrinsics, std::optional< vpHomogeneousMatrix > cMo_init=std::nullopt, bool enable_vvs=true)
Definition: vpPose.h:694
vpPoseMethodType
Methods that could be used to estimate the pose from points.
Definition: vpPose.h:81
@ DEMENTHON
Definition: vpPose.h:83
@ RANSAC
Definition: vpPose.h:87
@ LAGRANGE
Definition: vpPose.h:82
void setCovarianceComputation(const bool &flag)
Definition: vpPose.h:429
unsigned int npt
Number of point used in pose computation.
Definition: vpPose.h:113
int getNbParallelRansacThreads() const
Definition: vpPose.h:466
std::vector< vpPoint > getPoints() const
Definition: vpPose.h:497
std::vector< unsigned int > getRansacInlierIndex() const
Definition: vpPose.h:416
double m_dementhonSvThresh
SVD threshold use for the pseudo-inverse computation in poseDementhonPlan.
Definition: vpPose.h:769
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Definition: vpPose.h:114
void setRansacFilterFlag(const RANSAC_FILTER_FLAGS &flag)
Definition: vpPose.h:459
void setVvsIterMax(int nb)
Definition: vpPose.h:382
RANSAC_FILTER_FLAGS
Definition: vpPose.h:107
@ PREFILTER_DEGENERATE_POINTS
Definition: vpPose.h:109
@ NO_FILTER
No filter is applied.
Definition: vpPose.h:108
unsigned int getRansacNbInliers() const
Definition: vpPose.h:411
void setUseParallelRansac(bool use)
Definition: vpPose.h:490
static std::optional< vpHomogeneousMatrix > poseVirtualVSWithDepth(const std::vector< vpPoint > &points, const vpHomogeneousMatrix &cMo)
std::vector< vpPoint > getRansacInliers() const
Definition: vpPose.h:421
void setLambda(double lambda)
Definition: vpPose.h:364
void setVvsEpsilon(const double eps)
Definition: vpPose.h:369
void setNbParallelRansacThreads(int nb)
Definition: vpPose.h:476
double residual
Residual in meter.
Definition: vpPose.h:116
void setRansacThreshold(const double &t)
Definition: vpPose.h:392
Class for generating random numbers with uniform probability density.
Definition: vpUniRand.h:125