Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
56#include <atomic>
57#endif
58
59#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \
60 (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911)))
61#include <map>
62#include <optional>
63#endif
64
65#include <visp3/core/vpUniRand.h>
66
80class VISP_EXPORT vpPose
81{
82public:
84 typedef enum
85 {
88 LOWE,
92 LAGRANGE_LOWE,
94 DEMENTHON_LOWE,
96 VIRTUAL_VS,
98 DEMENTHON_VIRTUAL_VS,
100 LAGRANGE_VIRTUAL_VS,
102 DEMENTHON_LAGRANGE_VIRTUAL_VS,
105 } vpPoseMethodType;
106
108 {
111 CHECK_DEGENERATE_POINTS
112 };
113
114 unsigned int npt;
115 std::list<vpPoint> listP;
116
117 double residual;
118
119protected:
120 double lambda;
121
122private:
124 int vvsIterMax;
126 std::vector<vpPoint> c3d;
128 bool computeCovariance;
130 vpMatrix covarianceMatrix;
133 unsigned int ransacNbInlierConsensus;
135 int ransacMaxTrials;
137 std::vector<vpPoint> ransacInliers;
139 std::vector<unsigned int> ransacInlierIndex;
141 double ransacThreshold;
144 double distanceToPlaneForCoplanarityTest;
146 RANSAC_FILTER_FLAGS ransacFlag;
149 std::vector<vpPoint> listOfPoints;
151 bool useParallelRansac;
153 int nbParallelRansacThreads;
156 double vvsEpsilon;
157
158 // For parallel RANSAC
159 class RansacFunctor
160 {
161 public:
162 RansacFunctor(const vpHomogeneousMatrix &cMo_, unsigned int ransacNbInlierConsensus_, const int ransacMaxTrials_,
163 double ransacThreshold_, unsigned int initial_seed_, bool checkDegeneratePoints_,
164 const std::vector<vpPoint> &listOfUniquePoints_, bool (*func_)(const vpHomogeneousMatrix &))
165 : m_best_consensus(), m_checkDegeneratePoints(checkDegeneratePoints_), m_cMo(cMo_), m_foundSolution(false),
166 m_func(func_), m_listOfUniquePoints(listOfUniquePoints_), m_nbInliers(0), m_ransacMaxTrials(ransacMaxTrials_),
167 m_ransacNbInlierConsensus(ransacNbInlierConsensus_), m_ransacThreshold(ransacThreshold_),
168 m_uniRand(initial_seed_)
169 { }
170
171 void operator()() { m_foundSolution = poseRansacImpl(); }
172
173 // Access the return value.
174 bool getResult() const { return m_foundSolution; }
175
176 std::vector<unsigned int> getBestConsensus() const { return m_best_consensus; }
177
178 vpHomogeneousMatrix getEstimatedPose() const { return m_cMo; }
179
180 unsigned int getNbInliers() const { return m_nbInliers; }
181
182 private:
183 std::vector<unsigned int> m_best_consensus;
184 bool m_checkDegeneratePoints;
186 bool m_foundSolution;
187 bool (*m_func)(const vpHomogeneousMatrix &);
188 std::vector<vpPoint> m_listOfUniquePoints;
189 unsigned int m_nbInliers;
190 int m_ransacMaxTrials;
191 unsigned int m_ransacNbInlierConsensus;
192 double m_ransacThreshold;
193 vpUniRand m_uniRand;
194
195 bool poseRansacImpl();
196 };
197
198protected:
200 double computeResidualDementhon(const vpHomogeneousMatrix &cMo);
201
202 // method used in poseDementhonPlan()
203 int calculArbreDementhon(vpMatrix &b, vpColVector &U, vpHomogeneousMatrix &cMo);
204
205public:
206 vpPose();
207 vpPose(const std::vector<vpPoint> &lP);
208 virtual ~vpPose();
209 void addPoint(const vpPoint &P);
210 void addPoints(const std::vector<vpPoint> &lP);
211 void clearPoint();
212
213 bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneousMatrix &) = NULL);
214 bool computePoseDementhonLagrangeVVS(vpHomogeneousMatrix &cMo);
215 double computeResidual(const vpHomogeneousMatrix &cMo) const;
216 double computeResidual(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam) const;
217 double computeResidual(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, vpColVector &squaredResidual) const;
218 bool coplanar(int &coplanar_plane_type, double *p_a = NULL, double *p_b = NULL, double *p_c = NULL, double *p_d = NULL);
219 void displayModel(vpImage<unsigned char> &I, vpCameraParameters &cam, vpColor col = vpColor::none);
220 void displayModel(vpImage<vpRGBa> &I, vpCameraParameters &cam, vpColor col = vpColor::none);
221
222 void poseDementhonPlan(vpHomogeneousMatrix &cMo);
223 void poseDementhonNonPlan(vpHomogeneousMatrix &cMo);
224 void poseLagrangePlan(vpHomogeneousMatrix &cMo, bool *p_isPlan = NULL, double *p_a = NULL, double *p_b = NULL, double *p_c = NULL, double *p_d = NULL);
225 void poseLagrangeNonPlan(vpHomogeneousMatrix &cMo);
226 void poseLowe(vpHomogeneousMatrix &cMo);
227 bool poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneousMatrix &) = NULL);
228 void poseVirtualVSrobust(vpHomogeneousMatrix &cMo);
229 void poseVirtualVS(vpHomogeneousMatrix &cMo);
230 void printPoint();
231 void setDementhonSvThreshold(const double &svThresh);
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 }
239 else {
240 throw vpException(vpException::badValue, "Epsilon value must be >= 0.");
241 }
242 }
243 void setVvsIterMax(int nb) { vvsIterMax = nb; }
244
245 void setRansacNbInliersToReachConsensus(const unsigned int &nbC) { ransacNbInlierConsensus = nbC; }
246 void setRansacThreshold(const double &t)
247 {
248 // Test whether or not t is > 0
249 if (t > std::numeric_limits<double>::epsilon()) {
250 ransacThreshold = t;
251 }
252 else {
253 throw vpException(vpException::badValue, "The Ransac threshold must be positive as we deal with distance.");
254 }
255 }
256 void setRansacMaxTrials(const int &rM) { ransacMaxTrials = rM; }
257 unsigned int getRansacNbInliers() const { return (unsigned int)ransacInliers.size(); }
258 std::vector<unsigned int> getRansacInlierIndex() const { return ransacInlierIndex; }
259 std::vector<vpPoint> getRansacInliers() const { return ransacInliers; }
260
267 void setCovarianceComputation(const bool &flag) { computeCovariance = flag; }
268
279 {
280 if (!computeCovariance)
281 vpTRACE("Warning : The covariance matrix has not been computed. See "
282 "setCovarianceComputation() to do it.");
283
284 return covarianceMatrix;
285 }
286
298 inline void setRansacFilterFlag(const RANSAC_FILTER_FLAGS &flag) { ransacFlag = flag; }
299
305 inline int getNbParallelRansacThreads() const { return nbParallelRansacThreads; }
306
315 inline void setNbParallelRansacThreads(int nb) { nbParallelRansacThreads = nb; }
316
322 inline bool getUseParallelRansac() const { return useParallelRansac; }
323
329 inline void setUseParallelRansac(bool use) { useParallelRansac = use; }
330
336 std::vector<vpPoint> getPoints() const
337 {
338 std::vector<vpPoint> vectorOfPoints(listP.begin(), listP.end());
339 return vectorOfPoints;
340 }
341
342 static bool computePlanarObjectPoseFromRGBD(const vpImage<float> &depthMap, const std::vector<vpImagePoint> &corners,
343 const vpCameraParameters &colorIntrinsics,
344 const std::vector<vpPoint> &point3d, vpHomogeneousMatrix &cMo,
345 double *confidence_index = NULL);
346
347 static bool computePlanarObjectPoseFromRGBD(const vpImage<float> &depthMap,
348 const std::vector<std::vector<vpImagePoint> > &corners,
349 const vpCameraParameters &colorIntrinsics,
350 const std::vector<std::vector<vpPoint> > &point3d,
351 vpHomogeneousMatrix &cMo, double *confidence_index = NULL,
352 bool coplanar_points = true);
353 static int computeRansacIterations(double probability, double epsilon, const int sampleSize = 4,
354 int maxIterations = 2000);
355
356 static void display(vpImage<unsigned char> &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size,
357 vpColor col = vpColor::none);
358 static void display(vpImage<vpRGBa> &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size,
359 vpColor col = vpColor::none);
360
361 static void findMatch(std::vector<vpPoint> &p2D, std::vector<vpPoint> &p3D,
362 const unsigned int &numberOfInlierToReachAConsensus, const double &threshold,
363 unsigned int &ninliers, std::vector<vpPoint> &listInliers, vpHomogeneousMatrix &cMo,
364 const int &maxNbTrials = 10000, bool useParallelRansac = true, unsigned int nthreads = 0,
365 bool (*func)(const vpHomogeneousMatrix &) = NULL);
366
367 static double poseFromRectangle(vpPoint &p1, vpPoint &p2, vpPoint &p3, vpPoint &p4, double lx,
369
370#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \
371 (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911)))
372
387 template <typename DataId>
388 static std::optional<vpHomogeneousMatrix> computePlanarObjectPoseWithAtLeast3Points(
389 const vpPlane &plane_in_camera_frame, const std::map<DataId, vpPoint> &pts,
390 const std::map<DataId, vpImagePoint> &ips, const vpCameraParameters &camera_intrinsics,
391 std::optional<vpHomogeneousMatrix> cMo_init = std::nullopt, bool enable_vvs = true)
392 {
393 if (cMo_init && !enable_vvs) {
394 throw(vpException(
396 "It doesn't make sense to use an initialized pose without enabling VVS to compute the pose from 4 points"));
397 }
398
399 // Check if detection and model fit
400 // - The next line produces an internal compiler error with Visual Studio 2017:
401 // modules\vision\include\visp3/vision/vpPose.h(404): fatal error C1001: An internal error has occurred in the
402 // compiler.
403 // To work around this problem, try simplifying or changing the program near the locations listed above.
404 // Please choose the Technical Support command on the Visual C++
405 // Help menu, or open the Technical Support help file for more information
406 // - Note that the next line builds with Visual Studio 2022.
407 // for ([[maybe_unused]] const auto &[ip_id, _] : ips) {
408 for (const auto &[ip_id, ip_unused] : ips) {
409 (void)ip_unused;
410 if (pts.find(ip_id) == end(pts)) {
412 "Cannot compute pose with points and image points which do not have the same IDs"));
413 }
414 }
415
416 std::vector<vpPoint> P {}, Q {};
417 // The next line in C++17 produces a build error with Visual Studio 2017, that's why we
418 // use rather C++11 to loop through std::map
419 // for (auto [pt_id, pt] : pts) {
420 for (const auto &pt_map : pts) {
421 if (ips.find(pt_map.first) != end(ips)) {
422 double x = 0, y = 0;
423 vpPoint pt = pt_map.second;
424 vpPixelMeterConversion::convertPoint(camera_intrinsics, ips.at(pt_map.first), x, y);
425 const auto Z = plane_in_camera_frame.computeZ(x, y);
426
427 pt.set_x(x);
428 pt.set_y(y);
429 pt.set_Z(Z);
430
431 Q.push_back(pt);
432 P.emplace_back(x * Z, y * Z, Z);
433 }
434 }
435
436 if (Q.size() < 3) {
437 return std::nullopt;
438 }
439
440 auto cMo = cMo_init.value_or(vpHomogeneousMatrix::compute3d3dTransformation(P, Q));
441 if (!cMo.isValid()) {
442 return std::nullopt;
443 }
444
445 return enable_vvs ? vpPose::poseVirtualVSWithDepth(Q, cMo).value_or(cMo) : cMo;
446 }
447
448 static std::optional<vpHomogeneousMatrix> poseVirtualVSWithDepth(const std::vector<vpPoint> &points,
449 const vpHomogeneousMatrix &cMo);
450#endif
451
452#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
457 vp_deprecated void init();
459#endif
460};
461
462#endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
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)
Definition of the vpImage class member functions.
Definition vpImage.h:135
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
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:508
void set_Z(double cZ)
Set the point cZ coordinate in the camera frame.
Definition vpPoint.cpp:494
void set_y(double y)
Set the point y coordinate in the image plane.
Definition vpPoint.cpp:510
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition vpPose.h:81
std::vector< vpPoint > getPoints() const
Definition vpPose.h:336
void setRansacMaxTrials(const int &rM)
Definition vpPose.h:256
void setRansacNbInliersToReachConsensus(const unsigned int &nbC)
Definition vpPose.h:245
vpMatrix getCovarianceMatrix() const
Definition vpPose.h:278
std::vector< unsigned int > getRansacInlierIndex() const
Definition vpPose.h:258
bool getUseParallelRansac() const
Definition vpPose.h:322
vpPoseMethodType
Methods that could be used to estimate the pose from points.
Definition vpPose.h:85
@ DEMENTHON
Definition vpPose.h:87
@ RANSAC
Definition vpPose.h:91
@ LAGRANGE
Definition vpPose.h:86
void setCovarianceComputation(const bool &flag)
Definition vpPose.h:267
unsigned int npt
Number of point used in pose computation.
Definition vpPose.h:114
int getNbParallelRansacThreads() const
Definition vpPose.h:305
std::vector< vpPoint > getRansacInliers() const
Definition vpPose.h:259
double dementhonSvThresh
SVD threshold use for the pseudo-inverse computation in poseDementhonPlan.
Definition vpPose.h:199
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Definition vpPose.h:115
void setLambda(double a)
Definition vpPose.h:233
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:388
void setRansacFilterFlag(const RANSAC_FILTER_FLAGS &flag)
Definition vpPose.h:298
void setVvsIterMax(int nb)
Definition vpPose.h:243
RANSAC_FILTER_FLAGS
Definition vpPose.h:108
@ PREFILTER_DEGENERATE_POINTS
Definition vpPose.h:110
@ NO_FILTER
Definition vpPose.h:109
unsigned int getRansacNbInliers() const
Definition vpPose.h:257
void setUseParallelRansac(bool use)
Definition vpPose.h:329
static std::optional< vpHomogeneousMatrix > poseVirtualVSWithDepth(const std::vector< vpPoint > &points, const vpHomogeneousMatrix &cMo)
double lambda
parameters use for the virtual visual servoing approach
Definition vpPose.h:120
void setVvsEpsilon(const double eps)
Definition vpPose.h:234
void setNbParallelRansacThreads(int nb)
Definition vpPose.h:315
double residual
Residual in meter.
Definition vpPose.h:117
void setRansacThreshold(const double &t)
Definition vpPose.h:246
Class for generating random numbers with uniform probability density.
Definition vpUniRand.h:122
#define vpTRACE
Definition vpDebug.h:411