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