36#include <visp3/core/vpConfig.h>
38#ifdef VISP_HAVE_CATCH2
39#define CATCH_CONFIG_RUNNER
45#include <visp3/core/vpGaussRand.h>
46#include <visp3/core/vpHomogeneousMatrix.h>
47#include <visp3/core/vpIoTools.h>
48#include <visp3/core/vpMath.h>
49#include <visp3/core/vpPoint.h>
50#include <visp3/vision/vpPose.h>
64int checkInlierIndex(
const std::vector<unsigned int> &vectorOfFoundInlierIndex,
65 const std::vector<bool> &vectorOfOutlierFlags)
67 int nbInlierIndexOk = 0;
69 for (std::vector<unsigned int>::const_iterator it = vectorOfFoundInlierIndex.begin();
70 it != vectorOfFoundInlierIndex.end(); ++it) {
71 if (!vectorOfOutlierFlags[*it]) {
76 return nbInlierIndexOk;
79bool checkInlierPoints(
const std::vector<vpPoint> &vectorOfFoundInlierPoints,
80 const std::vector<unsigned int> &vectorOfFoundInlierIndex,
81 const std::vector<vpPoint> &bunnyModelPoints_noisy)
83 for (
size_t i = 0; i < vectorOfFoundInlierPoints.size(); i++) {
84 if (!samePoints(vectorOfFoundInlierPoints[i], bunnyModelPoints_noisy[vectorOfFoundInlierIndex[i]])) {
85 std::cerr <<
"Problem with the inlier index and the corresponding "
88 std::cerr <<
"Returned inliers: oX=" << std::setprecision(std::numeric_limits<double>::max_digits10)
89 << vectorOfFoundInlierPoints[i].get_oX()
90 <<
", oY=" << vectorOfFoundInlierPoints[i].get_oY()
91 <<
", oZ=" << vectorOfFoundInlierPoints[i].get_oZ()
92 <<
" ; x=" << vectorOfFoundInlierPoints[i].get_x()
93 <<
", y=" << vectorOfFoundInlierPoints[i].get_y()
95 const vpPoint& pt = bunnyModelPoints_noisy[vectorOfFoundInlierIndex[i]];
96 std::cerr <<
"Object points: oX=" << std::setprecision(std::numeric_limits<double>::max_digits10)
100 <<
" ; x=" << pt.
get_x()
101 <<
", y=" << pt.
get_y()
110void readBunnyModelPoints(
const std::string &filename, std::vector<vpPoint> &bunnyModelPoints,
111 std::vector<vpPoint> &bunnyModelPoints_noisy)
114 std::ifstream file(filename);
115 if (!file.is_open()) {
125 double oX = 0, oY = 0, oZ = 0;
127 while (file >> oX >> oY >> oZ) {
130 bunnyModelPoints.push_back(pt);
135 bunnyModelPoints_noisy.push_back(pt);
139 std::cout <<
"The raw model contains " << bunnyModelPoints.size() <<
" points." << std::endl;
140 std::cout <<
"cMo_groundTruth=\n" << cMo_groundTruth << std::endl << std::endl;
143bool testRansac(
const std::vector<vpPoint> &bunnyModelPoints_original,
144 const std::vector<vpPoint> &bunnyModelPoints_noisy_original,
145 size_t nb_model_points,
bool test_duplicate,
bool test_degenerate)
147 std::vector<vpPoint> bunnyModelPoints = bunnyModelPoints_original;
148 std::vector<vpPoint> bunnyModelPoints_noisy = bunnyModelPoints_noisy_original;
150 if (nb_model_points > 0) {
151 bunnyModelPoints.resize(nb_model_points);
152 bunnyModelPoints_noisy.resize(nb_model_points);
155 vpPose ground_truth_pose, real_pose;
156 ground_truth_pose.
addPoints(bunnyModelPoints);
157 real_pose.
addPoints(bunnyModelPoints_noisy);
166 if (r_lagrange < r_dementhon) {
167 cMo_estimated = cMo_lagrange;
169 cMo_estimated = cMo_dementhon;
174 std::cout <<
"\ncMo estimated using VVS on data with small gaussian noise:\n" << cMo_estimated << std::endl;
175 std::cout <<
"Corresponding residual: " << r_vvs << std::endl;
177 size_t nbOutliers = (size_t)(0.35 * bunnyModelPoints_noisy.size());
180 std::vector<bool> vectorOfOutlierFlags(bunnyModelPoints_noisy.size(),
false);
182 for (
size_t i = 0; i < nbOutliers; i++) {
183 bunnyModelPoints_noisy[i].set_x(bunnyModelPoints_noisy[i].get_x() + noise());
184 bunnyModelPoints_noisy[i].set_y(bunnyModelPoints_noisy[i].get_y() + noise());
185 vectorOfOutlierFlags[i] =
true;
188 if (test_duplicate) {
190 size_t nbDuplicatePoints = 100;
191 for (
size_t i = 0; i < nbDuplicatePoints; i++) {
192 size_t index = (size_t)rand() % bunnyModelPoints_noisy.size();
193 vpPoint duplicatePoint = bunnyModelPoints_noisy[index];
194 bunnyModelPoints_noisy.push_back(duplicatePoint);
195 vectorOfOutlierFlags.push_back(
true);
199 if (test_degenerate) {
201 size_t nbDegeneratePoints = 100;
202 double degenerate_tolerence = 9.999e-7;
205 std::vector<vpPoint> listOfDegeneratePoints;
206 for (
size_t i = 0; i < nbDegeneratePoints; i++) {
207 size_t index = (size_t)rand() % bunnyModelPoints_noisy.size();
208 vpPoint degeneratePoint = bunnyModelPoints_noisy[index];
211 degeneratePoint.
set_oX(degeneratePoint.
get_oX() + degenerate_tolerence);
212 degeneratePoint.
set_oY(degeneratePoint.
get_oY() + degenerate_tolerence);
213 degeneratePoint.
set_oZ(degeneratePoint.
get_oZ() - degenerate_tolerence);
216 listOfDegeneratePoints.push_back(degeneratePoint);
219 index = (size_t)rand() % bunnyModelPoints_noisy.size();
220 degeneratePoint = bunnyModelPoints_noisy[index];
222 degeneratePoint.
set_x(degeneratePoint.
get_x() + degenerate_tolerence);
223 degeneratePoint.
set_y(degeneratePoint.
get_y() - degenerate_tolerence);
226 listOfDegeneratePoints.push_back(degeneratePoint);
229 for (std::vector<vpPoint>::const_iterator it_degenerate = listOfDegeneratePoints.begin();
230 it_degenerate != listOfDegeneratePoints.end(); ++it_degenerate) {
231 bunnyModelPoints_noisy.push_back(*it_degenerate);
232 vectorOfOutlierFlags.push_back(
true);
237 std::vector<size_t> vectorOfIndex(bunnyModelPoints_noisy.size());
238 for (
size_t i = 0; i < vectorOfIndex.size(); i++) {
239 vectorOfIndex[i] = i;
243 std::random_device rng;
244 std::mt19937 urng(rng());
245 std::shuffle(vectorOfIndex.begin(), vectorOfIndex.end(), urng);
247 std::vector<vpPoint> bunnyModelPoints_noisy_tmp = bunnyModelPoints_noisy;
248 bunnyModelPoints_noisy.clear();
249 std::vector<bool> vectorOfOutlierFlags_tmp = vectorOfOutlierFlags;
250 vectorOfOutlierFlags.clear();
251 for (std::vector<size_t>::const_iterator it = vectorOfIndex.begin(); it != vectorOfIndex.end(); ++it) {
252 bunnyModelPoints_noisy.push_back(bunnyModelPoints_noisy_tmp[*it]);
253 vectorOfOutlierFlags.push_back(vectorOfOutlierFlags_tmp[*it]);
258 vpPose pose_ransac, pose_ransac2;
260 vpPose pose_ransac_parallel, pose_ransac_parallel2;
268 for (std::vector<vpPoint>::const_iterator it = bunnyModelPoints_noisy.begin(); it != bunnyModelPoints_noisy.end(); ++it) {
272 pose_ransac.
addPoints(bunnyModelPoints_noisy);
273 pose_ransac2.
addPoints(bunnyModelPoints_noisy);
274 pose_ransac_parallel.
addPoints(bunnyModelPoints_noisy);
275 pose_ransac_parallel2.
addPoints(bunnyModelPoints_noisy);
278 std::cout <<
"\nNumber of model points in the noisy data vector: " << bunnyModelPoints_noisy.size() <<
" points."
282 unsigned int nbInlierToReachConsensus = (
unsigned int)(60.0 * (
double)(bunnyModelPoints_noisy.size()) / 100.0);
283 double threshold = 0.001;
302 std::cout <<
"Number of RANSAC iterations to ensure p=0.99 and epsilon=0.4: " << ransac_iterations << std::endl;
306 chrono_RANSAC.
start();
308 chrono_RANSAC.
stop();
310 std::cout <<
"\ncMo estimated with RANSAC (1000 iterations) on noisy data:\n" << cMo_estimated_RANSAC << std::endl;
311 std::cout <<
"Computation time: " << chrono_RANSAC.
getDurationMs() <<
" ms" << std::endl;
313 double r_RANSAC_estimated = ground_truth_pose.
computeResidual(cMo_estimated_RANSAC);
314 std::cout <<
"Corresponding residual (1000 iterations): " << r_RANSAC_estimated << std::endl;
317 chrono_RANSAC.
start();
319 chrono_RANSAC.
stop();
321 std::cout <<
"\ncMo estimated with RANSAC (" << ransac_iterations <<
" iterations) on noisy data:\n"
322 << cMo_estimated_RANSAC_2 << std::endl;
323 std::cout <<
"Computation time: " << chrono_RANSAC.
getDurationMs() <<
" ms" << std::endl;
325 double r_RANSAC_estimated_2 = ground_truth_pose.
computeResidual(cMo_estimated_RANSAC_2);
326 std::cout <<
"Corresponding residual (" << ransac_iterations <<
" iterations): " << r_RANSAC_estimated_2 << std::endl;
333 if (r_lagrange < r_dementhon) {
334 cMo_estimated = cMo_lagrange;
336 cMo_estimated = cMo_dementhon;
340 std::cout <<
"\ncMo estimated with only VVS on noisy data:\n" << cMo_estimated << std::endl;
343 std::cout <<
"Corresponding residual: " << r_estimated << std::endl;
347 chrono_RANSAC_parallel.
start();
349 chrono_RANSAC_parallel.
stop();
351 std::cout <<
"\ncMo estimated with parallel RANSAC (1000 iterations) on "
353 << cMo_estimated_RANSAC_parallel << std::endl;
354 std::cout <<
"Computation time: " << chrono_RANSAC_parallel.
getDurationMs() <<
" ms" << std::endl;
356 double r_RANSAC_estimated_parallel = ground_truth_pose.
computeResidual(cMo_estimated_RANSAC_parallel);
357 std::cout <<
"Corresponding residual (1000 iterations): " << r_RANSAC_estimated_parallel << std::endl;
361 chrono_RANSAC_parallel2.
start();
363 chrono_RANSAC_parallel2.
stop();
365 std::cout <<
"\ncMo estimated with parallel RANSAC (" << ransac_iterations <<
" iterations) on noisy data:\n"
366 << cMo_estimated_RANSAC_parallel2 << std::endl;
367 std::cout <<
"Computation time: " << chrono_RANSAC_parallel2.
getDurationMs() <<
" ms" << std::endl;
369 double r_RANSAC_estimated_parallel2 = ground_truth_pose.
computeResidual(cMo_estimated_RANSAC_parallel2);
370 std::cout <<
"Corresponding residual (" << ransac_iterations <<
" iterations): " << r_RANSAC_estimated_parallel2
375 int nbInlierIndexOk = checkInlierIndex(vectorOfFoundInlierIndex, vectorOfOutlierFlags);
377 int nbTrueInlierIndex = (int)std::count(vectorOfOutlierFlags.begin(), vectorOfOutlierFlags.end(),
false);
378 std::cout <<
"\nThere are " << nbInlierIndexOk <<
" true inliers found, " << vectorOfFoundInlierIndex.size()
379 <<
" inliers returned and " << nbTrueInlierIndex <<
" true inliers." << std::endl;
382 std::vector<vpPoint> vectorOfFoundInlierPoints = pose_ransac.
getRansacInliers();
384 if (vectorOfFoundInlierPoints.size() != vectorOfFoundInlierIndex.size()) {
385 std::cerr <<
"The number of inlier index is different from the number of "
390 if (!checkInlierPoints(vectorOfFoundInlierPoints, vectorOfFoundInlierIndex, bunnyModelPoints_noisy)) {
396 std::cout <<
"\nCheck for RANSAC iterations: " << ransac_iterations << std::endl;
398 nbInlierIndexOk = checkInlierIndex(vectorOfFoundInlierIndex_2, vectorOfOutlierFlags);
400 std::cout <<
"There are " << nbInlierIndexOk <<
" true inliers found, " << vectorOfFoundInlierIndex_2.size()
401 <<
" inliers returned and " << nbTrueInlierIndex <<
" true inliers." << std::endl;
404 std::vector<vpPoint> vectorOfFoundInlierPoints_2 = pose_ransac2.
getRansacInliers();
405 if (vectorOfFoundInlierPoints_2.size() != vectorOfFoundInlierIndex_2.size()) {
406 std::cerr <<
"The number of inlier index is different from the number of "
411 if (!checkInlierPoints(vectorOfFoundInlierPoints_2, vectorOfFoundInlierIndex_2, bunnyModelPoints_noisy)) {
417 std::cout <<
"\nCheck for parallel RANSAC (1000 iterations)" << std::endl;
418 std::vector<unsigned int> vectorOfFoundInlierIndex_parallel = pose_ransac_parallel.
getRansacInlierIndex();
419 nbInlierIndexOk = checkInlierIndex(vectorOfFoundInlierIndex_parallel, vectorOfOutlierFlags);
421 std::cout <<
"There are " << nbInlierIndexOk <<
" true inliers found, " << vectorOfFoundInlierIndex_parallel.size()
422 <<
" inliers returned and " << nbTrueInlierIndex <<
" true inliers." << std::endl;
425 std::vector<vpPoint> vectorOfFoundInlierPoints_parallel = pose_ransac_parallel.
getRansacInliers();
426 if (vectorOfFoundInlierPoints_parallel.size() != vectorOfFoundInlierIndex_parallel.size()) {
427 std::cerr <<
"The number of inlier index is different from the number "
432 if (!checkInlierPoints(vectorOfFoundInlierPoints_parallel, vectorOfFoundInlierIndex_parallel,
433 bunnyModelPoints_noisy)) {
439 std::cout <<
"\nCheck for parallel RANSAC (" << ransac_iterations <<
" iterations)" << std::endl;
440 std::vector<unsigned int> vectorOfFoundInlierIndex_parallel2 = pose_ransac_parallel2.
getRansacInlierIndex();
441 nbInlierIndexOk = checkInlierIndex(vectorOfFoundInlierIndex_parallel2, vectorOfOutlierFlags);
443 std::cout <<
"There are " << nbInlierIndexOk <<
" true inliers found, " << vectorOfFoundInlierIndex_parallel2.size()
444 <<
" inliers returned and " << nbTrueInlierIndex <<
" true inliers." << std::endl;
447 std::vector<vpPoint> vectorOfFoundInlierPoints_parallel2 = pose_ransac_parallel2.
getRansacInliers();
448 if (vectorOfFoundInlierPoints_parallel2.size() != vectorOfFoundInlierIndex_parallel2.size()) {
449 std::cerr <<
"The number of inlier index is different from the number "
454 if (!checkInlierPoints(vectorOfFoundInlierPoints_parallel2, vectorOfFoundInlierIndex_parallel2,
455 bunnyModelPoints_noisy)) {
459 if (r_RANSAC_estimated > threshold ) {
460 std::cerr <<
"The pose estimated with the RANSAC method is badly estimated!" << std::endl;
461 std::cerr <<
"r_RANSAC_estimated=" << r_RANSAC_estimated << std::endl;
462 std::cerr <<
"threshold=" << threshold << std::endl;
465 if (r_RANSAC_estimated_parallel > threshold) {
466 std::cerr <<
"The pose estimated with the parallel RANSAC method is "
469 std::cerr <<
"r_RANSAC_estimated_parallel=" << r_RANSAC_estimated_parallel << std::endl;
470 std::cerr <<
"threshold=" << threshold << std::endl;
473 std::cout <<
"The pose estimated with the RANSAC method is well estimated!" << std::endl;
480TEST_CASE(
"Print RANSAC number of iterations",
"[ransac_pose]") {
481 const int sample_sizes[] = {2, 3, 4, 5, 6, 7, 8};
482 const double epsilon[] = {0.05, 0.1, 0.2, 0.25, 0.3, 0.4, 0.5};
485 const std::string spacing =
" ";
487 std::cout << spacing <<
" outliers percentage\n" <<
"nb pts\\";
488 for (
int cpt2 = 0; cpt2 < 7; cpt2++) {
489 std::cout << std::setfill(
' ') << std::setw(5) << epsilon[cpt2] <<
" ";
491 std::cout << std::endl;
493 std::cout << std::setfill(
' ') << std::setw(7) <<
"+";
494 for (
int cpt2 = 0; cpt2 < 6; cpt2++) {
495 std::cout << std::setw(7) <<
"-------";
497 std::cout << std::endl;
499 for (
int cpt1 = 0; cpt1 < 7; cpt1++) {
500 std::cout << std::setfill(
' ') << std::setw(6) << sample_sizes[cpt1] <<
"|";
502 for (
int cpt2 = 0; cpt2 < 7; cpt2++) {
504 std::cout << std::setfill(
' ') << std::setw(6) << ransac_iters;
506 std::cout << std::endl;
508 std::cout << std::endl;
511TEST_CASE(
"RANSAC pose estimation tests",
"[ransac_pose]") {
512 const std::vector<size_t> model_sizes = {10, 20, 50, 100, 200, 500, 1000, 0, 0};
513 const std::vector<bool> duplicates = {
false,
false,
false,
false,
false,
false,
false,
false,
true};
514 const std::vector<bool> degenerates = {
false,
false,
false,
false,
false,
false,
true,
true,
true};
520 std::vector<vpPoint> bunnyModelPoints, bunnyModelPoints_noisy_original;
521 readBunnyModelPoints(model_filename, bunnyModelPoints, bunnyModelPoints_noisy_original);
522 CHECK(bunnyModelPoints.size() == bunnyModelPoints_noisy_original.size());
524 for (
size_t i = 0; i < model_sizes.size(); i++) {
525 std::cout <<
"\n\n==============================================================================="
527 if (model_sizes[i] == 0) {
528 std::cout <<
"Test on " << bunnyModelPoints_noisy_original.size() <<
" model points." << std::endl;
530 std::cout <<
"Test on " << model_sizes[i] <<
" model points." << std::endl;
532 std::cout <<
"Test duplicate: " << duplicates[i] <<
" ; Test degenerate: " << degenerates[i] << std::endl;
534 CHECK(testRansac(bunnyModelPoints, bunnyModelPoints_noisy_original, model_sizes[i], duplicates[i], degenerates[i]));
538int main(
int argc,
char* argv[])
540#if defined(__mips__) || defined(__mips) || defined(mips) || defined(__MIPS__)
545#if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
547 Catch::Session session;
550 session.applyCommandLine(argc, argv);
552 int numFailed = session.run();
559 std::cout <<
"Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
void start(bool reset=true)
Class for generating random number with normal probability density.
Implementation of an homogeneous matrix and operations on such kind of matrices.
static double rad(double deg)
static bool equal(double x, double y, 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.
void set_oY(double oY)
Set the point oY coordinate in the object frame.
double get_x() const
Get the point x coordinate in the image plane.
void set_oZ(double oZ)
Set the point oZ coordinate in the object frame.
void set_oX(double oX)
Set the point oX coordinate in the object frame.
double get_oY() const
Get the point oY coordinate in the object frame.
void set_y(double y)
Set the point y coordinate in the image plane.
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)
std::vector< unsigned int > getRansacInlierIndex() const
std::vector< vpPoint > getRansacInliers() const
void addPoints(const std::vector< vpPoint > &lP)
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)
@ PREFILTER_DEGENERATE_POINTS
void setUseParallelRansac(bool use)
void setRansacThreshold(const double &t)
Implementation of a rotation vector as Euler angle minimal representation.
Implementation of a rotation vector as axis-angle minimal representation.
Class that consider the case of a translation vector.