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>
52 #if defined(VISP_HAVE_THREADS)
61 struct CompareObjectPointDegenerate
63 bool operator()(
const vpPoint &point1,
const vpPoint &point2)
const
71 if ((dist1 - dist2) < (-3 * EPS * EPS)) {
74 if ((dist1 - dist2) > (3 * EPS * EPS)) {
78 if ((point1.
oP[0] - point2.
oP[0]) < -EPS) {
81 if ((point1.
oP[0] - point2.
oP[0]) > EPS) {
85 if ((point1.
oP[1] - point2.
oP[1]) < -EPS) {
88 if ((point1.
oP[1] - point2.
oP[1]) > EPS) {
92 if ((point1.
oP[2] - point2.
oP[2]) < -EPS) {
95 if ((point1.
oP[2] - point2.
oP[2]) > EPS) {
104 struct CompareImagePointDegenerate
106 bool operator()(
const vpPoint &point1,
const vpPoint &point2)
const
111 if ((dist1 - dist2) < (-2 * EPS * EPS)) {
114 if ((dist1 - dist2) > (2 * EPS * EPS)) {
118 if ((point1.
p[0] - point2.
p[0]) < -EPS) {
121 if ((point1.
p[0] - point2.
p[0]) > EPS) {
125 if ((point1.
p[1] - point2.
p[1]) < -EPS) {
128 if ((point1.
p[1] - point2.
p[1]) > EPS) {
137 struct FindDegeneratePoint
139 explicit FindDegeneratePoint(
const vpPoint &pt) : m_pt(pt) { }
141 bool operator()(
const vpPoint &pt)
143 bool result_cond1 = ((std::fabs(m_pt.oP[0] - pt.
oP[0]) < EPS) && (std::fabs(m_pt.oP[1] - pt.
oP[1]) < EPS)
144 && (std::fabs(m_pt.oP[2] - pt.
oP[2]) < EPS));
145 bool result_cond2 = (std::fabs(m_pt.p[0] - pt.
p[0]) < EPS) && (std::fabs(m_pt.p[1] - pt.
p[1]) < EPS);
146 return result_cond1 || result_cond2;
153 bool vpPose::vpRansacFunctor::poseRansacImpl()
155 const unsigned int size =
static_cast<unsigned int>(m_listOfUniquePoints.size());
156 unsigned int nbMinRandom = 4;
161 bool foundSolution =
false;
162 while ((nbTrials < m_ransacMaxTrials) && (m_nbInliers < m_ransacNbInlierConsensus)) {
164 std::vector<unsigned int> cur_consensus;
166 std::vector<unsigned int> cur_outliers;
168 std::vector<unsigned int> cur_randoms;
171 std::vector<vpPoint> cur_inliers;
181 std::vector<bool> usedPt(size,
false);
184 for (
unsigned int i = 0; i < nbMinRandom;) {
185 if (
static_cast<size_t>(std::count(usedPt.begin(), usedPt.end(),
true)) == usedPt.size()) {
191 unsigned int r_ = m_uniRand.uniform(0, size);
195 r_ = m_uniRand.uniform(0, size);
199 vpPoint pt = m_listOfUniquePoints[r_];
201 bool degenerate =
false;
202 if (m_checkDegeneratePoints) {
203 if (std::find_if(poseMin.listOfPoints.begin(), poseMin.listOfPoints.end(), FindDegeneratePoint(pt)) !=
204 poseMin.listOfPoints.end()) {
211 cur_randoms.push_back(r_);
217 if (poseMin.
npt < nbMinRandom) {
222 bool is_pose_valid =
false;
223 double r_min = DBL_MAX;
235 is_pose_valid =
false;
240 double r = sqrt(r_min) /
static_cast<double>(nbMinRandom);
243 bool isPoseValid =
true;
244 if (m_func !=
nullptr) {
245 isPoseValid = m_func(cMo_tmp);
255 if (isPoseValid && (r < m_ransacThreshold)) {
256 unsigned int nbInliersCur = 0;
257 unsigned int iter = 0;
258 std::vector<vpPoint>::const_iterator m_listofuniquepoints_end = m_listOfUniquePoints.end();
259 for (std::vector<vpPoint>::const_iterator it = m_listOfUniquePoints.begin(); it != m_listofuniquepoints_end;
265 if (error < m_ransacThreshold) {
266 bool degenerate =
false;
267 if (m_checkDegeneratePoints) {
268 if (std::find_if(cur_inliers.begin(), cur_inliers.end(), FindDegeneratePoint(*it)) != cur_inliers.end()) {
277 cur_consensus.push_back(iter);
278 cur_inliers.push_back(*it);
281 cur_outliers.push_back(iter);
285 cur_outliers.push_back(iter);
289 if (nbInliersCur > m_nbInliers) {
290 foundSolution =
true;
291 m_best_consensus = cur_consensus;
292 m_nbInliers = nbInliersCur;
297 if (nbTrials >= m_ransacMaxTrials) {
298 foundSolution =
true;
310 return foundSolution;
317 if (
listP.size() != listOfPoints.size()) {
318 std::cerr <<
"You should not modify vpPose::listP!" << std::endl;
319 listOfPoints = std::vector<vpPoint>(
listP.begin(),
listP.end());
322 ransacInliers.clear();
323 ransacInlierIndex.clear();
325 std::vector<unsigned int> best_consensus;
326 unsigned int nbInliers = 0;
330 if (listOfPoints.size() < 4) {
334 std::vector<vpPoint> listOfUniquePoints;
335 std::map<size_t, size_t> mapOfUniquePointIndex;
341 if (prefilterDegeneratePoints) {
343 std::map<vpPoint, size_t, CompareObjectPointDegenerate> filterObjectPointMap;
345 std::vector<vpPoint>::const_iterator listofpoints_end = listOfPoints.end();
346 for (std::vector<vpPoint>::const_iterator it_pt = listOfPoints.begin(); it_pt != listofpoints_end;
347 ++it_pt, ++index_pt) {
348 if (filterObjectPointMap.find(*it_pt) == filterObjectPointMap.end()) {
349 filterObjectPointMap[*it_pt] = index_pt;
353 std::map<vpPoint, size_t, CompareImagePointDegenerate> filterImagePointMap;
354 std::map<vpPoint, size_t, CompareObjectPointDegenerate>::const_iterator filterobjectpointmap_end = filterObjectPointMap.end();
355 for (std::map<vpPoint, size_t, CompareObjectPointDegenerate>::const_iterator it = filterObjectPointMap.begin();
356 it != filterobjectpointmap_end; ++it) {
357 if (filterImagePointMap.find(it->first) == filterImagePointMap.end()) {
358 filterImagePointMap[it->first] = it->second;
360 listOfUniquePoints.push_back(it->first);
361 mapOfUniquePointIndex[listOfUniquePoints.size() - 1] = it->second;
367 listOfUniquePoints = listOfPoints;
370 std::vector<vpPoint>::const_iterator listofpoints_end = listOfPoints.end();
371 for (std::vector<vpPoint>::const_iterator it_pt = listOfPoints.begin(); it_pt != listofpoints_end;
372 ++it_pt, ++index_pt) {
373 mapOfUniquePointIndex[index_pt] = index_pt;
377 if (listOfUniquePoints.size() < 4) {
381 #if defined(VISP_HAVE_THREADS)
382 unsigned int nbThreads = 1;
383 bool executeParallelVersion = useParallelRansac;
385 bool executeParallelVersion =
false;
388 if (executeParallelVersion) {
389 #if defined(VISP_HAVE_THREADS)
390 if (nbParallelRansacThreads <= 0) {
392 nbThreads = std::thread::hardware_concurrency();
393 if (nbThreads <= 1) {
395 executeParallelVersion =
false;
399 nbThreads = nbParallelRansacThreads;
404 bool foundSolution =
false;
406 if (executeParallelVersion) {
407 #if defined(VISP_HAVE_THREADS)
408 std::vector<std::thread> threadpool;
409 std::vector<vpRansacFunctor> ransacWorkers;
411 int splitTrials = ransacMaxTrials / nbThreads;
412 for (
size_t i = 0; i < static_cast<size_t>(nbThreads); ++i) {
413 unsigned int initial_seed =
static_cast<unsigned int>(i);
414 if (i <
static_cast<size_t>(nbThreads) - 1) {
415 ransacWorkers.emplace_back(cMo, ransacNbInlierConsensus, splitTrials, ransacThreshold, initial_seed,
416 checkDegeneratePoints, listOfUniquePoints, func);
419 int maxTrialsRemainder = ransacMaxTrials - splitTrials * (nbThreads - 1);
420 ransacWorkers.emplace_back(cMo, ransacNbInlierConsensus, maxTrialsRemainder, ransacThreshold, initial_seed,
421 checkDegeneratePoints, listOfUniquePoints, func);
425 for (
auto &worker : ransacWorkers) {
426 threadpool.emplace_back(&vpRansacFunctor::operator(), &worker);
429 for (
auto &th : threadpool) {
433 bool successRansac =
false;
434 size_t best_consensus_size = 0;
436 for (
auto &worker : ransacWorkers) {
437 if (worker.getResult()) {
438 successRansac =
true;
440 if (worker.getBestConsensus().size() > best_consensus_size) {
441 nbInliers = worker.getNbInliers();
442 best_consensus = worker.getBestConsensus();
443 best_consensus_size = worker.getBestConsensus().size();
448 foundSolution = successRansac;
453 vpRansacFunctor sequentialRansac(cMo, ransacNbInlierConsensus, ransacMaxTrials, ransacThreshold, 0,
454 checkDegeneratePoints, listOfUniquePoints, func);
456 foundSolution = sequentialRansac.getResult();
459 nbInliers = sequentialRansac.getNbInliers();
460 best_consensus = sequentialRansac.getBestConsensus();
465 unsigned int nbMinRandom = 4;
476 if (nbInliers >= nbMinRandom)
481 size_t best_consensus_size = best_consensus.size();
482 for (
size_t i = 0; i < best_consensus_size; ++i) {
483 vpPoint pt = listOfUniquePoints[best_consensus[i]];
486 ransacInliers.push_back(pt);
490 std::vector<unsigned int>::const_iterator best_consensus_end = best_consensus.end();
491 for (std::vector<unsigned int>::const_iterator it_index = best_consensus.begin();
492 it_index != best_consensus_end; ++it_index) {
493 ransacInlierIndex.push_back(
static_cast<unsigned int>(mapOfUniquePointIndex[*it_index]));
502 if ((func !=
nullptr) && (!func(cMo))) {
506 if (computeCovariance) {
507 covarianceMatrix = pose.covarianceMatrix;
515 return foundSolution;
520 probability = std::max<double>(probability, 0.0);
521 probability = std::min<double>(probability, 1.0);
522 epsilon = std::max<double>(epsilon, 0.0);
523 epsilon = std::min<double>(epsilon, 1.0);
530 if (maxIterations <= 0) {
531 maxIterations = std::numeric_limits<int>::max();
534 double logarg, logval, N;
535 logarg = -std::pow(1.0 - epsilon, sampleSize);
536 #ifdef VISP_HAVE_FUNC_LOG1P
537 logval = log1p(logarg);
539 logval = log(1.0 + logarg);
541 if (
vpMath::nul(logval, std::numeric_limits<double>::epsilon())) {
542 std::cerr <<
"vpMath::nul(log(1.0 - std::pow(1.0 - epsilon, "
543 "sampleSize)), std::numeric_limits<double>::epsilon())"
548 N = log(std::max<double>(1.0 - probability, std::numeric_limits<double>::epsilon())) / logval;
549 if ((logval < 0.0) && (N < maxIterations)) {
550 return static_cast<int>(ceil(N));
553 return maxIterations;
557 const unsigned int &numberOfInlierToReachAConsensus,
const double &threshold,
559 const int &maxNbTrials,
bool useParallelRansac,
unsigned int nthreads,
564 unsigned int p2D_size = p2D.size();
565 unsigned int p3D_size = p3D.size();
566 for (
unsigned int i = 0; i < p2D_size; ++i) {
567 for (
unsigned int j = 0; j < p3D_size; ++j) {
568 vpPoint pt(p3D[j].getWorldCoordinates());
569 pt.
set_x(p2D[i].get_x());
570 pt.
set_y(p2D[i].get_y());
575 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)