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 defined(VISP_HAVE_CPP11_COMPATIBILITY) 68 struct CompareObjectPointDegenerate {
69 bool operator()(
const vpPoint &point1,
const vpPoint &point2)
const 71 if (point1.
oP[0] - point2.
oP[0] < -eps)
73 if (point1.
oP[0] - point2.
oP[0] > eps)
76 if (point1.
oP[1] - point2.
oP[1] < -eps)
78 if (point1.
oP[1] - point2.
oP[1] > eps)
81 if (point1.
oP[2] - point2.
oP[2] < -eps)
83 if (point1.
oP[2] - point2.
oP[2] > eps)
91 struct CompareImagePointDegenerate {
92 bool operator()(
const vpPoint &point1,
const vpPoint &point2)
const 94 if (point1.
p[0] - point2.
p[0] < -eps)
96 if (point1.
p[0] - point2.
p[0] > eps)
99 if (point1.
p[1] - point2.
p[1] < -eps)
101 if (point1.
p[1] - point2.
p[1] > eps)
109 struct FindDegeneratePoint {
110 explicit FindDegeneratePoint(
const vpPoint &pt) : m_pt(pt) {}
112 bool operator()(
const vpPoint &pt)
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));
123 bool vpPose::RansacFunctor::poseRansacImpl()
125 const unsigned int size = (
unsigned int)m_listOfUniquePoints.size();
126 const unsigned int nbMinRandom = 4;
129 #if defined(_WIN32) && (defined(_MSC_VER) || defined(__MINGW32__)) 130 srand(m_initial_seed);
135 bool foundSolution =
false;
136 while (nbTrials < m_ransacMaxTrials && m_nbInliers < m_ransacNbInlierConsensus) {
138 std::vector<unsigned int> cur_consensus;
140 std::vector<unsigned int> cur_outliers;
142 std::vector<unsigned int> cur_randoms;
145 std::vector<vpPoint> cur_inliers;
156 std::vector<bool> usedPt(size,
false);
159 for (
unsigned int i = 0; i < nbMinRandom;) {
160 if ((
size_t)std::count(usedPt.begin(), usedPt.end(),
true) == usedPt.size()) {
166 #if defined(_WIN32) && (defined(_MSC_VER) || defined(__MINGW32__)) || defined(ANDROID) 167 unsigned int r_ = (
unsigned int)rand() % size;
169 unsigned int r_ = (
unsigned int)rand_r(&m_initial_seed) % size;
174 #if defined(_WIN32) && (defined(_MSC_VER) || defined(__MINGW32__)) || defined(ANDROID) 175 r_ = (
unsigned int)rand() % size;
177 r_ = (
unsigned int)rand_r(&m_initial_seed) % size;
182 vpPoint pt = m_listOfUniquePoints[r_];
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()) {
194 cur_randoms.push_back(r_);
200 if (poseMin.
npt < nbMinRandom) {
206 bool is_valid_lagrange =
false;
207 bool is_valid_dementhon =
false;
210 double r_lagrange = DBL_MAX;
211 double r_dementhon = DBL_MAX;
216 is_valid_lagrange =
true;
222 is_valid_dementhon =
true;
227 is_valid_lagrange =
false;
228 r_lagrange = DBL_MAX;
232 is_valid_dementhon =
false;
233 r_dementhon = DBL_MAX;
238 if (is_valid_lagrange || is_valid_dementhon) {
240 if (r_lagrange < r_dementhon) {
242 cMo_tmp = cMo_lagrange;
245 cMo_tmp = cMo_dementhon;
247 r = sqrt(r) / (double)nbMinRandom;
250 bool isPoseValid =
true;
251 if (m_func != NULL) {
252 isPoseValid = m_func(cMo_tmp);
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();
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()) {
282 cur_consensus.push_back(iter);
283 cur_inliers.push_back(*it);
285 cur_outliers.push_back(iter);
288 cur_outliers.push_back(iter);
292 if (nbInliersCur > m_nbInliers) {
293 foundSolution =
true;
294 m_best_consensus = cur_consensus;
295 m_nbInliers = nbInliersCur;
300 if (nbTrials >= m_ransacMaxTrials) {
301 foundSolution =
true;
311 #ifdef VISP_HAVE_CPP11_COMPATIBILITY 312 if (m_nbInliers >= m_ransacNbInlierConsensus)
316 return foundSolution;
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());
341 ransacInliers.clear();
342 ransacInlierIndex.clear();
344 std::vector<unsigned int> best_consensus;
345 unsigned int nbInliers = 0;
349 if (listOfPoints.size() < 4) {
353 std::vector<vpPoint> listOfUniquePoints;
354 std::map<size_t, size_t> mapOfUniquePointIndex;
357 bool prefilterDegeneratePoints = ransacFlag == PREFILTER_DEGENERATE_POINTS;
358 bool checkDegeneratePoints = ransacFlag == CHECK_DEGENERATE_POINTS;
360 if (prefilterDegeneratePoints) {
362 std::map<vpPoint, size_t, CompareObjectPointDegenerate> filterObjectPointMap;
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;
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;
377 listOfUniquePoints.push_back(it->first);
378 mapOfUniquePointIndex[listOfUniquePoints.size() - 1] = it->second;
383 listOfUniquePoints = listOfPoints;
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;
392 if (listOfUniquePoints.size() < 4) {
396 #ifdef VISP_HAVE_CPP11_COMPATIBILITY 397 unsigned int nbThreads = 1;
398 bool executeParallelVersion = useParallelRansac;
400 bool executeParallelVersion =
false;
403 if (executeParallelVersion) {
404 #ifdef VISP_HAVE_CPP11_COMPATIBILITY 405 if (nbParallelRansacThreads <= 0) {
407 nbThreads = std::thread::hardware_concurrency();
408 if (nbThreads <= 1) {
410 executeParallelVersion =
false;
416 bool foundSolution =
false;
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();
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;
428 if (i < (
size_t)nthreads - 1) {
429 ransacWorkers.emplace_back(cMo, ransacNbInlierConsensus, splitTrials, ransacThreshold, initial_seed,
430 checkDegeneratePoints, listOfUniquePoints, func, abort);
432 int maxTrialsRemainder = ransacMaxTrials - splitTrials * (nbThreads - 1);
433 ransacWorkers.emplace_back(cMo, ransacNbInlierConsensus, maxTrialsRemainder, ransacThreshold, initial_seed,
434 checkDegeneratePoints, listOfUniquePoints, func, abort);
438 for (
auto& worker : ransacWorkers) {
439 threadpool.emplace_back(&RansacFunctor::operator(), &worker);
442 for (
auto& th : threadpool) {
446 bool successRansac =
false;
447 size_t best_consensus_size = 0;
448 for (
auto &worker : ransacWorkers) {
449 if (worker.getResult()) {
450 successRansac =
true;
452 if (worker.getBestConsensus().size() > best_consensus_size) {
453 nbInliers = worker.getNbInliers();
454 best_consensus = worker.getBestConsensus();
455 best_consensus_size = worker.getBestConsensus().size();
460 foundSolution = successRansac;
464 #ifdef VISP_HAVE_CPP11_COMPATIBILITY 465 std::atomic<bool> abort{
false};
467 RansacFunctor sequentialRansac(cMo, ransacNbInlierConsensus, ransacMaxTrials, ransacThreshold, 0,
468 checkDegeneratePoints, listOfUniquePoints, func
469 #ifdef VISP_HAVE_CPP11_COMPATIBILITY
474 foundSolution = sequentialRansac.getResult();
477 nbInliers = sequentialRansac.getNbInliers();
478 best_consensus = sequentialRansac.getBestConsensus();
483 const unsigned int nbMinRandom = 4;
507 if (nbInliers >= nbMinRandom)
512 for (
size_t i = 0; i < best_consensus.size(); i++) {
513 vpPoint pt = listOfUniquePoints[best_consensus[i]];
516 ransacInliers.push_back(pt);
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]);
526 bool is_valid_lagrange =
false;
527 bool is_valid_dementhon =
false;
530 double r_lagrange = DBL_MAX;
531 double r_dementhon = DBL_MAX;
536 is_valid_lagrange =
true;
542 is_valid_dementhon =
true;
547 is_valid_lagrange =
false;
548 r_lagrange = DBL_MAX;
552 is_valid_dementhon =
false;
553 r_dementhon = DBL_MAX;
556 if (is_valid_lagrange || is_valid_dementhon) {
557 if (r_lagrange < r_dementhon) {
569 if (func != NULL && !func(cMo)) {
573 if (computeCovariance) {
574 covarianceMatrix = pose.covarianceMatrix;
582 return foundSolution;
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);
616 if (maxIterations <= 0) {
617 maxIterations = std::numeric_limits<int>::max();
620 double logarg, logval, N;
621 logarg = -std::pow(1.0 - epsilon, sampleSize);
622 #ifdef VISP_HAVE_FUNC_LOG1P 623 logval = log1p(logarg);
625 logval = log(1.0 + logarg);
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())" 634 N = log((std::max)(1.0 - probability, std::numeric_limits<double>::epsilon())) / logval;
635 if (logval < 0.0 && N < maxIterations) {
639 return maxIterations;
673 const unsigned int &numberOfInlierToReachAConsensus,
const double &threshold,
675 const int &maxNbTrials,
676 const bool useParallelRansac,
const unsigned int nthreads,
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());
692 if (pose.
listP.size() < 4) {
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Implementation of an homogeneous matrix and operations on such kind of matrices.
static bool isNaN(const double value)
void setRansacThreshold(const double &t)
void set_x(const double x)
Set the point x coordinate in the image plane.
void track(const vpHomogeneousMatrix &cMo)
something is not initialized
std::list< vpPoint > listP
Array of point (use here class vpPoint)
std::vector< vpPoint > getRansacInliers() const
Class that defines what is a point.
static bool nul(double x, double s=0.001)
static double sqr(double x)
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
void setNbParallelRansacThreads(const int nb)
void set_y(const double y)
Set the point y coordinate in the image plane.
unsigned int npt
Number of point used in pose computation.
void setRansacMaxTrials(const int &rM)
void setRansacNbInliersToReachConsensus(const unsigned int &nbC)
void setUseParallelRansac(const bool use)
void setWorldCoordinates(const double oX, const double oY, const double oZ)
Error that can be emited by the vpPose class and its derivates.
unsigned int getRansacNbInliers() const
double get_x() const
Get the point x coordinate in the image plane.
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)
double get_y() const
Get the point y coordinate in the image plane.
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...
void addPoint(const vpPoint &P)
static int computeRansacIterations(double probability, double epsilon, const int sampleSize=4, int maxIterations=2000)
void setCovarianceComputation(const bool &flag)