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
double get_y() const
Get the point y coordinate in the image plane.
std::list< vpPoint > listP
Array of point (use here class vpPoint)
Class that defines what is a point.
unsigned int getRansacNbInliers() const
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.
double get_x() const
Get the point x 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.
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)
bool poseRansac(vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
void addPoint(const vpPoint &P)
static int computeRansacIterations(double probability, double epsilon, const int sampleSize=4, int maxIterations=2000)
std::vector< vpPoint > getRansacInliers() const
void setCovarianceComputation(const bool &flag)
double computeResidual(const vpHomogeneousMatrix &cMo) const
Compute and return the sum of squared residuals expressed in meter^2 for the pose matrix cMo...