Visual Servoing Platform  version 3.6.1 under development (2024-06-18)
vpPoseRansac.cpp
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 #include <algorithm> // std::count
40 #include <cmath> // std::fabs
41 #include <float.h> // DBL_MAX
42 #include <iostream>
43 #include <limits> // numeric_limits
44 #include <map>
45 
46 #include <visp3/core/vpColVector.h>
47 #include <visp3/core/vpMath.h>
48 #include <visp3/core/vpRansac.h>
49 #include <visp3/vision/vpPose.h>
50 #include <visp3/vision/vpPoseException.h>
51 
52 #if defined(VISP_HAVE_THREADS)
53 #include <thread>
54 #endif
55 
57 #define EPS 1e-6
58 
59 namespace
60 {
61 // For std::map<vpPoint>
62 struct CompareObjectPointDegenerate
63 {
64  bool operator()(const vpPoint &point1, const vpPoint &point2) const
65  {
66  bool rc = false;
67  const double dist1 =
68  (point1.get_oX() * point1.get_oX()) + (point1.get_oY() * point1.get_oY()) + (point1.get_oZ() * point1.get_oZ());
69  const double dist2 =
70  (point2.get_oX() * point2.get_oX()) + (point2.get_oY() * point2.get_oY()) + (point2.get_oZ() * point2.get_oZ());
71 
72  if ((dist1 - dist2) < (-3 * EPS * EPS)) {
73  return true;
74  }
75  if ((dist1 - dist2) > (3 * EPS * EPS)) {
76  return false;
77  }
78 
79  if ((point1.oP[0] - point2.oP[0]) < -EPS) {
80  return true;
81  }
82  if ((point1.oP[0] - point2.oP[0]) > EPS) {
83  return false;
84  }
85 
86  if ((point1.oP[1] - point2.oP[1]) < -EPS) {
87  return true;
88  }
89  if ((point1.oP[1] - point2.oP[1]) > EPS) {
90  return false;
91  }
92 
93  if ((point1.oP[2] - point2.oP[2]) < -EPS) {
94  return true;
95  }
96  if ((point1.oP[2] - point2.oP[2]) > EPS) {
97  return false;
98  }
99 
100  return rc;
101  }
102 };
103 
104 // For std::map<vpPoint>
105 struct CompareImagePointDegenerate
106 {
107  bool operator()(const vpPoint &point1, const vpPoint &point2) const
108  {
109  bool rc = false;
110  const double dist1 = (point1.get_x() * point1.get_x()) + (point1.get_y() * point1.get_y());
111  const double dist2 = (point2.get_x() * point2.get_x()) + (point2.get_y() * point2.get_y());
112  if ((dist1 - dist2) < (-2 * EPS * EPS)) {
113  return true;
114  }
115  if ((dist1 - dist2) > (2 * EPS * EPS)) {
116  return false;
117  }
118 
119  if ((point1.p[0] - point2.p[0]) < -EPS) {
120  return true;
121  }
122  if ((point1.p[0] - point2.p[0]) > EPS) {
123  return false;
124  }
125 
126  if ((point1.p[1] - point2.p[1]) < -EPS) {
127  return true;
128  }
129  if ((point1.p[1] - point2.p[1]) > EPS) {
130  return false;
131  }
132 
133  return rc;
134  }
135 };
136 
137 // std::find_if
138 struct FindDegeneratePoint
139 {
140  explicit FindDegeneratePoint(const vpPoint &pt) : m_pt(pt) { }
141 
142  bool operator()(const vpPoint &pt)
143  {
144  bool result_cond1 = ((std::fabs(m_pt.oP[0] - pt.oP[0]) < EPS) && (std::fabs(m_pt.oP[1] - pt.oP[1]) < EPS)
145  && (std::fabs(m_pt.oP[2] - pt.oP[2]) < EPS));
146  bool result_cond2 = (std::fabs(m_pt.p[0] - pt.p[0]) < EPS) && (std::fabs(m_pt.p[1] - pt.p[1]) < EPS);
147  return result_cond1 || result_cond2;
148  }
149 
150  vpPoint m_pt;
151 };
152 } // namespace
153 
154 bool vpPose::vpRansacFunctor::poseRansacImpl()
155 {
156  const unsigned int size = static_cast<unsigned int>(m_listOfUniquePoints.size());
157  unsigned int nbMinRandom = 4;
158  int nbTrials = 0;
159 
160  vpPoint p; // Point used to project using the estimated pose
161 
162  bool foundSolution = false;
163  while ((nbTrials < m_ransacMaxTrials) && (m_nbInliers < m_ransacNbInlierConsensus)) {
164  // Hold the list of the index of the inliers (points in the consensus set)
165  std::vector<unsigned int> cur_consensus;
166  // Hold the list of the index of the outliers
167  std::vector<unsigned int> cur_outliers;
168  // Hold the list of the index of the points randomly picked
169  std::vector<unsigned int> cur_randoms;
170  // Hold the list of the current inliers points to avoid to add a
171  // degenerate point if the flag is set
172  std::vector<vpPoint> cur_inliers;
173 
174  // Use a temporary variable because if not, the cMo passed in parameters
175  // will be modified when
176  // we compute the pose for the minimal sample sets but if the pose is not
177  // correct when we pass a function pointer we do not want to modify the
178  // cMo passed in parameters
179  vpHomogeneousMatrix cMo_tmp;
180 
181  // Vector of used points, initialized at false for all points
182  std::vector<bool> usedPt(size, false);
183 
184  vpPose poseMin;
185  for (unsigned int i = 0; i < nbMinRandom;) {
186  if (static_cast<size_t>(std::count(usedPt.begin(), usedPt.end(), true)) == usedPt.size()) {
187  // All points was picked once, break otherwise we stay in an infinite loop
188  break;
189  }
190 
191  // Pick a point randomly
192  unsigned int r_ = m_uniRand.uniform(0, size);
193 
194  while (usedPt[r_]) {
195  // If already picked, pick another point randomly
196  r_ = m_uniRand.uniform(0, size);
197  }
198  // Mark this point as already picked
199  usedPt[r_] = true;
200  vpPoint pt = m_listOfUniquePoints[r_];
201 
202  bool degenerate = false;
203  if (m_checkDegeneratePoints) {
204  if (std::find_if(poseMin.listOfPoints.begin(), poseMin.listOfPoints.end(), FindDegeneratePoint(pt)) !=
205  poseMin.listOfPoints.end()) {
206  degenerate = true;
207  }
208  }
209 
210  if (!degenerate) {
211  poseMin.addPoint(pt);
212  cur_randoms.push_back(r_);
213  // Increment the number of points picked
214  ++i;
215  }
216  }
217 
218  if (poseMin.npt < nbMinRandom) {
219  ++nbTrials;
220  continue;
221  }
222 
223  bool is_pose_valid = false;
224  double r_min = DBL_MAX;
225 
226  try {
227  is_pose_valid = poseMin.computePose(vpPose::DEMENTHON_LAGRANGE_VIRTUAL_VS, cMo_tmp);
228  r_min = poseMin.computeResidual(cMo_tmp);
229  }
230  catch (...) {
231  // no need to take action
232  }
233 
234  // If residual returned is not a number (NAN), set valid to false
235  if (vpMath::isNaN(r_min)) {
236  is_pose_valid = false;
237  }
238 
239  // If at pose computation is OK we can continue, otherwise pick another random set
240  if (is_pose_valid) {
241  double r = sqrt(r_min) / static_cast<double>(nbMinRandom); // FS should be r = sqrt(r_min / (double)nbMinRandom);
242  // Filter the pose using some criterion (orientation angles,
243  // translations, etc.)
244  bool isPoseValid = true;
245  if (m_func != nullptr) {
246  isPoseValid = m_func(cMo_tmp);
247  if (isPoseValid) {
248  m_cMo = cMo_tmp;
249  }
250  }
251  else {
252  // No post filtering on pose, so copy cMo_temp to cMo
253  m_cMo = cMo_tmp;
254  }
255 
256  if (isPoseValid && (r < m_ransacThreshold)) {
257  unsigned int nbInliersCur = 0;
258  unsigned int iter = 0;
259  std::vector<vpPoint>::const_iterator m_listofuniquepoints_end = m_listOfUniquePoints.end();
260  for (std::vector<vpPoint>::const_iterator it = m_listOfUniquePoints.begin(); it != m_listofuniquepoints_end;
261  ++it, ++iter) {
262  p.setWorldCoordinates(it->get_oX(), it->get_oY(), it->get_oZ());
263  p.track(m_cMo);
264 
265  double error = sqrt(vpMath::sqr(p.get_x() - it->get_x()) + vpMath::sqr(p.get_y() - it->get_y()));
266  if (error < m_ransacThreshold) {
267  bool degenerate = false;
268  if (m_checkDegeneratePoints) {
269  if (std::find_if(cur_inliers.begin(), cur_inliers.end(), FindDegeneratePoint(*it)) != cur_inliers.end()) {
270  degenerate = true;
271  }
272  }
273 
274  if (!degenerate) {
275  // the point is considered as inlier if the error is below the
276  // threshold
277  ++nbInliersCur;
278  cur_consensus.push_back(iter);
279  cur_inliers.push_back(*it);
280  }
281  else {
282  cur_outliers.push_back(iter);
283  }
284  }
285  else {
286  cur_outliers.push_back(iter);
287  }
288  }
289 
290  if (nbInliersCur > m_nbInliers) {
291  foundSolution = true;
292  m_best_consensus = cur_consensus;
293  m_nbInliers = nbInliersCur;
294  }
295 
296  ++nbTrials;
297 
298  if (nbTrials >= m_ransacMaxTrials) {
299  foundSolution = true;
300  }
301  }
302  else {
303  ++nbTrials;
304  }
305  }
306  else {
307  ++nbTrials;
308  }
309  }
310 
311  return foundSolution;
312 }
313 
315 {
316  // Check only for adding / removing problem
317  // Do not take into account problem with element modification here
318  if (listP.size() != listOfPoints.size()) {
319  std::cerr << "You should not modify vpPose::listP!" << std::endl;
320  listOfPoints = std::vector<vpPoint>(listP.begin(), listP.end());
321  }
322 
323  ransacInliers.clear();
324  ransacInlierIndex.clear();
325 
326  std::vector<unsigned int> best_consensus;
327  unsigned int nbInliers = 0;
328 
329  vpHomogeneousMatrix cMo_lagrange, cMo_dementhon;
330 
331  if (listOfPoints.size() < 4) {
332  throw(vpPoseException(vpPoseException::notInitializedError, "Not enough point to compute the pose"));
333  }
334 
335  std::vector<vpPoint> listOfUniquePoints;
336  std::map<size_t, size_t> mapOfUniquePointIndex;
337 
338  // Get RANSAC flags
339  bool prefilterDegeneratePoints = ransacFlag == PREFILTER_DEGENERATE_POINTS;
340  bool checkDegeneratePoints = ransacFlag == CHECK_DEGENERATE_POINTS;
341 
342  if (prefilterDegeneratePoints) {
343  // Remove degenerate object points
344  std::map<vpPoint, size_t, CompareObjectPointDegenerate> filterObjectPointMap;
345  size_t index_pt = 0;
346  std::vector<vpPoint>::const_iterator listofpoints_end = listOfPoints.end();
347  for (std::vector<vpPoint>::const_iterator it_pt = listOfPoints.begin(); it_pt != listofpoints_end;
348  ++it_pt, ++index_pt) {
349  if (filterObjectPointMap.find(*it_pt) == filterObjectPointMap.end()) {
350  filterObjectPointMap[*it_pt] = index_pt;
351  }
352  }
353 
354  std::map<vpPoint, size_t, CompareImagePointDegenerate> filterImagePointMap;
355  std::map<vpPoint, size_t, CompareObjectPointDegenerate>::const_iterator filterobjectpointmap_end = filterObjectPointMap.end();
356  for (std::map<vpPoint, size_t, CompareObjectPointDegenerate>::const_iterator it = filterObjectPointMap.begin();
357  it != filterobjectpointmap_end; ++it) {
358  if (filterImagePointMap.find(it->first) == filterImagePointMap.end()) {
359  filterImagePointMap[it->first] = it->second;
360 
361  listOfUniquePoints.push_back(it->first);
362  mapOfUniquePointIndex[listOfUniquePoints.size() - 1] = it->second;
363  }
364  }
365  }
366  else {
367  // No prefiltering
368  listOfUniquePoints = listOfPoints;
369 
370  size_t index_pt = 0;
371  std::vector<vpPoint>::const_iterator listofpoints_end = listOfPoints.end();
372  for (std::vector<vpPoint>::const_iterator it_pt = listOfPoints.begin(); it_pt != listofpoints_end;
373  ++it_pt, ++index_pt) {
374  mapOfUniquePointIndex[index_pt] = index_pt;
375  }
376  }
377 
378  if (listOfUniquePoints.size() < 4) {
379  throw(vpPoseException(vpPoseException::notInitializedError, "Not enough point to compute the pose"));
380  }
381 
382 #if defined(VISP_HAVE_THREADS)
383  unsigned int nbThreads = 1;
384  bool executeParallelVersion = useParallelRansac;
385 #else
386  bool executeParallelVersion = false;
387 #endif
388 
389  if (executeParallelVersion) {
390 #if defined(VISP_HAVE_THREADS)
391  if (nbParallelRansacThreads <= 0) {
392  // Get number of CPU threads
393  nbThreads = std::thread::hardware_concurrency();
394  if (nbThreads <= 1) {
395  nbThreads = 1;
396  executeParallelVersion = false;
397  }
398  }
399  else {
400  nbThreads = nbParallelRansacThreads;
401  }
402 #endif
403  }
404 
405  bool foundSolution = false;
406 
407  if (executeParallelVersion) {
408 #if defined(VISP_HAVE_THREADS)
409  std::vector<std::thread> threadpool;
410  std::vector<vpRansacFunctor> ransacWorkers;
411 
412  int splitTrials = ransacMaxTrials / nbThreads;
413  for (size_t i = 0; i < static_cast<size_t>(nbThreads); ++i) {
414  unsigned int initial_seed = static_cast<unsigned int>(i); //((unsigned int) time(nullptr) ^ i);
415  if (i < static_cast<size_t>(nbThreads) - 1) {
416  ransacWorkers.emplace_back(cMo, ransacNbInlierConsensus, splitTrials, ransacThreshold, initial_seed,
417  checkDegeneratePoints, listOfUniquePoints, func);
418  }
419  else {
420  int maxTrialsRemainder = ransacMaxTrials - splitTrials * (nbThreads - 1);
421  ransacWorkers.emplace_back(cMo, ransacNbInlierConsensus, maxTrialsRemainder, ransacThreshold, initial_seed,
422  checkDegeneratePoints, listOfUniquePoints, func);
423  }
424  }
425 
426  for (auto &worker : ransacWorkers) {
427  threadpool.emplace_back(&vpRansacFunctor::operator(), &worker);
428  }
429 
430  for (auto &th : threadpool) {
431  th.join();
432  }
433 
434  bool successRansac = false;
435  size_t best_consensus_size = 0;
436 
437  for (auto &worker : ransacWorkers) {
438  if (worker.getResult()) {
439  successRansac = true;
440 
441  if (worker.getBestConsensus().size() > best_consensus_size) {
442  nbInliers = worker.getNbInliers();
443  best_consensus = worker.getBestConsensus();
444  best_consensus_size = worker.getBestConsensus().size();
445  }
446  }
447  }
448 
449  foundSolution = successRansac;
450 #endif
451  }
452  else {
453  // Sequential RANSAC
454  vpRansacFunctor sequentialRansac(cMo, ransacNbInlierConsensus, ransacMaxTrials, ransacThreshold, 0,
455  checkDegeneratePoints, listOfUniquePoints, func);
456  sequentialRansac();
457  foundSolution = sequentialRansac.getResult();
458 
459  if (foundSolution) {
460  nbInliers = sequentialRansac.getNbInliers();
461  best_consensus = sequentialRansac.getBestConsensus();
462  }
463  }
464 
465  if (foundSolution) {
466  unsigned int nbMinRandom = 4;
467 
468  // --comment: print the nbInliers
469  // Display the random picked points (cur_randoms)
470  // Display the outliers (cur_outliers)
471 
472  // Even if the cardinality of the best consensus set is inferior to
473  // ransacNbInlierConsensus, we want to refine the solution with data in
474  // best_consensus and return this pose. This is an approach used for
475  // example in p118 in Multiple View Geometry in Computer Vision, Hartley,
476  // R.~I. and Zisserman, A.
477  if (nbInliers >= nbMinRandom) // if(nbInliers >= (unsigned)ransacNbInlierConsensus)
478  {
479  // Refine the solution using all the points in the consensus set and
480  // with VVS pose estimation
481  vpPose pose;
482  size_t best_consensus_size = best_consensus.size();
483  for (size_t i = 0; i < best_consensus_size; ++i) {
484  vpPoint pt = listOfUniquePoints[best_consensus[i]];
485 
486  pose.addPoint(pt);
487  ransacInliers.push_back(pt);
488  }
489 
490  // Update the list of inlier index
491  std::vector<unsigned int>::const_iterator best_consensus_end = best_consensus.end();
492  for (std::vector<unsigned int>::const_iterator it_index = best_consensus.begin();
493  it_index != best_consensus_end; ++it_index) {
494  ransacInlierIndex.push_back(static_cast<unsigned int>(mapOfUniquePointIndex[*it_index]));
495  }
496 
497  pose.setCovarianceComputation(computeCovariance);
499 
500  // In some rare cases, the final pose could not respect the pose
501  // criterion even if the 4 minimal points picked respect the pose
502  // criterion.
503  if ((func != nullptr) && (!func(cMo))) {
504  return false;
505  }
506 
507  if (computeCovariance) {
508  covarianceMatrix = pose.covarianceMatrix;
509  }
510  }
511  else {
512  return false;
513  }
514  }
515 
516  return foundSolution;
517 }
518 
519 int vpPose::computeRansacIterations(double probability, double epsilon, const int sampleSize, int maxIterations)
520 {
521  probability = std::max<double>(probability, 0.0);
522  probability = std::min<double>(probability, 1.0);
523  epsilon = std::max<double>(epsilon, 0.0);
524  epsilon = std::min<double>(epsilon, 1.0);
525 
526  if (vpMath::nul(epsilon)) {
527  // no outliers
528  return 1;
529  }
530 
531  if (maxIterations <= 0) {
532  maxIterations = std::numeric_limits<int>::max();
533  }
534 
535  double logarg, logval, N;
536  logarg = -std::pow(1.0 - epsilon, sampleSize);
537 #ifdef VISP_HAVE_FUNC_LOG1P
538  logval = log1p(logarg);
539 #else
540  logval = log(1.0 + logarg);
541 #endif
542  if (vpMath::nul(logval, std::numeric_limits<double>::epsilon())) {
543  std::cerr << "vpMath::nul(log(1.0 - std::pow(1.0 - epsilon, "
544  "sampleSize)), std::numeric_limits<double>::epsilon())"
545  << std::endl;
546  return 0;
547  }
548 
549  N = log(std::max<double>(1.0 - probability, std::numeric_limits<double>::epsilon())) / logval;
550  if ((logval < 0.0) && (N < maxIterations)) {
551  return static_cast<int>(ceil(N));
552  }
553 
554  return maxIterations;
555 }
556 
557 void vpPose::findMatch(std::vector<vpPoint> &p2D, std::vector<vpPoint> &p3D,
558  const unsigned int &numberOfInlierToReachAConsensus, const double &threshold,
559  unsigned int &ninliers, std::vector<vpPoint> &listInliers, vpHomogeneousMatrix &cMo,
560  const int &maxNbTrials, bool useParallelRansac, unsigned int nthreads,
561  bool (*func)(const vpHomogeneousMatrix &))
562 {
563  vpPose pose;
564 
565  unsigned int p2D_size = p2D.size();
566  unsigned int p3D_size = p3D.size();
567  for (unsigned int i = 0; i < p2D_size; ++i) {
568  for (unsigned int j = 0; j < p3D_size; ++j) {
569  vpPoint pt(p3D[j].getWorldCoordinates());
570  pt.set_x(p2D[i].get_x());
571  pt.set_y(p2D[i].get_y());
572  pose.addPoint(pt);
573  }
574  }
575 
576  if (pose.listP.size() < 4) {
577  vpERROR_TRACE("Ransac method cannot be used in that case ");
578  vpERROR_TRACE("(at least 4 points are required)");
579  vpERROR_TRACE("Not enough point (%d) to compute the pose ", pose.listP.size());
580  throw(vpPoseException(vpPoseException::notEnoughPointError, "Not enough point (%d) to compute the pose by ransac",
581  pose.listP.size()));
582  }
583  else {
584  pose.setUseParallelRansac(useParallelRansac);
585  pose.setNbParallelRansacThreads(nthreads);
586  // Since we add duplicate points, we need to check for degenerate configuration
588  pose.setRansacMaxTrials(maxNbTrials);
589  pose.setRansacNbInliersToReachConsensus(numberOfInlierToReachAConsensus);
590  pose.setRansacThreshold(threshold);
591  pose.computePose(vpPose::RANSAC, cMo, func);
592  ninliers = pose.getRansacNbInliers();
593  listInliers = pose.getRansacInliers();
594  }
595 }
596 
void track(const vpHomogeneousMatrix &cMo)
Implementation of an homogeneous matrix and operations on such kind of matrices.
static bool isNaN(double value)
Definition: vpMath.cpp:94
static double sqr(double x)
Definition: vpMath.h:203
static bool nul(double x, double threshold=0.001)
Definition: vpMath.h:447
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:79
double get_oX() const
Get the point oX coordinate in the object frame.
Definition: vpPoint.cpp:453
void set_x(double x)
Set the point x coordinate in the image plane.
Definition: vpPoint.cpp:503
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:464
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:457
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:462
double get_oY() const
Get the point oY coordinate in the object frame.
Definition: vpPoint.cpp:455
void setWorldCoordinates(double oX, double oY, double oZ)
Definition: vpPoint.cpp:111
void set_y(double y)
Set the point y coordinate in the image plane.
Definition: vpPoint.cpp:505
Error that can be emitted by the vpPose class and its derivatives.
@ notEnoughPointError
Not enough points to compute the pose.
@ notInitializedError
Something is not initialized.
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:80
void setRansacMaxTrials(const int &rM)
Definition: vpPose.h:406
static int computeRansacIterations(double probability, double epsilon, const int sampleSize=4, int maxIterations=2000)
void addPoint(const vpPoint &P)
Definition: vpPose.cpp:95
void setRansacNbInliersToReachConsensus(const unsigned int &nbC)
Definition: vpPose.h:387
bool poseRansac(vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=nullptr)
@ RANSAC
Definition: vpPose.h:90
@ DEMENTHON_LAGRANGE_VIRTUAL_VS
Definition: vpPose.h:101
void setCovarianceComputation(const bool &flag)
Definition: vpPose.h:429
static void findMatch(std::vector< vpPoint > &p2D, std::vector< vpPoint > &p3D, const unsigned int &numberOfInlierToReachAConsensus, const double &threshold, unsigned int &ninliers, std::vector< vpPoint > &listInliers, vpHomogeneousMatrix &cMo, const int &maxNbTrials=10000, bool useParallelRansac=true, unsigned int nthreads=0, bool(*func)(const vpHomogeneousMatrix &)=nullptr)
unsigned int npt
Number of point used in pose computation.
Definition: vpPose.h:116
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Definition: vpPose.h:117
double computeResidual(const vpHomogeneousMatrix &cMo) const
Compute and return the sum of squared residuals expressed in meter^2 for the pose matrix cMo.
Definition: vpPose.cpp:292
void setRansacFilterFlag(const RANSAC_FILTER_FLAGS &flag)
Definition: vpPose.h:460
@ CHECK_DEGENERATE_POINTS
Definition: vpPose.h:113
@ PREFILTER_DEGENERATE_POINTS
Definition: vpPose.h:112
unsigned int getRansacNbInliers() const
Definition: vpPose.h:411
void setUseParallelRansac(bool use)
Definition: vpPose.h:491
std::vector< vpPoint > getRansacInliers() const
Definition: vpPose.h:421
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=nullptr)
Definition: vpPose.cpp:343
void setNbParallelRansacThreads(int nb)
Definition: vpPose.h:477
void setRansacThreshold(const double &t)
Definition: vpPose.h:392
vpColVector p
Definition: vpTracker.h:69
#define vpERROR_TRACE
Definition: vpDebug.h:385