Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpPoseRansac.cpp
1/*
2 * ViSP, open source Visual Servoing Platform software.
3 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4 *
5 * This software is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or
8 * (at your option) any later version.
9 * See the file LICENSE.txt at the root directory of this source
10 * distribution for additional information about the GNU GPL.
11 *
12 * For using ViSP with software that can not be combined with the GNU
13 * GPL, please contact Inria about acquiring a ViSP Professional
14 * Edition License.
15 *
16 * See https://visp.inria.fr for more information.
17 *
18 * This software was developed at:
19 * Inria Rennes - Bretagne Atlantique
20 * Campus Universitaire de Beaulieu
21 * 35042 Rennes Cedex
22 * France
23 *
24 * If you have questions regarding the use of this file, please contact
25 * Inria at visp@inria.fr
26 *
27 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29 *
30 * Description:
31 * Pose computation.
32 */
33
39#include <algorithm> // std::count
40#include <cmath> // std::fabs
41#include <float.h> // DBL_MAX
42#include <iostream>
43#include <limits> // numeric_limits
44#include <map>
45
46#include <visp3/core/vpColVector.h>
47#include <visp3/core/vpMath.h>
48#include <visp3/core/vpRansac.h>
49#include <visp3/vision/vpPose.h>
50#include <visp3/vision/vpPoseException.h>
51
52#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
53#include <thread>
54#endif
55
56#define eps 1e-6
57
58namespace
59{
60// For std::map<vpPoint>
61struct CompareObjectPointDegenerate {
62 bool operator()(const vpPoint &point1, const vpPoint &point2) const
63 {
64 const double dist1 =
65 point1.get_oX() * point1.get_oX() + point1.get_oY() * point1.get_oY() + point1.get_oZ() * point1.get_oZ();
66 const double dist2 =
67 point2.get_oX() * point2.get_oX() + point2.get_oY() * point2.get_oY() + point2.get_oZ() * point2.get_oZ();
68 if (dist1 - dist2 < -3 * eps * eps)
69 return true;
70 if (dist1 - dist2 > 3 * eps * eps)
71 return false;
72
73 if (point1.oP[0] - point2.oP[0] < -eps)
74 return true;
75 if (point1.oP[0] - point2.oP[0] > eps)
76 return false;
77
78 if (point1.oP[1] - point2.oP[1] < -eps)
79 return true;
80 if (point1.oP[1] - point2.oP[1] > eps)
81 return false;
82
83 if (point1.oP[2] - point2.oP[2] < -eps)
84 return true;
85 if (point1.oP[2] - point2.oP[2] > eps)
86 return false;
87
88 return false;
89 }
90};
91
92// For std::map<vpPoint>
93struct CompareImagePointDegenerate {
94 bool operator()(const vpPoint &point1, const vpPoint &point2) const
95 {
96 const double dist1 = point1.get_x() * point1.get_x() + point1.get_y() * point1.get_y();
97 const double dist2 = point2.get_x() * point2.get_x() + point2.get_y() * point2.get_y();
98 if (dist1 - dist2 < -2 * eps * eps)
99 return true;
100 if (dist1 - dist2 > 2 * eps * eps)
101 return false;
102
103 if (point1.p[0] - point2.p[0] < -eps)
104 return true;
105 if (point1.p[0] - point2.p[0] > eps)
106 return false;
107
108 if (point1.p[1] - point2.p[1] < -eps)
109 return true;
110 if (point1.p[1] - point2.p[1] > eps)
111 return false;
112
113 return false;
114 }
115};
116
117// std::find_if
118struct FindDegeneratePoint {
119 explicit FindDegeneratePoint(const vpPoint &pt) : m_pt(pt) {}
120
121 bool operator()(const vpPoint &pt)
122 {
123 return ((std::fabs(m_pt.oP[0] - pt.oP[0]) < eps && std::fabs(m_pt.oP[1] - pt.oP[1]) < eps &&
124 std::fabs(m_pt.oP[2] - pt.oP[2]) < eps) ||
125 (std::fabs(m_pt.p[0] - pt.p[0]) < eps && std::fabs(m_pt.p[1] - pt.p[1]) < eps));
126 }
127
128 vpPoint m_pt;
129};
130} // namespace
131
132bool vpPose::RansacFunctor::poseRansacImpl()
133{
134 const unsigned int size = (unsigned int)m_listOfUniquePoints.size();
135 unsigned int nbMinRandom = 4;
136 int nbTrials = 0;
137
138 vpPoint p; // Point used to project using the estimated pose
139
140 bool foundSolution = false;
141 while (nbTrials < m_ransacMaxTrials && m_nbInliers < m_ransacNbInlierConsensus) {
142 // Hold the list of the index of the inliers (points in the consensus set)
143 std::vector<unsigned int> cur_consensus;
144 // Hold the list of the index of the outliers
145 std::vector<unsigned int> cur_outliers;
146 // Hold the list of the index of the points randomly picked
147 std::vector<unsigned int> cur_randoms;
148 // Hold the list of the current inliers points to avoid to add a
149 // degenerate point if the flag is set
150 std::vector<vpPoint> cur_inliers;
151
152 // Use a temporary variable because if not, the cMo passed in parameters
153 // will be modified when
154 // we compute the pose for the minimal sample sets but if the pose is not
155 // correct when we pass a function pointer we do not want to modify the
156 // cMo passed in parameters
157 vpHomogeneousMatrix cMo_tmp;
158
159 // Vector of used points, initialized at false for all points
160 std::vector<bool> usedPt(size, false);
161
162 vpPose poseMin;
163 for (unsigned int i = 0; i < nbMinRandom;) {
164 if ((size_t)std::count(usedPt.begin(), usedPt.end(), true) == usedPt.size()) {
165 // All points was picked once, break otherwise we stay in an infinite loop
166 break;
167 }
168
169 // Pick a point randomly
170 unsigned int r_ = m_uniRand.uniform(0, size);
171
172 while (usedPt[r_]) {
173 // If already picked, pick another point randomly
174 r_ = m_uniRand.uniform(0, size);
175 }
176 // Mark this point as already picked
177 usedPt[r_] = true;
178 vpPoint pt = m_listOfUniquePoints[r_];
179
180 bool degenerate = false;
181 if (m_checkDegeneratePoints) {
182 if (std::find_if(poseMin.listOfPoints.begin(), poseMin.listOfPoints.end(), FindDegeneratePoint(pt)) !=
183 poseMin.listOfPoints.end()) {
184 degenerate = true;
185 }
186 }
187
188 if (!degenerate) {
189 poseMin.addPoint(pt);
190 cur_randoms.push_back(r_);
191 // Increment the number of points picked
192 i++;
193 }
194 }
195
196 if (poseMin.npt < nbMinRandom) {
197 nbTrials++;
198 continue;
199 }
200
201 bool is_pose_valid = false;
202 double r_min = DBL_MAX;
203
204 try {
205 is_pose_valid = poseMin.computePose(vpPose::DEMENTHON_LAGRANGE_VIRTUAL_VS, cMo_tmp);
206 r_min = poseMin.computeResidual(cMo_tmp);
207 } catch (...) {
208 }
209
210 // If residual returned is not a number (NAN), set valid to false
211 if (vpMath::isNaN(r_min)) {
212 is_pose_valid = false;
213 }
214
215 // If at pose computation is OK we can continue, otherwise pick another random set
216 if (is_pose_valid) {
217 double r = sqrt(r_min) / (double)nbMinRandom; // FS should be r = sqrt(r_min / (double)nbMinRandom);
218 // Filter the pose using some criterion (orientation angles,
219 // translations, etc.)
220 bool isPoseValid = true;
221 if (m_func != NULL) {
222 isPoseValid = m_func(cMo_tmp);
223 if (isPoseValid) {
224 m_cMo = cMo_tmp;
225 }
226 } else {
227 // No post filtering on pose, so copy cMo_temp to cMo
228 m_cMo = cMo_tmp;
229 }
230
231 if (isPoseValid && r < m_ransacThreshold) {
232 unsigned int nbInliersCur = 0;
233 unsigned int iter = 0;
234 for (std::vector<vpPoint>::const_iterator it = m_listOfUniquePoints.begin(); it != m_listOfUniquePoints.end();
235 ++it, iter++) {
236 p.setWorldCoordinates(it->get_oX(), it->get_oY(), it->get_oZ());
237 p.track(m_cMo);
238
239 double error = sqrt(vpMath::sqr(p.get_x() - it->get_x()) + vpMath::sqr(p.get_y() - it->get_y()));
240 if (error < m_ransacThreshold) {
241 bool degenerate = false;
242 if (m_checkDegeneratePoints) {
243 if (std::find_if(cur_inliers.begin(), cur_inliers.end(), FindDegeneratePoint(*it)) != cur_inliers.end()) {
244 degenerate = true;
245 }
246 }
247
248 if (!degenerate) {
249 // the point is considered as inlier if the error is below the
250 // threshold
251 nbInliersCur++;
252 cur_consensus.push_back(iter);
253 cur_inliers.push_back(*it);
254 } else {
255 cur_outliers.push_back(iter);
256 }
257 } else {
258 cur_outliers.push_back(iter);
259 }
260 }
261
262 if (nbInliersCur > m_nbInliers) {
263 foundSolution = true;
264 m_best_consensus = cur_consensus;
265 m_nbInliers = nbInliersCur;
266 }
267
268 nbTrials++;
269
270 if (nbTrials >= m_ransacMaxTrials) {
271 foundSolution = true;
272 }
273 } else {
274 nbTrials++;
275 }
276 } else {
277 nbTrials++;
278 }
279 }
280
281 return foundSolution;
282}
283
298{
299 // Check only for adding / removing problem
300 // Do not take into account problem with element modification here
301 if (listP.size() != listOfPoints.size()) {
302 std::cerr << "You should not modify vpPose::listP!" << std::endl;
303 listOfPoints = std::vector<vpPoint>(listP.begin(), listP.end());
304 }
305
306 ransacInliers.clear();
307 ransacInlierIndex.clear();
308
309 std::vector<unsigned int> best_consensus;
310 unsigned int nbInliers = 0;
311
312 vpHomogeneousMatrix cMo_lagrange, cMo_dementhon;
313
314 if (listOfPoints.size() < 4) {
315 throw(vpPoseException(vpPoseException::notInitializedError, "Not enough point to compute the pose"));
316 }
317
318 std::vector<vpPoint> listOfUniquePoints;
319 std::map<size_t, size_t> mapOfUniquePointIndex;
320
321 // Get RANSAC flags
322 bool prefilterDegeneratePoints = ransacFlag == PREFILTER_DEGENERATE_POINTS;
323 bool checkDegeneratePoints = ransacFlag == CHECK_DEGENERATE_POINTS;
324
325 if (prefilterDegeneratePoints) {
326 // Remove degenerate object points
327 std::map<vpPoint, size_t, CompareObjectPointDegenerate> filterObjectPointMap;
328 size_t index_pt = 0;
329 for (std::vector<vpPoint>::const_iterator it_pt = listOfPoints.begin(); it_pt != listOfPoints.end();
330 ++it_pt, index_pt++) {
331 if (filterObjectPointMap.find(*it_pt) == filterObjectPointMap.end()) {
332 filterObjectPointMap[*it_pt] = index_pt;
333 }
334 }
335
336 std::map<vpPoint, size_t, CompareImagePointDegenerate> filterImagePointMap;
337 for (std::map<vpPoint, size_t, CompareObjectPointDegenerate>::const_iterator it = filterObjectPointMap.begin();
338 it != filterObjectPointMap.end(); ++it) {
339 if (filterImagePointMap.find(it->first) == filterImagePointMap.end()) {
340 filterImagePointMap[it->first] = it->second;
341
342 listOfUniquePoints.push_back(it->first);
343 mapOfUniquePointIndex[listOfUniquePoints.size() - 1] = it->second;
344 }
345 }
346 } else {
347 // No prefiltering
348 listOfUniquePoints = listOfPoints;
349
350 size_t index_pt = 0;
351 for (std::vector<vpPoint>::const_iterator it_pt = listOfPoints.begin(); it_pt != listOfPoints.end();
352 ++it_pt, index_pt++) {
353 mapOfUniquePointIndex[index_pt] = index_pt;
354 }
355 }
356
357 if (listOfUniquePoints.size() < 4) {
358 throw(vpPoseException(vpPoseException::notInitializedError, "Not enough point to compute the pose"));
359 }
360
361#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
362 unsigned int nbThreads = 1;
363 bool executeParallelVersion = useParallelRansac;
364#else
365 bool executeParallelVersion = false;
366#endif
367
368 if (executeParallelVersion) {
369#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
370 if (nbParallelRansacThreads <= 0) {
371 // Get number of CPU threads
372 nbThreads = std::thread::hardware_concurrency();
373 if (nbThreads <= 1) {
374 nbThreads = 1;
375 executeParallelVersion = false;
376 }
377 } else {
378 nbThreads = nbParallelRansacThreads;
379 }
380#endif
381 }
382
383 bool foundSolution = false;
384
385 if (executeParallelVersion) {
386#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
387 std::vector<std::thread> threadpool;
388 std::vector<RansacFunctor> ransacWorkers;
389
390 int splitTrials = ransacMaxTrials / nbThreads;
391 for (size_t i = 0; i < (size_t)nbThreads; i++) {
392 unsigned int initial_seed = (unsigned int)i; //((unsigned int) time(NULL) ^ i);
393 if (i < (size_t)nbThreads - 1) {
394 ransacWorkers.emplace_back(cMo, ransacNbInlierConsensus, splitTrials, ransacThreshold, initial_seed,
395 checkDegeneratePoints, listOfUniquePoints, func);
396 } else {
397 int maxTrialsRemainder = ransacMaxTrials - splitTrials * (nbThreads - 1);
398 ransacWorkers.emplace_back(cMo, ransacNbInlierConsensus, maxTrialsRemainder, ransacThreshold, initial_seed,
399 checkDegeneratePoints, listOfUniquePoints, func);
400 }
401 }
402
403 for (auto &worker : ransacWorkers) {
404 threadpool.emplace_back(&RansacFunctor::operator(), &worker);
405 }
406
407 for (auto &th : threadpool) {
408 th.join();
409 }
410
411 bool successRansac = false;
412 size_t best_consensus_size = 0;
413 for (auto &worker : ransacWorkers) {
414 if (worker.getResult()) {
415 successRansac = true;
416
417 if (worker.getBestConsensus().size() > best_consensus_size) {
418 nbInliers = worker.getNbInliers();
419 best_consensus = worker.getBestConsensus();
420 best_consensus_size = worker.getBestConsensus().size();
421 }
422 }
423 }
424
425 foundSolution = successRansac;
426#endif
427 } else {
428 // Sequential RANSAC
429 RansacFunctor sequentialRansac(cMo, ransacNbInlierConsensus, ransacMaxTrials, ransacThreshold, 0,
430 checkDegeneratePoints, listOfUniquePoints, func);
431 sequentialRansac();
432 foundSolution = sequentialRansac.getResult();
433
434 if (foundSolution) {
435 nbInliers = sequentialRansac.getNbInliers();
436 best_consensus = sequentialRansac.getBestConsensus();
437 }
438 }
439
440 if (foundSolution) {
441 unsigned int nbMinRandom = 4;
442 // std::cout << "Nombre d'inliers " << nbInliers << std::endl ;
443
444 // Display the random picked points
445 /*
446 std::cout << "Randoms : ";
447 for(unsigned int i = 0 ; i < cur_randoms.size() ; i++)
448 std::cout << cur_randoms[i] << " ";
449 std::cout << std::endl;
450 */
451
452 // Display the outliers
453 /*
454 std::cout << "Outliers : ";
455 for(unsigned int i = 0 ; i < cur_outliers.size() ; i++)
456 std::cout << cur_outliers[i] << " ";
457 std::cout << std::endl;
458 */
459
460 // Even if the cardinality of the best consensus set is inferior to
461 // ransacNbInlierConsensus, we want to refine the solution with data in
462 // best_consensus and return this pose. This is an approach used for
463 // example in p118 in Multiple View Geometry in Computer Vision, Hartley,
464 // R.~I. and Zisserman, A.
465 if (nbInliers >= nbMinRandom) // if(nbInliers >= (unsigned)ransacNbInlierConsensus)
466 {
467 // Refine the solution using all the points in the consensus set and
468 // with VVS pose estimation
469 vpPose pose;
470 for (size_t i = 0; i < best_consensus.size(); i++) {
471 vpPoint pt = listOfUniquePoints[best_consensus[i]];
472
473 pose.addPoint(pt);
474 ransacInliers.push_back(pt);
475 }
476
477 // Update the list of inlier index
478 for (std::vector<unsigned int>::const_iterator it_index = best_consensus.begin();
479 it_index != best_consensus.end(); ++it_index) {
480 ransacInlierIndex.push_back((unsigned int)mapOfUniquePointIndex[*it_index]);
481 }
482
483 pose.setCovarianceComputation(computeCovariance);
485
486 // In some rare cases, the final pose could not respect the pose
487 // criterion even if the 4 minimal points picked respect the pose
488 // criterion.
489 if (func != NULL && !func(cMo)) {
490 return false;
491 }
492
493 if (computeCovariance) {
494 covarianceMatrix = pose.covarianceMatrix;
495 }
496 } else {
497 return false;
498 }
499 }
500
501 return foundSolution;
502}
503
523int vpPose::computeRansacIterations(double probability, double epsilon, const int sampleSize, int maxIterations)
524{
525 probability = (std::max)(probability, 0.0);
526 probability = (std::min)(probability, 1.0);
527 epsilon = (std::max)(epsilon, 0.0);
528 epsilon = (std::min)(epsilon, 1.0);
529
530 if (vpMath::nul(epsilon)) {
531 // no outliers
532 return 1;
533 }
534
535 if (maxIterations <= 0) {
536 maxIterations = std::numeric_limits<int>::max();
537 }
538
539 double logarg, logval, N;
540 logarg = -std::pow(1.0 - epsilon, sampleSize);
541#ifdef VISP_HAVE_FUNC_LOG1P
542 logval = log1p(logarg);
543#else
544 logval = log(1.0 + logarg);
545#endif
546 if (vpMath::nul(logval, std::numeric_limits<double>::epsilon())) {
547 std::cerr << "vpMath::nul(log(1.0 - std::pow(1.0 - epsilon, "
548 "sampleSize)), std::numeric_limits<double>::epsilon())"
549 << std::endl;
550 return 0;
551 }
552
553 N = log((std::max)(1.0 - probability, std::numeric_limits<double>::epsilon())) / logval;
554 if (logval < 0.0 && N < maxIterations) {
555 return (int)ceil(N);
556 }
557
558 return maxIterations;
559}
560
591void vpPose::findMatch(std::vector<vpPoint> &p2D, std::vector<vpPoint> &p3D,
592 const unsigned int &numberOfInlierToReachAConsensus, const double &threshold,
593 unsigned int &ninliers, std::vector<vpPoint> &listInliers, vpHomogeneousMatrix &cMo,
594 const int &maxNbTrials, bool useParallelRansac, unsigned int nthreads,
595 bool (*func)(const vpHomogeneousMatrix &))
596{
597 vpPose pose;
598
599 for (unsigned int i = 0; i < p2D.size(); i++) {
600 for (unsigned int j = 0; j < p3D.size(); j++) {
601 vpPoint pt(p3D[j].getWorldCoordinates());
602 pt.set_x(p2D[i].get_x());
603 pt.set_y(p2D[i].get_y());
604 pose.addPoint(pt);
605 }
606 }
607
608 if (pose.listP.size() < 4) {
609 vpERROR_TRACE("Ransac method cannot be used in that case ");
610 vpERROR_TRACE("(at least 4 points are required)");
611 vpERROR_TRACE("Not enough point (%d) to compute the pose ", pose.listP.size());
612 throw(vpPoseException(vpPoseException::notEnoughPointError, "Not enough point (%d) to compute the pose by ransac",
613 pose.listP.size()));
614 } else {
615 pose.setUseParallelRansac(useParallelRansac);
616 pose.setNbParallelRansacThreads(nthreads);
617 // Since we add duplicate points, we need to check for degenerate configuration
619 pose.setRansacMaxTrials(maxNbTrials);
620 pose.setRansacNbInliersToReachConsensus(numberOfInlierToReachAConsensus);
621 pose.setRansacThreshold(threshold);
622 pose.computePose(vpPose::RANSAC, cMo, func);
623 ninliers = pose.getRansacNbInliers();
624 listInliers = pose.getRansacInliers();
625 }
626}
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:93
static double sqr(double x)
Definition vpMath.h:124
static bool nul(double x, double threshold=0.001)
Definition vpMath.h:360
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition vpPoint.h:77
double get_oX() const
Get the point oX coordinate in the object frame.
Definition vpPoint.cpp:458
void set_x(double x)
Set the point x coordinate in the image plane.
Definition vpPoint.cpp:508
double get_y() const
Get the point y coordinate in the image plane.
Definition vpPoint.cpp:469
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition vpPoint.cpp:462
double get_x() const
Get the point x coordinate in the image plane.
Definition vpPoint.cpp:467
double get_oY() const
Get the point oY coordinate in the object frame.
Definition vpPoint.cpp:460
void setWorldCoordinates(double oX, double oY, double oZ)
Definition vpPoint.cpp:110
void set_y(double y)
Set the point y coordinate in the image plane.
Definition vpPoint.cpp:510
Error that can be emitted by the vpPose class and its derivatives.
@ notEnoughPointError
Not enough points to compute the pose.
@ notInitializedError
Something is not initialized.
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition vpPose.h:81
void setRansacMaxTrials(const int &rM)
Definition vpPose.h:256
static int computeRansacIterations(double probability, double epsilon, const int sampleSize=4, int maxIterations=2000)
void addPoint(const vpPoint &P)
Definition vpPose.cpp:140
void setRansacNbInliersToReachConsensus(const unsigned int &nbC)
Definition vpPose.h:245
@ RANSAC
Definition vpPose.h:91
@ DEMENTHON_LAGRANGE_VIRTUAL_VS
Definition vpPose.h:102
void setCovarianceComputation(const bool &flag)
Definition vpPose.h:267
unsigned int npt
Number of point used in pose computation.
Definition vpPose.h:114
std::vector< vpPoint > getRansacInliers() const
Definition vpPose.h:259
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Definition vpPose.h:115
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:369
void setRansacFilterFlag(const RANSAC_FILTER_FLAGS &flag)
Definition vpPose.h:298
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition vpPose.cpp:469
@ CHECK_DEGENERATE_POINTS
Definition vpPose.h:111
@ PREFILTER_DEGENERATE_POINTS
Definition vpPose.h:110
unsigned int getRansacNbInliers() const
Definition vpPose.h:257
void setUseParallelRansac(bool use)
Definition vpPose.h:329
bool poseRansac(vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
void setNbParallelRansacThreads(int nb)
Definition vpPose.h:315
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:246
vpColVector p
Definition vpTracker.h:68
#define vpERROR_TRACE
Definition vpDebug.h:388