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>
59 struct CompareObjectPointDegenerate
61 bool operator()(
const vpPoint &point1,
const vpPoint &point2)
const
67 if (dist1 - dist2 < -3 * eps * eps)
69 if (dist1 - dist2 > 3 * eps * eps)
72 if (point1.
oP[0] - point2.
oP[0] < -eps)
74 if (point1.
oP[0] - point2.
oP[0] > eps)
77 if (point1.
oP[1] - point2.
oP[1] < -eps)
79 if (point1.
oP[1] - point2.
oP[1] > eps)
82 if (point1.
oP[2] - point2.
oP[2] < -eps)
84 if (point1.
oP[2] - point2.
oP[2] > eps)
92 struct CompareImagePointDegenerate
94 bool operator()(
const vpPoint &point1,
const vpPoint &point2)
const
98 if (dist1 - dist2 < -2 * eps * eps)
100 if (dist1 - dist2 > 2 * eps * eps)
103 if (point1.
p[0] - point2.
p[0] < -eps)
105 if (point1.
p[0] - point2.
p[0] > eps)
108 if (point1.
p[1] - point2.
p[1] < -eps)
110 if (point1.
p[1] - point2.
p[1] > eps)
118 struct FindDegeneratePoint
120 explicit FindDegeneratePoint(
const vpPoint &pt) : m_pt(pt) { }
122 bool operator()(
const vpPoint &pt)
124 return ((std::fabs(m_pt.oP[0] - pt.
oP[0]) < eps && std::fabs(m_pt.oP[1] - pt.
oP[1]) < eps &&
125 std::fabs(m_pt.oP[2] - pt.
oP[2]) < eps) ||
126 (std::fabs(m_pt.p[0] - pt.
p[0]) < eps && std::fabs(m_pt.p[1] - pt.
p[1]) < eps));
133 bool vpPose::vpRansacFunctor::poseRansacImpl()
135 const unsigned int size = (
unsigned int)m_listOfUniquePoints.size();
136 unsigned int nbMinRandom = 4;
141 bool foundSolution =
false;
142 while (nbTrials < m_ransacMaxTrials && m_nbInliers < m_ransacNbInlierConsensus) {
144 std::vector<unsigned int> cur_consensus;
146 std::vector<unsigned int> cur_outliers;
148 std::vector<unsigned int> cur_randoms;
151 std::vector<vpPoint> cur_inliers;
161 std::vector<bool> usedPt(size,
false);
164 for (
unsigned int i = 0; i < nbMinRandom;) {
165 if ((
size_t)std::count(usedPt.begin(), usedPt.end(),
true) == usedPt.size()) {
171 unsigned int r_ = m_uniRand.uniform(0, size);
175 r_ = m_uniRand.uniform(0, size);
179 vpPoint pt = m_listOfUniquePoints[r_];
181 bool degenerate =
false;
182 if (m_checkDegeneratePoints) {
183 if (std::find_if(poseMin.listOfPoints.begin(), poseMin.listOfPoints.end(), FindDegeneratePoint(pt)) !=
184 poseMin.listOfPoints.end()) {
191 cur_randoms.push_back(r_);
197 if (poseMin.
npt < nbMinRandom) {
202 bool is_pose_valid =
false;
203 double r_min = DBL_MAX;
214 is_pose_valid =
false;
219 double r = sqrt(r_min) / (double)nbMinRandom;
222 bool isPoseValid =
true;
223 if (m_func !=
nullptr) {
224 isPoseValid = m_func(cMo_tmp);
234 if (isPoseValid && r < m_ransacThreshold) {
235 unsigned int nbInliersCur = 0;
236 unsigned int iter = 0;
237 for (std::vector<vpPoint>::const_iterator it = m_listOfUniquePoints.begin(); it != m_listOfUniquePoints.end();
243 if (error < m_ransacThreshold) {
244 bool degenerate =
false;
245 if (m_checkDegeneratePoints) {
246 if (std::find_if(cur_inliers.begin(), cur_inliers.end(), FindDegeneratePoint(*it)) != cur_inliers.end()) {
255 cur_consensus.push_back(iter);
256 cur_inliers.push_back(*it);
259 cur_outliers.push_back(iter);
263 cur_outliers.push_back(iter);
267 if (nbInliersCur > m_nbInliers) {
268 foundSolution =
true;
269 m_best_consensus = cur_consensus;
270 m_nbInliers = nbInliersCur;
275 if (nbTrials >= m_ransacMaxTrials) {
276 foundSolution =
true;
288 return foundSolution;
295 if (
listP.size() != listOfPoints.size()) {
296 std::cerr <<
"You should not modify vpPose::listP!" << std::endl;
297 listOfPoints = std::vector<vpPoint>(
listP.begin(),
listP.end());
300 ransacInliers.clear();
301 ransacInlierIndex.clear();
303 std::vector<unsigned int> best_consensus;
304 unsigned int nbInliers = 0;
308 if (listOfPoints.size() < 4) {
312 std::vector<vpPoint> listOfUniquePoints;
313 std::map<size_t, size_t> mapOfUniquePointIndex;
319 if (prefilterDegeneratePoints) {
321 std::map<vpPoint, size_t, CompareObjectPointDegenerate> filterObjectPointMap;
323 for (std::vector<vpPoint>::const_iterator it_pt = listOfPoints.begin(); it_pt != listOfPoints.end();
324 ++it_pt, index_pt++) {
325 if (filterObjectPointMap.find(*it_pt) == filterObjectPointMap.end()) {
326 filterObjectPointMap[*it_pt] = index_pt;
330 std::map<vpPoint, size_t, CompareImagePointDegenerate> filterImagePointMap;
331 for (std::map<vpPoint, size_t, CompareObjectPointDegenerate>::const_iterator it = filterObjectPointMap.begin();
332 it != filterObjectPointMap.end(); ++it) {
333 if (filterImagePointMap.find(it->first) == filterImagePointMap.end()) {
334 filterImagePointMap[it->first] = it->second;
336 listOfUniquePoints.push_back(it->first);
337 mapOfUniquePointIndex[listOfUniquePoints.size() - 1] = it->second;
343 listOfUniquePoints = listOfPoints;
346 for (std::vector<vpPoint>::const_iterator it_pt = listOfPoints.begin(); it_pt != listOfPoints.end();
347 ++it_pt, index_pt++) {
348 mapOfUniquePointIndex[index_pt] = index_pt;
352 if (listOfUniquePoints.size() < 4) {
356 unsigned int nbThreads = 1;
357 bool executeParallelVersion = useParallelRansac;
359 if (executeParallelVersion) {
360 if (nbParallelRansacThreads <= 0) {
362 nbThreads = std::thread::hardware_concurrency();
363 if (nbThreads <= 1) {
365 executeParallelVersion =
false;
369 nbThreads = nbParallelRansacThreads;
373 bool foundSolution =
false;
375 if (executeParallelVersion) {
376 std::vector<std::thread> threadpool;
377 std::vector<vpRansacFunctor> ransacWorkers;
379 int splitTrials = ransacMaxTrials / nbThreads;
380 for (
size_t i = 0; i < (size_t)nbThreads; i++) {
381 unsigned int initial_seed = (
unsigned int)i;
382 if (i < (
size_t)nbThreads - 1) {
383 ransacWorkers.emplace_back(cMo, ransacNbInlierConsensus, splitTrials, ransacThreshold, initial_seed,
384 checkDegeneratePoints, listOfUniquePoints, func);
387 int maxTrialsRemainder = ransacMaxTrials - splitTrials * (nbThreads - 1);
388 ransacWorkers.emplace_back(cMo, ransacNbInlierConsensus, maxTrialsRemainder, ransacThreshold, initial_seed,
389 checkDegeneratePoints, listOfUniquePoints, func);
393 for (
auto &worker : ransacWorkers) {
394 threadpool.emplace_back(&vpRansacFunctor::operator(), &worker);
397 for (
auto &th : threadpool) {
401 bool successRansac =
false;
402 size_t best_consensus_size = 0;
403 for (
auto &worker : ransacWorkers) {
404 if (worker.getResult()) {
405 successRansac =
true;
407 if (worker.getBestConsensus().size() > best_consensus_size) {
408 nbInliers = worker.getNbInliers();
409 best_consensus = worker.getBestConsensus();
410 best_consensus_size = worker.getBestConsensus().size();
415 foundSolution = successRansac;
419 vpRansacFunctor sequentialRansac(cMo, ransacNbInlierConsensus, ransacMaxTrials, ransacThreshold, 0,
420 checkDegeneratePoints, listOfUniquePoints, func);
422 foundSolution = sequentialRansac.getResult();
425 nbInliers = sequentialRansac.getNbInliers();
426 best_consensus = sequentialRansac.getBestConsensus();
431 unsigned int nbMinRandom = 4;
455 if (nbInliers >= nbMinRandom)
460 for (
size_t i = 0; i < best_consensus.size(); i++) {
461 vpPoint pt = listOfUniquePoints[best_consensus[i]];
464 ransacInliers.push_back(pt);
468 for (std::vector<unsigned int>::const_iterator it_index = best_consensus.begin();
469 it_index != best_consensus.end(); ++it_index) {
470 ransacInlierIndex.push_back((
unsigned int)mapOfUniquePointIndex[*it_index]);
479 if (func !=
nullptr && !func(cMo)) {
483 if (computeCovariance) {
484 covarianceMatrix = pose.covarianceMatrix;
492 return foundSolution;
497 probability = (std::max)(probability, 0.0);
498 probability = (std::min)(probability, 1.0);
499 epsilon = (std::max)(epsilon, 0.0);
500 epsilon = (std::min)(epsilon, 1.0);
507 if (maxIterations <= 0) {
508 maxIterations = std::numeric_limits<int>::max();
511 double logarg, logval, N;
512 logarg = -std::pow(1.0 - epsilon, sampleSize);
513 #ifdef VISP_HAVE_FUNC_LOG1P
514 logval = log1p(logarg);
516 logval = log(1.0 + logarg);
518 if (
vpMath::nul(logval, std::numeric_limits<double>::epsilon())) {
519 std::cerr <<
"vpMath::nul(log(1.0 - std::pow(1.0 - epsilon, "
520 "sampleSize)), std::numeric_limits<double>::epsilon())"
525 N = log((std::max)(1.0 - probability, std::numeric_limits<double>::epsilon())) / logval;
526 if (logval < 0.0 && N < maxIterations) {
530 return maxIterations;
534 const unsigned int &numberOfInlierToReachAConsensus,
const double &threshold,
536 const int &maxNbTrials,
bool useParallelRansac,
unsigned int nthreads,
541 for (
unsigned int i = 0; i < p2D.size(); i++) {
542 for (
unsigned int j = 0; j < p3D.size(); j++) {
543 vpPoint pt(p3D[j].getWorldCoordinates());
544 pt.
set_x(p2D[i].get_x());
545 pt.
set_y(p2D[i].get_y());
550 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 threshold=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 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...
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)
bool poseRansac(vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=nullptr)
@ DEMENTHON_LAGRANGE_VIRTUAL_VS
void setCovarianceComputation(const bool &flag)
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 &)=nullptr)
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)
@ CHECK_DEGENERATE_POINTS
@ PREFILTER_DEGENERATE_POINTS
unsigned int getRansacNbInliers() const
void setUseParallelRansac(bool use)
std::vector< vpPoint > getRansacInliers() const
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=nullptr)
void setNbParallelRansacThreads(int nb)
void setRansacThreshold(const double &t)