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>
59 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
68 struct CompareObjectPointDegenerate {
69 bool operator()(
const vpPoint &point1,
const vpPoint &point2)
const
75 if (dist1 - dist2 < -3 * eps * eps)
77 if (dist1 - dist2 > 3 * eps * eps)
80 if (point1.
oP[0] - point2.
oP[0] < -eps)
82 if (point1.
oP[0] - point2.
oP[0] > eps)
85 if (point1.
oP[1] - point2.
oP[1] < -eps)
87 if (point1.
oP[1] - point2.
oP[1] > eps)
90 if (point1.
oP[2] - point2.
oP[2] < -eps)
92 if (point1.
oP[2] - point2.
oP[2] > eps)
100 struct CompareImagePointDegenerate {
101 bool operator()(
const vpPoint &point1,
const vpPoint &point2)
const
105 if (dist1 - dist2 < -2 * eps * eps)
107 if (dist1 - dist2 > 2 * eps * eps)
110 if (point1.
p[0] - point2.
p[0] < -eps)
112 if (point1.
p[0] - point2.
p[0] > eps)
115 if (point1.
p[1] - point2.
p[1] < -eps)
117 if (point1.
p[1] - point2.
p[1] > eps)
125 struct FindDegeneratePoint {
126 explicit FindDegeneratePoint(
const vpPoint &pt) : m_pt(pt) {}
128 bool operator()(
const vpPoint &pt)
130 return ((std::fabs(m_pt.oP[0] - pt.
oP[0]) < eps && std::fabs(m_pt.oP[1] - pt.
oP[1]) < eps &&
131 std::fabs(m_pt.oP[2] - pt.
oP[2]) < eps) ||
132 (std::fabs(m_pt.p[0] - pt.
p[0]) < eps && std::fabs(m_pt.p[1] - pt.
p[1]) < eps));
139 bool vpPose::RansacFunctor::poseRansacImpl()
141 const unsigned int size = (
unsigned int)m_listOfUniquePoints.size();
142 unsigned int nbMinRandom = 4;
147 bool foundSolution =
false;
148 while (nbTrials < m_ransacMaxTrials && m_nbInliers < m_ransacNbInlierConsensus) {
150 std::vector<unsigned int> cur_consensus;
152 std::vector<unsigned int> cur_outliers;
154 std::vector<unsigned int> cur_randoms;
157 std::vector<vpPoint> cur_inliers;
168 std::vector<bool> usedPt(size,
false);
171 for (
unsigned int i = 0; i < nbMinRandom;) {
172 if ((
size_t)std::count(usedPt.begin(), usedPt.end(),
true) == usedPt.size()) {
178 unsigned int r_ = m_uniRand.uniform(0, size);
182 r_ = m_uniRand.uniform(0, size);
186 vpPoint pt = m_listOfUniquePoints[r_];
188 bool degenerate =
false;
189 if (m_checkDegeneratePoints) {
190 if (std::find_if(poseMin.listOfPoints.begin(), poseMin.listOfPoints.end(), FindDegeneratePoint(pt)) !=
191 poseMin.listOfPoints.end()) {
198 cur_randoms.push_back(r_);
204 if (poseMin.
npt < nbMinRandom) {
210 bool is_valid_lagrange =
false;
211 bool is_valid_dementhon =
false;
214 double r_lagrange = DBL_MAX;
215 double r_dementhon = DBL_MAX;
220 is_valid_lagrange =
true;
227 is_valid_dementhon =
true;
233 is_valid_lagrange =
false;
234 r_lagrange = DBL_MAX;
238 is_valid_dementhon =
false;
239 r_dementhon = DBL_MAX;
244 if (is_valid_lagrange || is_valid_dementhon) {
246 if (r_lagrange < r_dementhon) {
248 cMo_tmp = cMo_lagrange;
251 cMo_tmp = cMo_dementhon;
253 r = sqrt(r) / (double)nbMinRandom;
256 bool isPoseValid =
true;
257 if (m_func != NULL) {
258 isPoseValid = m_func(cMo_tmp);
267 if (isPoseValid && r < m_ransacThreshold) {
268 unsigned int nbInliersCur = 0;
269 unsigned int iter = 0;
270 for (std::vector<vpPoint>::const_iterator it = m_listOfUniquePoints.begin(); it != m_listOfUniquePoints.end();
276 if (error < m_ransacThreshold) {
277 bool degenerate =
false;
278 if (m_checkDegeneratePoints) {
279 if (std::find_if(cur_inliers.begin(), cur_inliers.end(), FindDegeneratePoint(*it)) != cur_inliers.end()) {
288 cur_consensus.push_back(iter);
289 cur_inliers.push_back(*it);
291 cur_outliers.push_back(iter);
294 cur_outliers.push_back(iter);
298 if (nbInliersCur > m_nbInliers) {
299 foundSolution =
true;
300 m_best_consensus = cur_consensus;
301 m_nbInliers = nbInliersCur;
306 if (nbTrials >= m_ransacMaxTrials) {
307 foundSolution =
true;
317 return foundSolution;
337 if (
listP.size() != listOfPoints.size()) {
338 std::cerr <<
"You should not modify vpPose::listP!" << std::endl;
339 listOfPoints = std::vector<vpPoint>(
listP.begin(),
listP.end());
342 ransacInliers.clear();
343 ransacInlierIndex.clear();
345 std::vector<unsigned int> best_consensus;
346 unsigned int nbInliers = 0;
350 if (listOfPoints.size() < 4) {
354 std::vector<vpPoint> listOfUniquePoints;
355 std::map<size_t, size_t> mapOfUniquePointIndex;
361 if (prefilterDegeneratePoints) {
363 std::map<vpPoint, size_t, CompareObjectPointDegenerate> filterObjectPointMap;
365 for (std::vector<vpPoint>::const_iterator it_pt = listOfPoints.begin(); it_pt != listOfPoints.end();
366 ++it_pt, index_pt++) {
367 if (filterObjectPointMap.find(*it_pt) == filterObjectPointMap.end()) {
368 filterObjectPointMap[*it_pt] = index_pt;
372 std::map<vpPoint, size_t, CompareImagePointDegenerate> filterImagePointMap;
373 for (std::map<vpPoint, size_t, CompareObjectPointDegenerate>::const_iterator it = filterObjectPointMap.begin();
374 it != filterObjectPointMap.end(); ++it) {
375 if (filterImagePointMap.find(it->first) == filterImagePointMap.end()) {
376 filterImagePointMap[it->first] = it->second;
378 listOfUniquePoints.push_back(it->first);
379 mapOfUniquePointIndex[listOfUniquePoints.size() - 1] = it->second;
384 listOfUniquePoints = listOfPoints;
387 for (std::vector<vpPoint>::const_iterator it_pt = listOfPoints.begin(); it_pt != listOfPoints.end();
388 ++it_pt, index_pt++) {
389 mapOfUniquePointIndex[index_pt] = index_pt;
393 if (listOfUniquePoints.size() < 4) {
397 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
398 unsigned int nbThreads = 1;
399 bool executeParallelVersion = useParallelRansac;
401 bool executeParallelVersion =
false;
404 if (executeParallelVersion) {
405 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
406 if (nbParallelRansacThreads <= 0) {
408 nbThreads = std::thread::hardware_concurrency();
409 if (nbThreads <= 1) {
411 executeParallelVersion =
false;
414 nbThreads = nbParallelRansacThreads;
419 bool foundSolution =
false;
421 if (executeParallelVersion) {
422 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
423 std::vector<std::thread> threadpool;
424 std::vector<RansacFunctor> ransacWorkers;
426 int splitTrials = ransacMaxTrials / nbThreads;
427 for (
size_t i = 0; i < (size_t)nbThreads; i++) {
428 unsigned int initial_seed = (
unsigned int)i;
429 if (i < (
size_t)nbThreads - 1) {
430 ransacWorkers.emplace_back(cMo, ransacNbInlierConsensus, splitTrials, ransacThreshold, initial_seed,
431 checkDegeneratePoints, listOfUniquePoints, func);
433 int maxTrialsRemainder = ransacMaxTrials - splitTrials * (nbThreads - 1);
434 ransacWorkers.emplace_back(cMo, ransacNbInlierConsensus, maxTrialsRemainder, ransacThreshold, initial_seed,
435 checkDegeneratePoints, listOfUniquePoints, func);
439 for (
auto &worker : ransacWorkers) {
440 threadpool.emplace_back(&RansacFunctor::operator(), &worker);
443 for (
auto &th : threadpool) {
447 bool successRansac =
false;
448 size_t best_consensus_size = 0;
449 for (
auto &worker : ransacWorkers) {
450 if (worker.getResult()) {
451 successRansac =
true;
453 if (worker.getBestConsensus().size() > best_consensus_size) {
454 nbInliers = worker.getNbInliers();
455 best_consensus = worker.getBestConsensus();
456 best_consensus_size = worker.getBestConsensus().size();
461 foundSolution = successRansac;
465 RansacFunctor sequentialRansac(cMo, ransacNbInlierConsensus, ransacMaxTrials, ransacThreshold, 0,
466 checkDegeneratePoints, listOfUniquePoints, func);
468 foundSolution = sequentialRansac.getResult();
471 nbInliers = sequentialRansac.getNbInliers();
472 best_consensus = sequentialRansac.getBestConsensus();
477 unsigned int nbMinRandom = 4;
501 if (nbInliers >= nbMinRandom)
506 for (
size_t i = 0; i < best_consensus.size(); i++) {
507 vpPoint pt = listOfUniquePoints[best_consensus[i]];
510 ransacInliers.push_back(pt);
514 for (std::vector<unsigned int>::const_iterator it_index = best_consensus.begin();
515 it_index != best_consensus.end(); ++it_index) {
516 ransacInlierIndex.push_back((
unsigned int)mapOfUniquePointIndex[*it_index]);
520 bool is_valid_lagrange =
false;
521 bool is_valid_dementhon =
false;
524 double r_lagrange = DBL_MAX;
525 double r_dementhon = DBL_MAX;
530 is_valid_lagrange =
true;
537 is_valid_dementhon =
true;
543 is_valid_lagrange =
false;
544 r_lagrange = DBL_MAX;
548 is_valid_dementhon =
false;
549 r_dementhon = DBL_MAX;
552 if (is_valid_lagrange || is_valid_dementhon) {
553 if (r_lagrange < r_dementhon) {
565 if (func != NULL && !func(cMo)) {
569 if (computeCovariance) {
570 covarianceMatrix = pose.covarianceMatrix;
578 return foundSolution;
602 probability = (std::max)(probability, 0.0);
603 probability = (std::min)(probability, 1.0);
604 epsilon = (std::max)(epsilon, 0.0);
605 epsilon = (std::min)(epsilon, 1.0);
612 if (maxIterations <= 0) {
613 maxIterations = std::numeric_limits<int>::max();
616 double logarg, logval, N;
617 logarg = -std::pow(1.0 - epsilon, sampleSize);
618 #ifdef VISP_HAVE_FUNC_LOG1P
619 logval = log1p(logarg);
621 logval = log(1.0 + logarg);
623 if (
vpMath::nul(logval, std::numeric_limits<double>::epsilon())) {
624 std::cerr <<
"vpMath::nul(log(1.0 - std::pow(1.0 - epsilon, "
625 "sampleSize)), std::numeric_limits<double>::epsilon())"
630 N = log((std::max)(1.0 - probability, std::numeric_limits<double>::epsilon())) / logval;
631 if (logval < 0.0 && N < maxIterations) {
635 return maxIterations;
669 const unsigned int &numberOfInlierToReachAConsensus,
const double &threshold,
671 const int &maxNbTrials,
bool useParallelRansac,
unsigned int nthreads,
677 for (
unsigned int i = 0; i < p2D.size(); i++) {
678 for (
unsigned int j = 0; j < p3D.size(); j++) {
679 vpPoint pt(p3D[j].getWorldCoordinates());
680 pt.
set_x(p2D[i].get_x());
681 pt.
set_y(p2D[i].get_y());
687 if (pose.
listP.size() < 4) {
void track(const vpHomogeneousMatrix &cMo)
Implementation of an homogeneous matrix and operations on such kind of matrices.
static bool isNaN(double value)
static double sqr(double x)
static bool nul(double x, double s=0.001)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
double get_oX() const
Get the point oX coordinate in the object frame.
void set_x(double x)
Set the point x coordinate in the image plane.
double get_y() const
Get the point y coordinate in the image plane.
double get_oZ() const
Get the point oZ coordinate in the object frame.
double get_x() const
Get the point x coordinate in the image plane.
double get_oY() const
Get the point oY coordinate in the object frame.
void setWorldCoordinates(double oX, double oY, double oZ)
void set_y(double y)
Set the point y coordinate in the image plane.
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...
void setRansacMaxTrials(const int &rM)
static int computeRansacIterations(double probability, double epsilon, const int sampleSize=4, int maxIterations=2000)
void addPoint(const vpPoint &P)
void setRansacNbInliersToReachConsensus(const unsigned int &nbC)
void setCovarianceComputation(const bool &flag)
unsigned int npt
Number of point used in pose computation.
std::list< vpPoint > listP
Array of point (use here class vpPoint)
double computeResidual(const vpHomogeneousMatrix &cMo) const
Compute and return the sum of squared residuals expressed in meter^2 for the pose matrix cMo.
void setRansacFilterFlag(const RANSAC_FILTER_FLAGS &flag)
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
@ CHECK_DEGENERATE_POINTS
@ PREFILTER_DEGENERATE_POINTS
unsigned int getRansacNbInliers() const
void setUseParallelRansac(bool use)
std::vector< vpPoint > getRansacInliers() const
bool poseRansac(vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
void setNbParallelRansacThreads(int nb)
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)