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