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> 64 int 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;
79 bool 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()
110 void 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;
143 bool 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;
480 TEST_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;
511 TEST_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]));
538 int 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;
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 equal(double x, double y, double s=0.001)
void addPoints(const std::vector< vpPoint > &lP)
double get_oY() const
Get the point oY coordinate in the object frame.
void setRansacThreshold(const double &t)
std::vector< unsigned int > getRansacInlierIndex() const
double get_y() const
Get the point y coordinate in the image plane.
Implementation of a rotation vector as Euler angle minimal representation.
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
void set_x(double x)
Set the point x coordinate in the image plane.
void start(bool reset=true)
void set_y(double y)
Set the point y coordinate in the image plane.
void set_oY(double oY)
Set the point oY coordinate in the object frame.
void setUseParallelRansac(bool use)
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
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.
static double rad(double deg)
void setRansacMaxTrials(const int &rM)
void setRansacNbInliersToReachConsensus(const unsigned int &nbC)
double get_oX() const
Get the point oX coordinate in the object frame.
void set_oZ(double oZ)
Set the point oZ coordinate in the object frame.
Class for generating random number with normal probability density.
void set_oX(double oX)
Set the point oX coordinate in the object frame.
void setRansacFilterFlag(const RANSAC_FILTER_FLAGS &flag)
void addPoint(const vpPoint &P)
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
static int computeRansacIterations(double probability, double epsilon, const int sampleSize=4, int maxIterations=2000)
std::vector< vpPoint > getRansacInliers() const
double computeResidual(const vpHomogeneousMatrix &cMo) const
Compute and return the sum of squared residuals expressed in meter^2 for the pose matrix cMo...