Visual Servoing Platform version 3.5.0
vpPose.h
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 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 *
39 *****************************************************************************/
40
46#ifndef _vpPose_h_
47#define _vpPose_h_
48
49#include <visp3/core/vpHomogeneousMatrix.h>
50#include <visp3/core/vpPoint.h>
51#include <visp3/core/vpRGBa.h>
52#include <visp3/vision/vpHomography.h>
53#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
54#include <visp3/core/vpList.h>
55#endif
56#include <visp3/core/vpThread.h>
57
58#include <list>
59#include <math.h>
60#include <vector>
61#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
62#include <atomic>
63#endif
64
65#include <visp3/core/vpUniRand.h>
66
80class VISP_EXPORT vpPose
81{
82public:
84 typedef enum {
87 LOWE,
91 LAGRANGE_LOWE,
93 DEMENTHON_LOWE,
95 VIRTUAL_VS,
97 DEMENTHON_VIRTUAL_VS,
99 LAGRANGE_VIRTUAL_VS
101 } vpPoseMethodType;
102
106 CHECK_DEGENERATE_POINTS
107 };
108
109 unsigned int npt;
110 std::list<vpPoint> listP;
111
112 double residual;
113
114protected:
115 double lambda;
116
117private:
119 int vvsIterMax;
121 std::vector<vpPoint> c3d;
123 bool computeCovariance;
125 vpMatrix covarianceMatrix;
128 unsigned int ransacNbInlierConsensus;
130 int ransacMaxTrials;
132 std::vector<vpPoint> ransacInliers;
134 std::vector<unsigned int> ransacInlierIndex;
136 double ransacThreshold;
139 double distanceToPlaneForCoplanarityTest;
141 RANSAC_FILTER_FLAGS ransacFlag;
144 std::vector<vpPoint> listOfPoints;
146 bool useParallelRansac;
148 int nbParallelRansacThreads;
151 double vvsEpsilon;
152
153 // For parallel RANSAC
154 class RansacFunctor
155 {
156 public:
157 RansacFunctor(const vpHomogeneousMatrix &cMo_, unsigned int ransacNbInlierConsensus_, const int ransacMaxTrials_,
158 double ransacThreshold_, unsigned int initial_seed_, bool checkDegeneratePoints_,
159 const std::vector<vpPoint> &listOfUniquePoints_, bool (*func_)(const vpHomogeneousMatrix &))
160 : m_best_consensus(), m_checkDegeneratePoints(checkDegeneratePoints_), m_cMo(cMo_), m_foundSolution(false),
161 m_func(func_), m_listOfUniquePoints(listOfUniquePoints_), m_nbInliers(0), m_ransacMaxTrials(ransacMaxTrials_),
162 m_ransacNbInlierConsensus(ransacNbInlierConsensus_), m_ransacThreshold(ransacThreshold_),
163 m_uniRand(initial_seed_)
164 {
165 }
166
167 void operator()() { m_foundSolution = poseRansacImpl(); }
168
169 // Access the return value.
170 bool getResult() const { return m_foundSolution; }
171
172 std::vector<unsigned int> getBestConsensus() const { return m_best_consensus; }
173
174 vpHomogeneousMatrix getEstimatedPose() const { return m_cMo; }
175
176 unsigned int getNbInliers() const { return m_nbInliers; }
177
178 private:
179 std::vector<unsigned int> m_best_consensus;
180 bool m_checkDegeneratePoints;
182 bool m_foundSolution;
183 bool (*m_func)(const vpHomogeneousMatrix &);
184 std::vector<vpPoint> m_listOfUniquePoints;
185 unsigned int m_nbInliers;
186 int m_ransacMaxTrials;
187 unsigned int m_ransacNbInlierConsensus;
188 double m_ransacThreshold;
189 vpUniRand m_uniRand;
190
191 bool poseRansacImpl();
192 };
193
194protected:
195 double computeResidualDementhon(const vpHomogeneousMatrix &cMo);
196
197 // method used in poseDementhonPlan()
198 int calculArbreDementhon(vpMatrix &b, vpColVector &U, vpHomogeneousMatrix &cMo);
199
200public:
201 vpPose();
202 vpPose(const std::vector<vpPoint> &lP);
203 virtual ~vpPose();
204 void addPoint(const vpPoint &P);
205 void addPoints(const std::vector<vpPoint> &lP);
206 void clearPoint();
207
208 bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneousMatrix &) = NULL);
209 double computeResidual(const vpHomogeneousMatrix &cMo) const;
210 bool coplanar(int &coplanar_plane_type);
211 void displayModel(vpImage<unsigned char> &I, vpCameraParameters &cam, vpColor col = vpColor::none);
212 void displayModel(vpImage<vpRGBa> &I, vpCameraParameters &cam, vpColor col = vpColor::none);
213
214 void poseDementhonPlan(vpHomogeneousMatrix &cMo);
215 void poseDementhonNonPlan(vpHomogeneousMatrix &cMo);
216 void poseLagrangePlan(vpHomogeneousMatrix &cMo);
217 void poseLagrangeNonPlan(vpHomogeneousMatrix &cMo);
218 void poseLowe(vpHomogeneousMatrix &cMo);
219 bool poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneousMatrix &) = NULL);
220 void poseVirtualVSrobust(vpHomogeneousMatrix &cMo);
221 void poseVirtualVS(vpHomogeneousMatrix &cMo);
222 void printPoint();
223 void setDistanceToPlaneForCoplanarityTest(double d);
224 void setLambda(double a) { lambda = a; }
225 void setVvsEpsilon(const double eps)
226 {
227 if (eps >= 0) {
228 vvsEpsilon = eps;
229 } else {
230 throw vpException(vpException::badValue, "Epsilon value must be >= 0.");
231 }
232 }
233 void setVvsIterMax(int nb) { vvsIterMax = nb; }
234
235 void setRansacNbInliersToReachConsensus(const unsigned int &nbC) { ransacNbInlierConsensus = nbC; }
236 void setRansacThreshold(const double &t)
237 {
238 // Test whether or not t is > 0
239 if (t > std::numeric_limits<double>::epsilon()) {
240 ransacThreshold = t;
241 } else {
242 throw vpException(vpException::badValue, "The Ransac threshold must be positive as we deal with distance.");
243 }
244 }
245 void setRansacMaxTrials(const int &rM) { ransacMaxTrials = rM; }
246 unsigned int getRansacNbInliers() const { return (unsigned int)ransacInliers.size(); }
247 std::vector<unsigned int> getRansacInlierIndex() const { return ransacInlierIndex; }
248 std::vector<vpPoint> getRansacInliers() const { return ransacInliers; }
249
256 void setCovarianceComputation(const bool &flag) { computeCovariance = flag; }
257
268 {
269 if (!computeCovariance)
270 vpTRACE("Warning : The covariance matrix has not been computed. See "
271 "setCovarianceComputation() to do it.");
272
273 return covarianceMatrix;
274 }
275
287 inline void setRansacFilterFlag(const RANSAC_FILTER_FLAGS &flag) { ransacFlag = flag; }
288
294 inline int getNbParallelRansacThreads() const { return nbParallelRansacThreads; }
295
304 inline void setNbParallelRansacThreads(int nb) { nbParallelRansacThreads = nb; }
305
311 inline bool getUseParallelRansac() const { return useParallelRansac; }
312
318 inline void setUseParallelRansac(bool use) { useParallelRansac = use; }
319
325 std::vector<vpPoint> getPoints() const
326 {
327 std::vector<vpPoint> vectorOfPoints(listP.begin(), listP.end());
328 return vectorOfPoints;
329 }
330
331 static void display(vpImage<unsigned char> &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size,
332 vpColor col = vpColor::none);
333 static void display(vpImage<vpRGBa> &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size,
334 vpColor col = vpColor::none);
335 static double poseFromRectangle(vpPoint &p1, vpPoint &p2, vpPoint &p3, vpPoint &p4, double lx,
337
338 static int computeRansacIterations(double probability, double epsilon, const int sampleSize = 4,
339 int maxIterations = 2000);
340
341 static void findMatch(std::vector<vpPoint> &p2D, std::vector<vpPoint> &p3D,
342 const unsigned int &numberOfInlierToReachAConsensus, const double &threshold,
343 unsigned int &ninliers, std::vector<vpPoint> &listInliers, vpHomogeneousMatrix &cMo,
344 const int &maxNbTrials = 10000, bool useParallelRansac = true, unsigned int nthreads = 0,
345 bool (*func)(const vpHomogeneousMatrix &) = NULL);
346
347 static bool computePlanarObjectPoseFromRGBD(const vpImage<float> &depthMap, const std::vector<vpImagePoint> &corners,
348 const vpCameraParameters &colorIntrinsics,
349 const std::vector<vpPoint> &point3d, vpHomogeneousMatrix &cMo,
350 double *confidence_index = NULL);
351
352 static bool computePlanarObjectPoseFromRGBD(const vpImage<float> &depthMap,
353 const std::vector<std::vector<vpImagePoint> > &corners,
354 const vpCameraParameters &colorIntrinsics,
355 const std::vector<std::vector<vpPoint> > &point3d,
356 vpHomogeneousMatrix &cMo, double *confidence_index = NULL,
357 bool coplanar_points = true);
358
359#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
364 vp_deprecated void init();
366#endif
367};
368
369#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
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:82
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:325
void setRansacMaxTrials(const int &rM)
Definition: vpPose.h:245
void setRansacNbInliersToReachConsensus(const unsigned int &nbC)
Definition: vpPose.h:235
vpMatrix getCovarianceMatrix() const
Definition: vpPose.h:267
std::vector< unsigned int > getRansacInlierIndex() const
Definition: vpPose.h:247
bool getUseParallelRansac() const
Definition: vpPose.h:311
@ DEMENTHON
Definition: vpPose.h:86
@ RANSAC
Definition: vpPose.h:90
@ LAGRANGE
Definition: vpPose.h:85
void setCovarianceComputation(const bool &flag)
Definition: vpPose.h:256
unsigned int npt
Number of point used in pose computation.
Definition: vpPose.h:109
int getNbParallelRansacThreads() const
Definition: vpPose.h:294
std::vector< vpPoint > getRansacInliers() const
Definition: vpPose.h:248
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Definition: vpPose.h:110
void setLambda(double a)
Definition: vpPose.h:224
void setRansacFilterFlag(const RANSAC_FILTER_FLAGS &flag)
Definition: vpPose.h:287
void setVvsIterMax(int nb)
Definition: vpPose.h:233
RANSAC_FILTER_FLAGS
Definition: vpPose.h:103
@ PREFILTER_DEGENERATE_POINTS
Definition: vpPose.h:105
@ NO_FILTER
Definition: vpPose.h:104
unsigned int getRansacNbInliers() const
Definition: vpPose.h:246
void setUseParallelRansac(bool use)
Definition: vpPose.h:318
double lambda
parameters use for the virtual visual servoing approach
Definition: vpPose.h:115
void setVvsEpsilon(const double eps)
Definition: vpPose.h:225
void setNbParallelRansacThreads(int nb)
Definition: vpPose.h:304
double residual
Residual in meter.
Definition: vpPose.h:112
void setRansacThreshold(const double &t)
Definition: vpPose.h:236
Class for generating random numbers with uniform probability density.
Definition: vpUniRand.h:101
#define vpTRACE
Definition: vpDebug.h:416