Visual Servoing Platform version 3.5.0
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
65namespace
66{
67// For std::map<vpPoint>
68struct 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>
98struct 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
123struct 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
137bool 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);
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
594int 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
662void 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}
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:85
static double sqr(double x)
Definition: vpMath.h:116
static bool nul(double x, double s=0.001)
Definition: vpMath.h:286
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:81
void setRansacMaxTrials(const int &rM)
Definition: vpPose.h:245
static int computeRansacIterations(double probability, double epsilon, const int sampleSize=4, int maxIterations=2000)
void addPoint(const vpPoint &P)
Definition: vpPose.cpp:149
void setRansacNbInliersToReachConsensus(const unsigned int &nbC)
Definition: vpPose.h:235
@ DEMENTHON
Definition: vpPose.h:86
@ RANSAC
Definition: vpPose.h:90
@ VIRTUAL_VS
Definition: vpPose.h:95
@ LAGRANGE
Definition: vpPose.h:85
void setCovarianceComputation(const bool &flag)
Definition: vpPose.h:256
unsigned int npt
Number of point used in pose computation.
Definition: vpPose.h:109
std::vector< vpPoint > getRansacInliers() const
Definition: vpPose.h:248
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Definition: vpPose.h:110
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 setRansacFilterFlag(const RANSAC_FILTER_FLAGS &flag)
Definition: vpPose.h:287
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition: vpPose.cpp:374
@ CHECK_DEGENERATE_POINTS
Definition: vpPose.h:106
@ PREFILTER_DEGENERATE_POINTS
Definition: vpPose.h:105
unsigned int getRansacNbInliers() const
Definition: vpPose.h:246
void setUseParallelRansac(bool use)
Definition: vpPose.h:318
bool poseRansac(vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
void setNbParallelRansacThreads(int nb)
Definition: vpPose.h:304
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:236
vpColVector p
Definition: vpTracker.h:73
#define vpERROR_TRACE
Definition: vpDebug.h:393