Visual Servoing Platform  version 3.6.1 under development (2024-03-04)
vpPose.h
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2023 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 _vpPose_h_
40 #define _vpPose_h_
41 
42 #include <visp3/core/vpCameraParameters.h>
43 #include <visp3/core/vpHomogeneousMatrix.h>
44 #include <visp3/core/vpPixelMeterConversion.h>
45 #include <visp3/core/vpPoint.h>
46 #include <visp3/core/vpRGBa.h>
47 #include <visp3/vision/vpHomography.h>
48 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
49 #include <visp3/core/vpList.h>
50 #endif
51 
52 #include <list>
53 #include <math.h>
54 #include <vector>
55 
56 // Check if std:c++17 or higher.
57 // Here we cannot use (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) in the declaration of the class
58 #if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
59 #include <map>
60 #include <optional>
61 #endif
62 
63 #include <visp3/core/vpUniRand.h>
64 
77 class VISP_EXPORT vpPose
78 {
79 public:
81  typedef enum
82  {
85  LOWE,
89  LAGRANGE_LOWE,
91  DEMENTHON_LOWE,
93  VIRTUAL_VS,
95  DEMENTHON_VIRTUAL_VS,
97  LAGRANGE_VIRTUAL_VS,
99  DEMENTHON_LAGRANGE_VIRTUAL_VS,
102  } vpPoseMethodType;
103 
108  {
111  CHECK_DEGENERATE_POINTS
112  };
113 
114  unsigned int npt;
115  std::list<vpPoint> listP;
116 
117  double residual;
118 
119 public:
123  vpPose();
124 
128  vpPose(const std::vector<vpPoint> &lP);
129 
133  virtual ~vpPose();
134 
143  void addPoint(const vpPoint &P);
144 
153  void addPoints(const std::vector<vpPoint> &lP);
154 
158  void clearPoint();
159 
184  bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneousMatrix &) = nullptr);
185 
195  bool computePoseDementhonLagrangeVVS(vpHomogeneousMatrix &cMo);
196 
210  double computeResidual(const vpHomogeneousMatrix &cMo) const;
211 
224  double computeResidual(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam) const;
225 
240  double computeResidual(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, vpColVector &squaredResidual) const;
241 
257  bool coplanar(int &coplanar_plane_type, double *p_a = nullptr, double *p_b = nullptr, double *p_c = nullptr, double *p_d = nullptr);
258 
263  void displayModel(vpImage<unsigned char> &I, vpCameraParameters &cam, vpColor col = vpColor::none);
264 
269  void displayModel(vpImage<vpRGBa> &I, vpCameraParameters &cam, vpColor col = vpColor::none);
270 
276  void poseDementhonPlan(vpHomogeneousMatrix &cMo);
277 
283  void poseDementhonNonPlan(vpHomogeneousMatrix &cMo);
284 
295  void poseLagrangePlan(vpHomogeneousMatrix &cMo, bool *p_isPlan = nullptr, double *p_a = nullptr, double *p_b = nullptr,
296  double *p_c = nullptr, double *p_d = nullptr);
297 
303  void poseLagrangeNonPlan(vpHomogeneousMatrix &cMo);
304 
312  void poseLowe(vpHomogeneousMatrix &cMo);
313 
327  bool poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneousMatrix &) = nullptr);
328 
335  void poseVirtualVSrobust(vpHomogeneousMatrix &cMo);
336 
342  void poseVirtualVS(vpHomogeneousMatrix &cMo);
343 
347  void printPoint();
348 
352  void setDementhonSvThreshold(const double &svThresh);
353 
357  void setDistanceToPlaneForCoplanarityTest(double d);
358 
362  void setLambda(double lambda) { m_lambda = lambda; }
363 
367  void setVvsEpsilon(const double eps)
368  {
369  if (eps >= 0) {
370  vvsEpsilon = eps;
371  }
372  else {
373  throw vpException(vpException::badValue, "Epsilon value must be >= 0.");
374  }
375  }
376 
380  void setVvsIterMax(int nb) { vvsIterMax = nb; }
381 
385  void setRansacNbInliersToReachConsensus(const unsigned int &nbC) { ransacNbInlierConsensus = nbC; }
386 
390  void setRansacThreshold(const double &t)
391  {
392  // Test whether or not t is > 0
393  if (t > std::numeric_limits<double>::epsilon()) {
394  ransacThreshold = t;
395  }
396  else {
397  throw vpException(vpException::badValue, "The Ransac threshold must be positive as we deal with distance.");
398  }
399  }
400 
404  void setRansacMaxTrials(const int &rM) { ransacMaxTrials = rM; }
405 
409  unsigned int getRansacNbInliers() const { return (unsigned int)ransacInliers.size(); }
410 
414  std::vector<unsigned int> getRansacInlierIndex() const { return ransacInlierIndex; }
415 
419  std::vector<vpPoint> getRansacInliers() const { return ransacInliers; }
420 
427  void setCovarianceComputation(const bool &flag) { computeCovariance = flag; }
428 
439  {
440  if (!computeCovariance)
441  vpTRACE("Warning : The covariance matrix has not been computed. See "
442  "setCovarianceComputation() to do it.");
443 
444  return covarianceMatrix;
445  }
446 
458  inline void setRansacFilterFlag(const RANSAC_FILTER_FLAGS &flag) { ransacFlag = flag; }
459 
465  inline int getNbParallelRansacThreads() const { return nbParallelRansacThreads; }
466 
475  inline void setNbParallelRansacThreads(int nb) { nbParallelRansacThreads = nb; }
476 
482  inline bool getUseParallelRansac() const { return useParallelRansac; }
483 
489  inline void setUseParallelRansac(bool use) { useParallelRansac = use; }
490 
496  std::vector<vpPoint> getPoints() const
497  {
498  std::vector<vpPoint> vectorOfPoints(listP.begin(), listP.end());
499  return vectorOfPoints;
500  }
501 
522  static bool computePlanarObjectPoseFromRGBD(const vpImage<float> &depthMap, const std::vector<vpImagePoint> &corners,
523  const vpCameraParameters &colorIntrinsics,
524  const std::vector<vpPoint> &point3d, vpHomogeneousMatrix &cMo,
525  double *confidence_index = nullptr);
526 
559  static bool computePlanarObjectPoseFromRGBD(const vpImage<float> &depthMap,
560  const std::vector<std::vector<vpImagePoint> > &corners,
561  const vpCameraParameters &colorIntrinsics,
562  const std::vector<std::vector<vpPoint> > &point3d,
563  vpHomogeneousMatrix &cMo, double *confidence_index = nullptr,
564  bool coplanar_points = true);
565 
585  static int computeRansacIterations(double probability, double epsilon, const int sampleSize = 4,
586  int maxIterations = 2000);
587 
598  static void display(vpImage<unsigned char> &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size,
599  vpColor col = vpColor::none);
600 
611  static void display(vpImage<vpRGBa> &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size,
612  vpColor col = vpColor::none);
613 
644  static void findMatch(std::vector<vpPoint> &p2D, std::vector<vpPoint> &p3D,
645  const unsigned int &numberOfInlierToReachAConsensus, const double &threshold,
646  unsigned int &ninliers, std::vector<vpPoint> &listInliers, vpHomogeneousMatrix &cMo,
647  const int &maxNbTrials = 10000, bool useParallelRansac = true, unsigned int nthreads = 0,
648  bool (*func)(const vpHomogeneousMatrix &) = nullptr);
649 
669  static double poseFromRectangle(vpPoint &p1, vpPoint &p2, vpPoint &p3, vpPoint &p4, double lx,
671 
672  // Check if std:c++17 or higher.
673  // Here we cannot use (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) in the declaration of the class
674 #if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
675 
690  template <typename DataId>
691  static std::optional<vpHomogeneousMatrix> computePlanarObjectPoseWithAtLeast3Points(
692  const vpPlane &plane_in_camera_frame, const std::map<DataId, vpPoint> &pts,
693  const std::map<DataId, vpImagePoint> &ips, const vpCameraParameters &camera_intrinsics,
694  std::optional<vpHomogeneousMatrix> cMo_init = std::nullopt, bool enable_vvs = true)
695  {
696  if (cMo_init && !enable_vvs) {
697  throw(vpException(
699  "It doesn't make sense to use an initialized pose without enabling VVS to compute the pose from 4 points"));
700  }
701 
702  // Check if detection and model fit
703  // - The next line produces an internal compiler error with Visual Studio 2017:
704  // modules\vision\include\visp3/vision/vpPose.h(404): fatal error C1001: An internal error has occurred in the
705  // compiler.
706  // To work around this problem, try simplifying or changing the program near the locations listed above.
707  // Please choose the Technical Support command on the Visual C++
708  // Help menu, or open the Technical Support help file for more information
709  // - Note that the next line builds with Visual Studio 2022.
710  // for ([[maybe_unused]] const auto &[ip_id, _] : ips) {
711  for (const auto &[ip_id, ip_unused] : ips) {
712  (void)ip_unused;
713  if (pts.find(ip_id) == end(pts)) {
715  "Cannot compute pose with points and image points which do not have the same IDs"));
716  }
717  }
718 
719  std::vector<vpPoint> P {}, Q {};
720  // The next line in C++17 produces a build error with Visual Studio 2017, that's why we
721  // use rather C++11 to loop through std::map
722  // for (auto [pt_id, pt] : pts) {
723  for (const auto &pt_map : pts) {
724  if (ips.find(pt_map.first) != end(ips)) {
725  double x = 0, y = 0;
726  vpPoint pt = pt_map.second;
727  vpPixelMeterConversion::convertPoint(camera_intrinsics, ips.at(pt_map.first), x, y);
728  const auto Z = plane_in_camera_frame.computeZ(x, y);
729 
730  pt.set_x(x);
731  pt.set_y(y);
732  pt.set_Z(Z);
733 
734  Q.push_back(pt);
735  P.emplace_back(x * Z, y * Z, Z);
736  }
737  }
738 
739  if (Q.size() < 3) {
740  return std::nullopt;
741  }
742 
743  auto cMo = cMo_init.value_or(vpHomogeneousMatrix::compute3d3dTransformation(P, Q));
744  if (!cMo.isValid()) {
745  return std::nullopt;
746  }
747 
748  return enable_vvs ? vpPose::poseVirtualVSWithDepth(Q, cMo).value_or(cMo) : cMo;
749  }
750 
760  static std::optional<vpHomogeneousMatrix> poseVirtualVSWithDepth(const std::vector<vpPoint> &points,
761  const vpHomogeneousMatrix &cMo);
762 #endif
763 
764 protected:
765  double m_lambda;
767 
776  double computeResidualDementhon(const vpHomogeneousMatrix &cMo);
777 
782  int calculArbreDementhon(vpMatrix &b, vpColVector &U, vpHomogeneousMatrix &cMo);
783 
784 private:
786  int vvsIterMax;
788  std::vector<vpPoint> c3d;
790  bool computeCovariance;
792  vpMatrix covarianceMatrix;
795  unsigned int ransacNbInlierConsensus;
797  int ransacMaxTrials;
799  std::vector<vpPoint> ransacInliers;
801  std::vector<unsigned int> ransacInlierIndex;
803  double ransacThreshold;
806  double distanceToPlaneForCoplanarityTest;
808  RANSAC_FILTER_FLAGS ransacFlag;
811  std::vector<vpPoint> listOfPoints;
813  bool useParallelRansac;
815  int nbParallelRansacThreads;
818  double vvsEpsilon;
819 
823  class vpRansacFunctor
824  {
825  public:
829  vpRansacFunctor(const vpHomogeneousMatrix &cMo_, unsigned int ransacNbInlierConsensus_, const int ransacMaxTrials_,
830  double ransacThreshold_, unsigned int initial_seed_, bool checkDegeneratePoints_,
831  const std::vector<vpPoint> &listOfUniquePoints_, bool (*func_)(const vpHomogeneousMatrix &))
832  : m_best_consensus(), m_checkDegeneratePoints(checkDegeneratePoints_), m_cMo(cMo_), m_foundSolution(false),
833  m_func(func_), m_listOfUniquePoints(listOfUniquePoints_), m_nbInliers(0), m_ransacMaxTrials(ransacMaxTrials_),
834  m_ransacNbInlierConsensus(ransacNbInlierConsensus_), m_ransacThreshold(ransacThreshold_),
835  m_uniRand(initial_seed_)
836  { }
837 
841  void operator()() { m_foundSolution = poseRansacImpl(); }
842 
846  bool getResult() const { return m_foundSolution; }
847 
851  std::vector<unsigned int> getBestConsensus() const { return m_best_consensus; }
852 
856  vpHomogeneousMatrix getEstimatedPose() const { return m_cMo; }
857 
861  unsigned int getNbInliers() const { return m_nbInliers; }
862 
863  private:
864  std::vector<unsigned int> m_best_consensus;
865  bool m_checkDegeneratePoints;
866  vpHomogeneousMatrix m_cMo;
867  bool m_foundSolution;
868  bool (*m_func)(const vpHomogeneousMatrix &);
869  std::vector<vpPoint> m_listOfUniquePoints;
870  unsigned int m_nbInliers;
871  int m_ransacMaxTrials;
872  unsigned int m_ransacNbInlierConsensus;
873  double m_ransacThreshold;
874  vpUniRand m_uniRand;
875 
880  bool poseRansacImpl();
881  };
882 };
883 
884 #endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:152
static const vpColor none
Definition: vpColor.h:223
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:85
@ fatalError
Fatal error.
Definition: vpException.h:84
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:146
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:54
double computeZ(double x, double y) const
Definition: vpPlane.cpp:232
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:77
void set_x(double x)
Set the point x coordinate in the image plane.
Definition: vpPoint.cpp:500
void set_Z(double cZ)
Set the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:486
void set_y(double y)
Set the point y coordinate in the image plane.
Definition: vpPoint.cpp:502
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:78
void setRansacMaxTrials(const int &rM)
Definition: vpPose.h:404
double m_lambda
Parameters use for the virtual visual servoing approach.
Definition: vpPose.h:765
void setRansacNbInliersToReachConsensus(const unsigned int &nbC)
Definition: vpPose.h:385
vpMatrix getCovarianceMatrix() const
Definition: vpPose.h:438
bool getUseParallelRansac() const
Definition: vpPose.h:482
vpPoseMethodType
Methods that could be used to estimate the pose from points.
Definition: vpPose.h:82
@ DEMENTHON
Definition: vpPose.h:84
@ RANSAC
Definition: vpPose.h:88
@ LAGRANGE
Definition: vpPose.h:83
void setCovarianceComputation(const bool &flag)
Definition: vpPose.h:427
unsigned int npt
Number of point used in pose computation.
Definition: vpPose.h:114
int getNbParallelRansacThreads() const
Definition: vpPose.h:465
std::vector< vpPoint > getPoints() const
Definition: vpPose.h:496
std::vector< unsigned int > getRansacInlierIndex() const
Definition: vpPose.h:414
double m_dementhonSvThresh
SVD threshold use for the pseudo-inverse computation in poseDementhonPlan.
Definition: vpPose.h:766
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Definition: vpPose.h:115
void setRansacFilterFlag(const RANSAC_FILTER_FLAGS &flag)
Definition: vpPose.h:458
void setVvsIterMax(int nb)
Definition: vpPose.h:380
RANSAC_FILTER_FLAGS
Definition: vpPose.h:108
@ PREFILTER_DEGENERATE_POINTS
Definition: vpPose.h:110
@ NO_FILTER
No filter is applied.
Definition: vpPose.h:109
unsigned int getRansacNbInliers() const
Definition: vpPose.h:409
void setUseParallelRansac(bool use)
Definition: vpPose.h:489
std::vector< vpPoint > getRansacInliers() const
Definition: vpPose.h:419
void setLambda(double lambda)
Definition: vpPose.h:362
void setVvsEpsilon(const double eps)
Definition: vpPose.h:367
void setNbParallelRansacThreads(int nb)
Definition: vpPose.h:475
double residual
Residual in meter.
Definition: vpPose.h:117
void setRansacThreshold(const double &t)
Definition: vpPose.h:390
Class for generating random numbers with uniform probability density.
Definition: vpUniRand.h:123
#define vpTRACE
Definition: vpDebug.h:405