Visual Servoing Platform  version 3.5.1 under development (2022-07-07)
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  } vpPoseMethodType;
111 
115  CHECK_DEGENERATE_POINTS
116  };
117 
118  unsigned int npt;
119  std::list<vpPoint> listP;
120 
121  double residual;
122 
123 protected:
124  double lambda;
125 
126 private:
128  int vvsIterMax;
130  std::vector<vpPoint> c3d;
132  bool computeCovariance;
134  vpMatrix covarianceMatrix;
137  unsigned int ransacNbInlierConsensus;
139  int ransacMaxTrials;
141  std::vector<vpPoint> ransacInliers;
143  std::vector<unsigned int> ransacInlierIndex;
145  double ransacThreshold;
148  double distanceToPlaneForCoplanarityTest;
150  RANSAC_FILTER_FLAGS ransacFlag;
153  std::vector<vpPoint> listOfPoints;
155  bool useParallelRansac;
157  int nbParallelRansacThreads;
160  double vvsEpsilon;
161 
162  // For parallel RANSAC
163  class RansacFunctor
164  {
165  public:
166  RansacFunctor(const vpHomogeneousMatrix &cMo_, unsigned int ransacNbInlierConsensus_, const int ransacMaxTrials_,
167  double ransacThreshold_, unsigned int initial_seed_, bool checkDegeneratePoints_,
168  const std::vector<vpPoint> &listOfUniquePoints_, bool (*func_)(const vpHomogeneousMatrix &))
169  : m_best_consensus(), m_checkDegeneratePoints(checkDegeneratePoints_), m_cMo(cMo_), m_foundSolution(false),
170  m_func(func_), m_listOfUniquePoints(listOfUniquePoints_), m_nbInliers(0), m_ransacMaxTrials(ransacMaxTrials_),
171  m_ransacNbInlierConsensus(ransacNbInlierConsensus_), m_ransacThreshold(ransacThreshold_),
172  m_uniRand(initial_seed_)
173  {
174  }
175 
176  void operator()() { m_foundSolution = poseRansacImpl(); }
177 
178  // Access the return value.
179  bool getResult() const { return m_foundSolution; }
180 
181  std::vector<unsigned int> getBestConsensus() const { return m_best_consensus; }
182 
183  vpHomogeneousMatrix getEstimatedPose() const { return m_cMo; }
184 
185  unsigned int getNbInliers() const { return m_nbInliers; }
186 
187  private:
188  std::vector<unsigned int> m_best_consensus;
189  bool m_checkDegeneratePoints;
190  vpHomogeneousMatrix m_cMo;
191  bool m_foundSolution;
192  bool (*m_func)(const vpHomogeneousMatrix &);
193  std::vector<vpPoint> m_listOfUniquePoints;
194  unsigned int m_nbInliers;
195  int m_ransacMaxTrials;
196  unsigned int m_ransacNbInlierConsensus;
197  double m_ransacThreshold;
198  vpUniRand m_uniRand;
199 
200  bool poseRansacImpl();
201  };
202 
203 protected:
204  double computeResidualDementhon(const vpHomogeneousMatrix &cMo);
205 
206  // method used in poseDementhonPlan()
207  int calculArbreDementhon(vpMatrix &b, vpColVector &U, vpHomogeneousMatrix &cMo);
208 
209 public:
210  vpPose();
211  vpPose(const std::vector<vpPoint> &lP);
212  virtual ~vpPose();
213  void addPoint(const vpPoint &P);
214  void addPoints(const std::vector<vpPoint> &lP);
215  void clearPoint();
216 
217  bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneousMatrix &) = NULL);
218  double computeResidual(const vpHomogeneousMatrix &cMo) const;
219  bool coplanar(int &coplanar_plane_type);
220  void displayModel(vpImage<unsigned char> &I, vpCameraParameters &cam, vpColor col = vpColor::none);
221  void displayModel(vpImage<vpRGBa> &I, vpCameraParameters &cam, vpColor col = vpColor::none);
222 
223  void poseDementhonPlan(vpHomogeneousMatrix &cMo);
224  void poseDementhonNonPlan(vpHomogeneousMatrix &cMo);
225  void poseLagrangePlan(vpHomogeneousMatrix &cMo);
226  void poseLagrangeNonPlan(vpHomogeneousMatrix &cMo);
227  void poseLowe(vpHomogeneousMatrix &cMo);
228  bool poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneousMatrix &) = NULL);
229  void poseVirtualVSrobust(vpHomogeneousMatrix &cMo);
230  void poseVirtualVS(vpHomogeneousMatrix &cMo);
231  void printPoint();
232  void setDistanceToPlaneForCoplanarityTest(double d);
233  void setLambda(double a) { lambda = a; }
234  void setVvsEpsilon(const double eps)
235  {
236  if (eps >= 0) {
237  vvsEpsilon = eps;
238  } else {
239  throw vpException(vpException::badValue, "Epsilon value must be >= 0.");
240  }
241  }
242  void setVvsIterMax(int nb) { vvsIterMax = nb; }
243 
244  void setRansacNbInliersToReachConsensus(const unsigned int &nbC) { ransacNbInlierConsensus = nbC; }
245  void setRansacThreshold(const double &t)
246  {
247  // Test whether or not t is > 0
248  if (t > std::numeric_limits<double>::epsilon()) {
249  ransacThreshold = t;
250  } else {
251  throw vpException(vpException::badValue, "The Ransac threshold must be positive as we deal with distance.");
252  }
253  }
254  void setRansacMaxTrials(const int &rM) { ransacMaxTrials = rM; }
255  unsigned int getRansacNbInliers() const { return (unsigned int)ransacInliers.size(); }
256  std::vector<unsigned int> getRansacInlierIndex() const { return ransacInlierIndex; }
257  std::vector<vpPoint> getRansacInliers() const { return ransacInliers; }
258 
265  void setCovarianceComputation(const bool &flag) { computeCovariance = flag; }
266 
277  {
278  if (!computeCovariance)
279  vpTRACE("Warning : The covariance matrix has not been computed. See "
280  "setCovarianceComputation() to do it.");
281 
282  return covarianceMatrix;
283  }
284 
296  inline void setRansacFilterFlag(const RANSAC_FILTER_FLAGS &flag) { ransacFlag = flag; }
297 
303  inline int getNbParallelRansacThreads() const { return nbParallelRansacThreads; }
304 
313  inline void setNbParallelRansacThreads(int nb) { nbParallelRansacThreads = nb; }
314 
320  inline bool getUseParallelRansac() const { return useParallelRansac; }
321 
327  inline void setUseParallelRansac(bool use) { useParallelRansac = use; }
328 
334  std::vector<vpPoint> getPoints() const
335  {
336  std::vector<vpPoint> vectorOfPoints(listP.begin(), listP.end());
337  return vectorOfPoints;
338  }
339 
340  static bool computePlanarObjectPoseFromRGBD(const vpImage<float> &depthMap, const std::vector<vpImagePoint> &corners,
341  const vpCameraParameters &colorIntrinsics,
342  const std::vector<vpPoint> &point3d, vpHomogeneousMatrix &cMo,
343  double *confidence_index = NULL);
344 
345  static bool computePlanarObjectPoseFromRGBD(const vpImage<float> &depthMap,
346  const std::vector<std::vector<vpImagePoint> > &corners,
347  const vpCameraParameters &colorIntrinsics,
348  const std::vector<std::vector<vpPoint> > &point3d,
349  vpHomogeneousMatrix &cMo, double *confidence_index = NULL,
350  bool coplanar_points = true);
351  static int computeRansacIterations(double probability, double epsilon, const int sampleSize = 4,
352  int maxIterations = 2000);
353 
354  static void display(vpImage<unsigned char> &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size,
355  vpColor col = vpColor::none);
356  static void display(vpImage<vpRGBa> &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size,
357  vpColor col = vpColor::none);
358 
359  static void findMatch(std::vector<vpPoint> &p2D, std::vector<vpPoint> &p3D,
360  const unsigned int &numberOfInlierToReachAConsensus, const double &threshold,
361  unsigned int &ninliers, std::vector<vpPoint> &listInliers, vpHomogeneousMatrix &cMo,
362  const int &maxNbTrials = 10000, bool useParallelRansac = true, unsigned int nthreads = 0,
363  bool (*func)(const vpHomogeneousMatrix &) = NULL);
364 
365  static double poseFromRectangle(vpPoint &p1, vpPoint &p2, vpPoint &p3, vpPoint &p4, double lx,
367 
368 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \
369  (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911)))
370 
385  template <typename DataId>
386  static std::optional<vpHomogeneousMatrix> computePlanarObjectPoseWithAtLeast3Points(
387  const vpPlane &plane_in_camera_frame, const std::map<DataId, vpPoint> &pts,
388  const std::map<DataId, vpImagePoint> &ips, const vpCameraParameters &camera_intrinsics,
389  std::optional<vpHomogeneousMatrix> cMo_init = std::nullopt, bool enable_vvs = true)
390  {
391  if (cMo_init && !enable_vvs) {
392  throw(vpException(
394  "It doesn't make sense to use an initialized pose without enabling VVS to compute the pose from 4 points"));
395  }
396 
397  // Check if detection and model fit
398  // - The next line produces an internal compiler error with Visual Studio 2017:
399  // modules\vision\include\visp3/vision/vpPose.h(404): fatal error C1001: An internal error has occurred in the
400  // compiler.
401  // To work around this problem, try simplifying or changing the program near the locations listed above.
402  // Please choose the Technical Support command on the Visual C++
403  // Help menu, or open the Technical Support help file for more information
404  // - Note that the next line builds with Visual Studio 2022.
405  // for ([[maybe_unused]] const auto &[ip_id, _] : ips) {
406  for (const auto &[ip_id, ip_unused] : ips) {
407  (void)ip_unused;
408  if (pts.find(ip_id) == end(pts)) {
410  "Cannot compute pose with points and image points which do not have the same IDs"));
411  }
412  }
413 
414  std::vector<vpPoint> P{}, Q{};
415  // The next line in C++17 produces a build error with Visual Studio 2017, that's why we
416  // use rather C++11 to loop through std::map
417  // for (auto [pt_id, pt] : pts) {
418  for (const auto &pt_map : pts) {
419  if (ips.find(pt_map.first) != end(ips)) {
420  double x = 0, y = 0;
421  vpPoint pt = pt_map.second;
422  vpPixelMeterConversion::convertPoint(camera_intrinsics, ips.at(pt_map.first), x, y);
423  const auto Z = plane_in_camera_frame.computeZ(x, y);
424 
425  pt.set_x(x);
426  pt.set_y(y);
427  pt.set_Z(Z);
428 
429  Q.push_back(pt);
430  P.emplace_back(x * Z, y * Z, Z);
431  }
432  }
433 
434  if (Q.size() < 3) {
435  return std::nullopt;
436  }
437 
438  auto cMo = cMo_init.value_or(vpHomogeneousMatrix::compute3d3dTransformation(P, Q));
439  if (!cMo.isValid()) {
440  return std::nullopt;
441  }
442 
443  return enable_vvs ? vpPose::poseVirtualVSWithDepth(Q, cMo).value_or(cMo) : cMo;
444  }
445 
446  static std::optional<vpHomogeneousMatrix> poseVirtualVSWithDepth(std::vector<vpPoint> points,
447  vpHomogeneousMatrix cMo);
448 #endif
449 
450 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
455  vp_deprecated void init();
457 #endif
458 };
459 
460 #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:254
void setRansacNbInliersToReachConsensus(const unsigned int &nbC)
Definition: vpPose.h:244
vpMatrix getCovarianceMatrix() const
Definition: vpPose.h:276
bool getUseParallelRansac() const
Definition: vpPose.h:320
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:386
@ DEMENTHON
Definition: vpPose.h:95
@ RANSAC
Definition: vpPose.h:99
@ LAGRANGE
Definition: vpPose.h:94
void setCovarianceComputation(const bool &flag)
Definition: vpPose.h:265
unsigned int npt
Number of point used in pose computation.
Definition: vpPose.h:118
int getNbParallelRansacThreads() const
Definition: vpPose.h:303
std::vector< vpPoint > getPoints() const
Definition: vpPose.h:334
std::vector< unsigned int > getRansacInlierIndex() const
Definition: vpPose.h:256
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Definition: vpPose.h:119
void setLambda(double a)
Definition: vpPose.h:233
void setRansacFilterFlag(const RANSAC_FILTER_FLAGS &flag)
Definition: vpPose.h:296
void setVvsIterMax(int nb)
Definition: vpPose.h:242
RANSAC_FILTER_FLAGS
Definition: vpPose.h:112
@ PREFILTER_DEGENERATE_POINTS
Definition: vpPose.h:114
@ NO_FILTER
Definition: vpPose.h:113
unsigned int getRansacNbInliers() const
Definition: vpPose.h:255
void setUseParallelRansac(bool use)
Definition: vpPose.h:327
static std::optional< vpHomogeneousMatrix > poseVirtualVSWithDepth(std::vector< vpPoint > points, vpHomogeneousMatrix cMo)
std::vector< vpPoint > getRansacInliers() const
Definition: vpPose.h:257
double lambda
parameters use for the virtual visual servoing approach
Definition: vpPose.h:124
void setVvsEpsilon(const double eps)
Definition: vpPose.h:234
void setNbParallelRansacThreads(int nb)
Definition: vpPose.h:313
double residual
Residual in meter.
Definition: vpPose.h:121
void setRansacThreshold(const double &t)
Definition: vpPose.h:245
Class for generating random numbers with uniform probability density.
Definition: vpUniRand.h:101
#define vpTRACE
Definition: vpDebug.h:416