Visual Servoing Platform  version 3.5.1 under development (2023-03-29)
vpPose.h
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2022 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Pose computation.
33  *
34  * Authors:
35  * Eric Marchand
36  * Francois Chaumette
37  * Aurelien Yol
38  * Fabien Spindler
39  * Julien Dufour
40  *
41  *****************************************************************************/
42 
48 #ifndef _vpPose_h_
49 #define _vpPose_h_
50 
51 #include <visp3/core/vpHomogeneousMatrix.h>
52 #include <visp3/core/vpPixelMeterConversion.h>
53 #include <visp3/core/vpPoint.h>
54 #include <visp3/core/vpRGBa.h>
55 #include <visp3/vision/vpHomography.h>
56 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
57 #include <visp3/core/vpList.h>
58 #endif
59 #include <visp3/core/vpThread.h>
60 
61 #include <list>
62 #include <math.h>
63 #include <vector>
64 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
65 #include <atomic>
66 #endif
67 
68 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \
69  (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911)))
70 #include <map>
71 #include <optional>
72 #endif
73 
74 #include <visp3/core/vpUniRand.h>
75 
89 class VISP_EXPORT vpPose
90 {
91 public:
93  typedef enum {
96  LOWE,
100  LAGRANGE_LOWE,
102  DEMENTHON_LOWE,
104  VIRTUAL_VS,
106  DEMENTHON_VIRTUAL_VS,
108  LAGRANGE_VIRTUAL_VS,
110  DEMENTHON_LAGRANGE_VIRTUAL_VS,
113  } vpPoseMethodType;
114 
118  CHECK_DEGENERATE_POINTS
119  };
120 
121  unsigned int npt;
122  std::list<vpPoint> listP;
123 
124  double residual;
125 
126 protected:
127  double lambda;
128 
129 private:
131  int vvsIterMax;
133  std::vector<vpPoint> c3d;
135  bool computeCovariance;
137  vpMatrix covarianceMatrix;
140  unsigned int ransacNbInlierConsensus;
142  int ransacMaxTrials;
144  std::vector<vpPoint> ransacInliers;
146  std::vector<unsigned int> ransacInlierIndex;
148  double ransacThreshold;
151  double distanceToPlaneForCoplanarityTest;
153  RANSAC_FILTER_FLAGS ransacFlag;
156  std::vector<vpPoint> listOfPoints;
158  bool useParallelRansac;
160  int nbParallelRansacThreads;
163  double vvsEpsilon;
164 
165  // For parallel RANSAC
166  class RansacFunctor
167  {
168  public:
169  RansacFunctor(const vpHomogeneousMatrix &cMo_, unsigned int ransacNbInlierConsensus_, const int ransacMaxTrials_,
170  double ransacThreshold_, unsigned int initial_seed_, bool checkDegeneratePoints_,
171  const std::vector<vpPoint> &listOfUniquePoints_, bool (*func_)(const vpHomogeneousMatrix &))
172  : m_best_consensus(), m_checkDegeneratePoints(checkDegeneratePoints_), m_cMo(cMo_), m_foundSolution(false),
173  m_func(func_), m_listOfUniquePoints(listOfUniquePoints_), m_nbInliers(0), m_ransacMaxTrials(ransacMaxTrials_),
174  m_ransacNbInlierConsensus(ransacNbInlierConsensus_), m_ransacThreshold(ransacThreshold_),
175  m_uniRand(initial_seed_)
176  {
177  }
178 
179  void operator()() { m_foundSolution = poseRansacImpl(); }
180 
181  // Access the return value.
182  bool getResult() const { return m_foundSolution; }
183 
184  std::vector<unsigned int> getBestConsensus() const { return m_best_consensus; }
185 
186  vpHomogeneousMatrix getEstimatedPose() const { return m_cMo; }
187 
188  unsigned int getNbInliers() const { return m_nbInliers; }
189 
190  private:
191  std::vector<unsigned int> m_best_consensus;
192  bool m_checkDegeneratePoints;
193  vpHomogeneousMatrix m_cMo;
194  bool m_foundSolution;
195  bool (*m_func)(const vpHomogeneousMatrix &);
196  std::vector<vpPoint> m_listOfUniquePoints;
197  unsigned int m_nbInliers;
198  int m_ransacMaxTrials;
199  unsigned int m_ransacNbInlierConsensus;
200  double m_ransacThreshold;
201  vpUniRand m_uniRand;
202 
203  bool poseRansacImpl();
204  };
205 
206 protected:
208  double computeResidualDementhon(const vpHomogeneousMatrix &cMo);
209 
210  // method used in poseDementhonPlan()
211  int calculArbreDementhon(vpMatrix &b, vpColVector &U, vpHomogeneousMatrix &cMo);
212 
213 public:
214  vpPose();
215  vpPose(const std::vector<vpPoint> &lP);
216  virtual ~vpPose();
217  void addPoint(const vpPoint &P);
218  void addPoints(const std::vector<vpPoint> &lP);
219  void clearPoint();
220 
221  bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneousMatrix &) = NULL);
222  bool computePoseDementhonLagrangeVVS(vpHomogeneousMatrix &cMo);
223  double computeResidual(const vpHomogeneousMatrix &cMo) const;
224  bool coplanar(int &coplanar_plane_type, double *p_a = NULL, double *p_b = NULL, double *p_c = NULL, double *p_d = NULL);
225  void displayModel(vpImage<unsigned char> &I, vpCameraParameters &cam, vpColor col = vpColor::none);
226  void displayModel(vpImage<vpRGBa> &I, vpCameraParameters &cam, vpColor col = vpColor::none);
227 
228  void poseDementhonPlan(vpHomogeneousMatrix &cMo);
229  void poseDementhonNonPlan(vpHomogeneousMatrix &cMo);
230  void poseLagrangePlan(vpHomogeneousMatrix &cMo, bool *p_isPlan = NULL, double *p_a = NULL, double *p_b = NULL, double *p_c = NULL, double *p_d = NULL);
231  void poseLagrangeNonPlan(vpHomogeneousMatrix &cMo);
232  void poseLowe(vpHomogeneousMatrix &cMo);
233  bool poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneousMatrix &) = NULL);
234  void poseVirtualVSrobust(vpHomogeneousMatrix &cMo);
235  void poseVirtualVS(vpHomogeneousMatrix &cMo);
236  void printPoint();
237  void setDementhonSvThreshold(const double& svThresh);
238  void setDistanceToPlaneForCoplanarityTest(double d);
239  void setLambda(double a) { lambda = a; }
240  void setVvsEpsilon(const double eps)
241  {
242  if (eps >= 0) {
243  vvsEpsilon = eps;
244  } else {
245  throw vpException(vpException::badValue, "Epsilon value must be >= 0.");
246  }
247  }
248  void setVvsIterMax(int nb) { vvsIterMax = nb; }
249 
250  void setRansacNbInliersToReachConsensus(const unsigned int &nbC) { ransacNbInlierConsensus = nbC; }
251  void setRansacThreshold(const double &t)
252  {
253  // Test whether or not t is > 0
254  if (t > std::numeric_limits<double>::epsilon()) {
255  ransacThreshold = t;
256  } else {
257  throw vpException(vpException::badValue, "The Ransac threshold must be positive as we deal with distance.");
258  }
259  }
260  void setRansacMaxTrials(const int &rM) { ransacMaxTrials = rM; }
261  unsigned int getRansacNbInliers() const { return (unsigned int)ransacInliers.size(); }
262  std::vector<unsigned int> getRansacInlierIndex() const { return ransacInlierIndex; }
263  std::vector<vpPoint> getRansacInliers() const { return ransacInliers; }
264 
271  void setCovarianceComputation(const bool &flag) { computeCovariance = flag; }
272 
283  {
284  if (!computeCovariance)
285  vpTRACE("Warning : The covariance matrix has not been computed. See "
286  "setCovarianceComputation() to do it.");
287 
288  return covarianceMatrix;
289  }
290 
302  inline void setRansacFilterFlag(const RANSAC_FILTER_FLAGS &flag) { ransacFlag = flag; }
303 
309  inline int getNbParallelRansacThreads() const { return nbParallelRansacThreads; }
310 
319  inline void setNbParallelRansacThreads(int nb) { nbParallelRansacThreads = nb; }
320 
326  inline bool getUseParallelRansac() const { return useParallelRansac; }
327 
333  inline void setUseParallelRansac(bool use) { useParallelRansac = use; }
334 
340  std::vector<vpPoint> getPoints() const
341  {
342  std::vector<vpPoint> vectorOfPoints(listP.begin(), listP.end());
343  return vectorOfPoints;
344  }
345 
346  static bool computePlanarObjectPoseFromRGBD(const vpImage<float> &depthMap, const std::vector<vpImagePoint> &corners,
347  const vpCameraParameters &colorIntrinsics,
348  const std::vector<vpPoint> &point3d, vpHomogeneousMatrix &cMo,
349  double *confidence_index = NULL);
350 
351  static bool computePlanarObjectPoseFromRGBD(const vpImage<float> &depthMap,
352  const std::vector<std::vector<vpImagePoint> > &corners,
353  const vpCameraParameters &colorIntrinsics,
354  const std::vector<std::vector<vpPoint> > &point3d,
355  vpHomogeneousMatrix &cMo, double *confidence_index = NULL,
356  bool coplanar_points = true);
357  static int computeRansacIterations(double probability, double epsilon, const int sampleSize = 4,
358  int maxIterations = 2000);
359 
360  static void display(vpImage<unsigned char> &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size,
361  vpColor col = vpColor::none);
362  static void display(vpImage<vpRGBa> &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size,
363  vpColor col = vpColor::none);
364 
365  static void findMatch(std::vector<vpPoint> &p2D, std::vector<vpPoint> &p3D,
366  const unsigned int &numberOfInlierToReachAConsensus, const double &threshold,
367  unsigned int &ninliers, std::vector<vpPoint> &listInliers, vpHomogeneousMatrix &cMo,
368  const int &maxNbTrials = 10000, bool useParallelRansac = true, unsigned int nthreads = 0,
369  bool (*func)(const vpHomogeneousMatrix &) = NULL);
370 
371  static double poseFromRectangle(vpPoint &p1, vpPoint &p2, vpPoint &p3, vpPoint &p4, double lx,
373 
374 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \
375  (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911)))
376 
391  template <typename DataId>
392  static std::optional<vpHomogeneousMatrix> computePlanarObjectPoseWithAtLeast3Points(
393  const vpPlane &plane_in_camera_frame, const std::map<DataId, vpPoint> &pts,
394  const std::map<DataId, vpImagePoint> &ips, const vpCameraParameters &camera_intrinsics,
395  std::optional<vpHomogeneousMatrix> cMo_init = std::nullopt, bool enable_vvs = true)
396  {
397  if (cMo_init && !enable_vvs) {
398  throw(vpException(
400  "It doesn't make sense to use an initialized pose without enabling VVS to compute the pose from 4 points"));
401  }
402 
403  // Check if detection and model fit
404  // - The next line produces an internal compiler error with Visual Studio 2017:
405  // modules\vision\include\visp3/vision/vpPose.h(404): fatal error C1001: An internal error has occurred in the
406  // compiler.
407  // To work around this problem, try simplifying or changing the program near the locations listed above.
408  // Please choose the Technical Support command on the Visual C++
409  // Help menu, or open the Technical Support help file for more information
410  // - Note that the next line builds with Visual Studio 2022.
411  // for ([[maybe_unused]] const auto &[ip_id, _] : ips) {
412  for (const auto &[ip_id, ip_unused] : ips) {
413  (void)ip_unused;
414  if (pts.find(ip_id) == end(pts)) {
416  "Cannot compute pose with points and image points which do not have the same IDs"));
417  }
418  }
419 
420  std::vector<vpPoint> P{}, Q{};
421  // The next line in C++17 produces a build error with Visual Studio 2017, that's why we
422  // use rather C++11 to loop through std::map
423  // for (auto [pt_id, pt] : pts) {
424  for (const auto &pt_map : pts) {
425  if (ips.find(pt_map.first) != end(ips)) {
426  double x = 0, y = 0;
427  vpPoint pt = pt_map.second;
428  vpPixelMeterConversion::convertPoint(camera_intrinsics, ips.at(pt_map.first), x, y);
429  const auto Z = plane_in_camera_frame.computeZ(x, y);
430 
431  pt.set_x(x);
432  pt.set_y(y);
433  pt.set_Z(Z);
434 
435  Q.push_back(pt);
436  P.emplace_back(x * Z, y * Z, Z);
437  }
438  }
439 
440  if (Q.size() < 3) {
441  return std::nullopt;
442  }
443 
444  auto cMo = cMo_init.value_or(vpHomogeneousMatrix::compute3d3dTransformation(P, Q));
445  if (!cMo.isValid()) {
446  return std::nullopt;
447  }
448 
449  return enable_vvs ? vpPose::poseVirtualVSWithDepth(Q, cMo).value_or(cMo) : cMo;
450  }
451 
452  static std::optional<vpHomogeneousMatrix> poseVirtualVSWithDepth(const std::vector<vpPoint> &points,
453  const vpHomogeneousMatrix &cMo);
454 #endif
455 
456 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
461  vp_deprecated void init();
463 #endif
464 };
465 
466 #endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
static const vpColor none
Definition: vpColor.h:229
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:97
@ fatalError
Fatal error.
Definition: vpException.h:96
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:154
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:59
double computeZ(double x, double y) const
Definition: vpPlane.cpp:235
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:82
void set_x(double x)
Set the point x coordinate in the image plane.
Definition: vpPoint.cpp:511
void set_Z(double cZ)
Set the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:497
void set_y(double y)
Set the point y coordinate in the image plane.
Definition: vpPoint.cpp:513
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:90
void setRansacMaxTrials(const int &rM)
Definition: vpPose.h:260
void setRansacNbInliersToReachConsensus(const unsigned int &nbC)
Definition: vpPose.h:250
vpMatrix getCovarianceMatrix() const
Definition: vpPose.h:282
bool getUseParallelRansac() const
Definition: vpPose.h:326
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:392
vpPoseMethodType
Methods that could be used to estimate the pose from points.
Definition: vpPose.h:93
@ DEMENTHON
Definition: vpPose.h:95
@ RANSAC
Definition: vpPose.h:99
@ LAGRANGE
Definition: vpPose.h:94
void setCovarianceComputation(const bool &flag)
Definition: vpPose.h:271
unsigned int npt
Number of point used in pose computation.
Definition: vpPose.h:121
int getNbParallelRansacThreads() const
Definition: vpPose.h:309
std::vector< vpPoint > getPoints() const
Definition: vpPose.h:340
std::vector< unsigned int > getRansacInlierIndex() const
Definition: vpPose.h:262
double dementhonSvThresh
SVD threshold use for the pseudo-inverse computation in poseDementhonPlan.
Definition: vpPose.h:207
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Definition: vpPose.h:122
void setLambda(double a)
Definition: vpPose.h:239
void setRansacFilterFlag(const RANSAC_FILTER_FLAGS &flag)
Definition: vpPose.h:302
void setVvsIterMax(int nb)
Definition: vpPose.h:248
RANSAC_FILTER_FLAGS
Definition: vpPose.h:115
@ PREFILTER_DEGENERATE_POINTS
Definition: vpPose.h:117
@ NO_FILTER
Definition: vpPose.h:116
unsigned int getRansacNbInliers() const
Definition: vpPose.h:261
void setUseParallelRansac(bool use)
Definition: vpPose.h:333
static std::optional< vpHomogeneousMatrix > poseVirtualVSWithDepth(const std::vector< vpPoint > &points, const vpHomogeneousMatrix &cMo)
std::vector< vpPoint > getRansacInliers() const
Definition: vpPose.h:263
double lambda
parameters use for the virtual visual servoing approach
Definition: vpPose.h:127
void setVvsEpsilon(const double eps)
Definition: vpPose.h:240
void setNbParallelRansacThreads(int nb)
Definition: vpPose.h:319
double residual
Residual in meter.
Definition: vpPose.h:124
void setRansacThreshold(const double &t)
Definition: vpPose.h:251
Class for generating random numbers with uniform probability density.
Definition: vpUniRand.h:124
#define vpTRACE
Definition: vpDebug.h:416