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