34 #include <visp3/core/vpConfig.h>
36 #ifdef VISP_HAVE_CATCH2
37 #define CATCH_CONFIG_RUNNER
43 #include <visp3/core/vpGaussRand.h>
44 #include <visp3/core/vpHomogeneousMatrix.h>
45 #include <visp3/core/vpIoTools.h>
46 #include <visp3/core/vpMath.h>
47 #include <visp3/core/vpPoint.h>
48 #include <visp3/vision/vpPose.h>
52 #if (VISP_HAVE_DATASET_VERSION >= 0x030300)
62 int checkInlierIndex(
const std::vector<unsigned int> &vectorOfFoundInlierIndex,
63 const std::vector<bool> &vectorOfOutlierFlags)
65 int nbInlierIndexOk = 0;
67 for (std::vector<unsigned int>::const_iterator it = vectorOfFoundInlierIndex.begin();
68 it != vectorOfFoundInlierIndex.end(); ++it) {
69 if (!vectorOfOutlierFlags[*it]) {
74 return nbInlierIndexOk;
77 bool checkInlierPoints(
const std::vector<vpPoint> &vectorOfFoundInlierPoints,
78 const std::vector<unsigned int> &vectorOfFoundInlierIndex,
79 const std::vector<vpPoint> &bunnyModelPoints_noisy)
81 for (
size_t i = 0; i < vectorOfFoundInlierPoints.size(); i++) {
82 if (!samePoints(vectorOfFoundInlierPoints[i], bunnyModelPoints_noisy[vectorOfFoundInlierIndex[i]])) {
83 std::cerr <<
"Problem with the inlier index and the corresponding "
86 std::cerr <<
"Returned inliers: oX=" << std::setprecision(std::numeric_limits<double>::max_digits10)
87 << vectorOfFoundInlierPoints[i].get_oX() <<
", oY=" << vectorOfFoundInlierPoints[i].get_oY()
88 <<
", oZ=" << vectorOfFoundInlierPoints[i].get_oZ() <<
" ; x=" << vectorOfFoundInlierPoints[i].get_x()
89 <<
", y=" << vectorOfFoundInlierPoints[i].get_y() << std::endl;
90 const vpPoint &pt = bunnyModelPoints_noisy[vectorOfFoundInlierIndex[i]];
91 std::cerr <<
"Object points: oX=" << std::setprecision(std::numeric_limits<double>::max_digits10) << pt.
get_oX()
101 void readBunnyModelPoints(
const std::string &filename, std::vector<vpPoint> &bunnyModelPoints,
102 std::vector<vpPoint> &bunnyModelPoints_noisy)
105 std::ifstream file(filename);
106 if (!file.is_open()) {
116 double oX = 0, oY = 0, oZ = 0;
118 while (file >> oX >> oY >> oZ) {
121 bunnyModelPoints.push_back(pt);
126 bunnyModelPoints_noisy.push_back(pt);
130 std::cout <<
"The raw model contains " << bunnyModelPoints.size() <<
" points." << std::endl;
131 std::cout <<
"cMo_groundTruth=\n" << cMo_groundTruth << std::endl << std::endl;
134 bool testRansac(
const std::vector<vpPoint> &bunnyModelPoints_original,
135 const std::vector<vpPoint> &bunnyModelPoints_noisy_original,
size_t nb_model_points,
136 bool test_duplicate,
bool test_degenerate)
138 std::vector<vpPoint> bunnyModelPoints = bunnyModelPoints_original;
139 std::vector<vpPoint> bunnyModelPoints_noisy = bunnyModelPoints_noisy_original;
141 if (nb_model_points > 0) {
142 bunnyModelPoints.resize(nb_model_points);
143 bunnyModelPoints_noisy.resize(nb_model_points);
146 vpPose ground_truth_pose, real_pose;
148 ground_truth_pose.
addPoints(bunnyModelPoints);
149 real_pose.
addPoints(bunnyModelPoints_noisy);
153 std::cout <<
"\ncMo estimated using VVS on data with small gaussian noise:\n" << cMo_estimated << std::endl;
154 std::cout <<
"Corresponding residual: " << r_vvs << std::endl;
156 size_t nbOutliers = (size_t)(0.35 * bunnyModelPoints_noisy.size());
159 std::vector<bool> vectorOfOutlierFlags(bunnyModelPoints_noisy.size(),
false);
161 for (
size_t i = 0; i < nbOutliers; i++) {
162 bunnyModelPoints_noisy[i].set_x(bunnyModelPoints_noisy[i].get_x() + noise());
163 bunnyModelPoints_noisy[i].set_y(bunnyModelPoints_noisy[i].get_y() + noise());
164 vectorOfOutlierFlags[i] =
true;
167 if (test_duplicate) {
169 size_t nbDuplicatePoints = 100;
170 for (
size_t i = 0; i < nbDuplicatePoints; i++) {
171 size_t index = (size_t)rand() % bunnyModelPoints_noisy.size();
172 vpPoint duplicatePoint = bunnyModelPoints_noisy[index];
173 bunnyModelPoints_noisy.push_back(duplicatePoint);
174 vectorOfOutlierFlags.push_back(
true);
178 if (test_degenerate) {
180 size_t nbDegeneratePoints = 100;
181 double degenerate_tolerence = 9.999e-7;
184 std::vector<vpPoint> listOfDegeneratePoints;
185 for (
size_t i = 0; i < nbDegeneratePoints; i++) {
186 size_t index = (size_t)rand() % bunnyModelPoints_noisy.size();
187 vpPoint degeneratePoint = bunnyModelPoints_noisy[index];
190 degeneratePoint.
set_oX(degeneratePoint.
get_oX() + degenerate_tolerence);
191 degeneratePoint.
set_oY(degeneratePoint.
get_oY() + degenerate_tolerence);
192 degeneratePoint.
set_oZ(degeneratePoint.
get_oZ() - degenerate_tolerence);
195 listOfDegeneratePoints.push_back(degeneratePoint);
198 index = (size_t)rand() % bunnyModelPoints_noisy.size();
199 degeneratePoint = bunnyModelPoints_noisy[index];
201 degeneratePoint.
set_x(degeneratePoint.
get_x() + degenerate_tolerence);
202 degeneratePoint.
set_y(degeneratePoint.
get_y() - degenerate_tolerence);
205 listOfDegeneratePoints.push_back(degeneratePoint);
208 for (std::vector<vpPoint>::const_iterator it_degenerate = listOfDegeneratePoints.begin();
209 it_degenerate != listOfDegeneratePoints.end(); ++it_degenerate) {
210 bunnyModelPoints_noisy.push_back(*it_degenerate);
211 vectorOfOutlierFlags.push_back(
true);
216 std::vector<size_t> vectorOfIndex(bunnyModelPoints_noisy.size());
217 for (
size_t i = 0; i < vectorOfIndex.size(); i++) {
218 vectorOfIndex[i] = i;
222 std::random_device rng;
223 std::mt19937 urng(rng());
224 std::shuffle(vectorOfIndex.begin(), vectorOfIndex.end(), urng);
226 std::vector<vpPoint> bunnyModelPoints_noisy_tmp = bunnyModelPoints_noisy;
227 bunnyModelPoints_noisy.clear();
228 std::vector<bool> vectorOfOutlierFlags_tmp = vectorOfOutlierFlags;
229 vectorOfOutlierFlags.clear();
230 for (std::vector<size_t>::const_iterator it = vectorOfIndex.begin(); it != vectorOfIndex.end(); ++it) {
231 bunnyModelPoints_noisy.push_back(bunnyModelPoints_noisy_tmp[*it]);
232 vectorOfOutlierFlags.push_back(vectorOfOutlierFlags_tmp[*it]);
237 vpPose pose_ransac, pose_ransac2;
239 vpPose pose_ransac_parallel, pose_ransac_parallel2;
247 for (std::vector<vpPoint>::const_iterator it = bunnyModelPoints_noisy.begin(); it != bunnyModelPoints_noisy.end();
252 pose_ransac.
addPoints(bunnyModelPoints_noisy);
253 pose_ransac2.
addPoints(bunnyModelPoints_noisy);
254 pose_ransac_parallel.
addPoints(bunnyModelPoints_noisy);
255 pose_ransac_parallel2.
addPoints(bunnyModelPoints_noisy);
258 std::cout <<
"\nNumber of model points in the noisy data vector: " << bunnyModelPoints_noisy.size() <<
" points."
262 unsigned int nbInlierToReachConsensus = (
unsigned int)(60.0 * (
double)(bunnyModelPoints_noisy.size()) / 100.0);
263 double threshold = 0.001;
282 std::cout <<
"Number of RANSAC iterations to ensure p=0.99 and epsilon=0.4: " << ransac_iterations << std::endl;
286 chrono_RANSAC.
start();
288 chrono_RANSAC.
stop();
290 std::cout <<
"\ncMo estimated with RANSAC (1000 iterations) on noisy data:\n" << cMo_estimated_RANSAC << std::endl;
291 std::cout <<
"Computation time: " << chrono_RANSAC.
getDurationMs() <<
" ms" << std::endl;
293 double r_RANSAC_estimated = ground_truth_pose.
computeResidual(cMo_estimated_RANSAC);
294 std::cout <<
"Corresponding residual (1000 iterations): " << r_RANSAC_estimated << std::endl;
297 chrono_RANSAC.
start();
299 chrono_RANSAC.
stop();
301 std::cout <<
"\ncMo estimated with RANSAC (" << ransac_iterations <<
" iterations) on noisy data:\n"
302 << cMo_estimated_RANSAC_2 << std::endl;
303 std::cout <<
"Computation time: " << chrono_RANSAC.
getDurationMs() <<
" ms" << std::endl;
305 double r_RANSAC_estimated_2 = ground_truth_pose.
computeResidual(cMo_estimated_RANSAC_2);
306 std::cout <<
"Corresponding residual (" << ransac_iterations <<
" iterations): " << r_RANSAC_estimated_2 << std::endl;
309 std::cout <<
"\ncMo estimated with only VVS on noisy data:\n" << cMo_estimated << std::endl;
312 std::cout <<
"Corresponding residual: " << r_estimated << std::endl;
316 chrono_RANSAC_parallel.
start();
318 chrono_RANSAC_parallel.
stop();
320 std::cout <<
"\ncMo estimated with parallel RANSAC (1000 iterations) on "
322 << cMo_estimated_RANSAC_parallel << std::endl;
323 std::cout <<
"Computation time: " << chrono_RANSAC_parallel.
getDurationMs() <<
" ms" << std::endl;
325 double r_RANSAC_estimated_parallel = ground_truth_pose.
computeResidual(cMo_estimated_RANSAC_parallel);
326 std::cout <<
"Corresponding residual (1000 iterations): " << r_RANSAC_estimated_parallel << std::endl;
330 chrono_RANSAC_parallel2.
start();
332 chrono_RANSAC_parallel2.
stop();
334 std::cout <<
"\ncMo estimated with parallel RANSAC (" << ransac_iterations <<
" iterations) on noisy data:\n"
335 << cMo_estimated_RANSAC_parallel2 << std::endl;
336 std::cout <<
"Computation time: " << chrono_RANSAC_parallel2.
getDurationMs() <<
" ms" << std::endl;
338 double r_RANSAC_estimated_parallel2 = ground_truth_pose.
computeResidual(cMo_estimated_RANSAC_parallel2);
339 std::cout <<
"Corresponding residual (" << ransac_iterations <<
" iterations): " << r_RANSAC_estimated_parallel2
344 int nbInlierIndexOk = checkInlierIndex(vectorOfFoundInlierIndex, vectorOfOutlierFlags);
346 int nbTrueInlierIndex = (int)std::count(vectorOfOutlierFlags.begin(), vectorOfOutlierFlags.end(),
false);
347 std::cout <<
"\nThere are " << nbInlierIndexOk <<
" true inliers found, " << vectorOfFoundInlierIndex.size()
348 <<
" inliers returned and " << nbTrueInlierIndex <<
" true inliers." << std::endl;
351 std::vector<vpPoint> vectorOfFoundInlierPoints = pose_ransac.
getRansacInliers();
353 if (vectorOfFoundInlierPoints.size() != vectorOfFoundInlierIndex.size()) {
354 std::cerr <<
"The number of inlier index is different from the number of "
359 if (!checkInlierPoints(vectorOfFoundInlierPoints, vectorOfFoundInlierIndex, bunnyModelPoints_noisy)) {
365 std::cout <<
"\nCheck for RANSAC iterations: " << ransac_iterations << std::endl;
367 nbInlierIndexOk = checkInlierIndex(vectorOfFoundInlierIndex_2, vectorOfOutlierFlags);
369 std::cout <<
"There are " << nbInlierIndexOk <<
" true inliers found, " << vectorOfFoundInlierIndex_2.size()
370 <<
" inliers returned and " << nbTrueInlierIndex <<
" true inliers." << std::endl;
373 std::vector<vpPoint> vectorOfFoundInlierPoints_2 = pose_ransac2.
getRansacInliers();
374 if (vectorOfFoundInlierPoints_2.size() != vectorOfFoundInlierIndex_2.size()) {
375 std::cerr <<
"The number of inlier index is different from the number of "
380 if (!checkInlierPoints(vectorOfFoundInlierPoints_2, vectorOfFoundInlierIndex_2, bunnyModelPoints_noisy)) {
386 std::cout <<
"\nCheck for parallel RANSAC (1000 iterations)" << std::endl;
387 std::vector<unsigned int> vectorOfFoundInlierIndex_parallel = pose_ransac_parallel.
getRansacInlierIndex();
388 nbInlierIndexOk = checkInlierIndex(vectorOfFoundInlierIndex_parallel, vectorOfOutlierFlags);
390 std::cout <<
"There are " << nbInlierIndexOk <<
" true inliers found, " << vectorOfFoundInlierIndex_parallel.size()
391 <<
" inliers returned and " << nbTrueInlierIndex <<
" true inliers." << std::endl;
394 std::vector<vpPoint> vectorOfFoundInlierPoints_parallel = pose_ransac_parallel.
getRansacInliers();
395 if (vectorOfFoundInlierPoints_parallel.size() != vectorOfFoundInlierIndex_parallel.size()) {
396 std::cerr <<
"The number of inlier index is different from the number "
401 if (!checkInlierPoints(vectorOfFoundInlierPoints_parallel, vectorOfFoundInlierIndex_parallel,
402 bunnyModelPoints_noisy)) {
408 std::cout <<
"\nCheck for parallel RANSAC (" << ransac_iterations <<
" iterations)" << std::endl;
409 std::vector<unsigned int> vectorOfFoundInlierIndex_parallel2 = pose_ransac_parallel2.
getRansacInlierIndex();
410 nbInlierIndexOk = checkInlierIndex(vectorOfFoundInlierIndex_parallel2, vectorOfOutlierFlags);
412 std::cout <<
"There are " << nbInlierIndexOk <<
" true inliers found, " << vectorOfFoundInlierIndex_parallel2.size()
413 <<
" inliers returned and " << nbTrueInlierIndex <<
" true inliers." << std::endl;
416 std::vector<vpPoint> vectorOfFoundInlierPoints_parallel2 = pose_ransac_parallel2.
getRansacInliers();
417 if (vectorOfFoundInlierPoints_parallel2.size() != vectorOfFoundInlierIndex_parallel2.size()) {
418 std::cerr <<
"The number of inlier index is different from the number "
423 if (!checkInlierPoints(vectorOfFoundInlierPoints_parallel2, vectorOfFoundInlierIndex_parallel2,
424 bunnyModelPoints_noisy)) {
428 if (r_RANSAC_estimated > threshold ) {
429 std::cerr <<
"The pose estimated with the RANSAC method is badly estimated!" << std::endl;
430 std::cerr <<
"r_RANSAC_estimated=" << r_RANSAC_estimated << std::endl;
431 std::cerr <<
"threshold=" << threshold << std::endl;
434 if (r_RANSAC_estimated_parallel > threshold) {
435 std::cerr <<
"The pose estimated with the parallel RANSAC method is "
438 std::cerr <<
"r_RANSAC_estimated_parallel=" << r_RANSAC_estimated_parallel << std::endl;
439 std::cerr <<
"threshold=" << threshold << std::endl;
442 std::cout <<
"The pose estimated with the RANSAC method is well estimated!" << std::endl;
450 TEST_CASE(
"Print RANSAC number of iterations",
"[ransac_pose]")
452 const int sample_sizes[] = {2, 3, 4, 5, 6, 7, 8};
453 const double epsilon[] = {0.05, 0.1, 0.2, 0.25, 0.3, 0.4, 0.5};
456 const std::string spacing =
" ";
458 std::cout << spacing <<
" outliers percentage\n"
460 for (
int cpt2 = 0; cpt2 < 7; cpt2++) {
461 std::cout << std::setfill(
' ') << std::setw(5) << epsilon[cpt2] <<
" ";
463 std::cout << std::endl;
465 std::cout << std::setfill(
' ') << std::setw(7) <<
"+";
466 for (
int cpt2 = 0; cpt2 < 6; cpt2++) {
467 std::cout << std::setw(7) <<
"-------";
469 std::cout << std::endl;
471 for (
int cpt1 = 0; cpt1 < 7; cpt1++) {
472 std::cout << std::setfill(
' ') << std::setw(6) << sample_sizes[cpt1] <<
"|";
474 for (
int cpt2 = 0; cpt2 < 7; cpt2++) {
476 std::cout << std::setfill(
' ') << std::setw(6) << ransac_iters;
478 std::cout << std::endl;
480 std::cout << std::endl;
483 #if (VISP_HAVE_DATASET_VERSION >= 0x030300)
484 TEST_CASE(
"RANSAC pose estimation tests",
"[ransac_pose]")
486 const std::vector<size_t> model_sizes = {10, 20, 50, 100, 200, 500, 1000, 0, 0};
487 const std::vector<bool> duplicates = {
false,
false,
false,
false,
false,
false,
false,
false,
true};
488 const std::vector<bool> degenerates = {
false,
false,
false,
false,
false,
false,
true,
true,
true};
494 std::vector<vpPoint> bunnyModelPoints, bunnyModelPoints_noisy_original;
495 readBunnyModelPoints(model_filename, bunnyModelPoints, bunnyModelPoints_noisy_original);
496 CHECK(bunnyModelPoints.size() == bunnyModelPoints_noisy_original.size());
498 for (
size_t i = 0; i < model_sizes.size(); i++) {
499 std::cout <<
"\n\n===============================================================================" << std::endl;
500 if (model_sizes[i] == 0) {
501 std::cout <<
"Test on " << bunnyModelPoints_noisy_original.size() <<
" model points." << std::endl;
503 std::cout <<
"Test on " << model_sizes[i] <<
" model points." << std::endl;
505 std::cout <<
"Test duplicate: " << duplicates[i] <<
" ; Test degenerate: " << degenerates[i] << std::endl;
507 CHECK(testRansac(bunnyModelPoints, bunnyModelPoints_noisy_original, model_sizes[i], duplicates[i], degenerates[i]));
512 int main(
int argc,
char *argv[])
514 #if defined(__mips__) || defined(__mips) || defined(mips) || defined(__MIPS__)
519 #if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
521 Catch::Session session;
524 session.applyCommandLine(argc, argv);
526 int numFailed = session.run();
533 std::cout <<
"Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
538 int main() {
return EXIT_SUCCESS; }
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 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.
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)
@ DEMENTHON_LAGRANGE_VIRTUAL_VS
std::vector< unsigned int > getRansacInlierIndex() 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)
@ PREFILTER_DEGENERATE_POINTS
void setUseParallelRansac(bool use)
std::vector< vpPoint > getRansacInliers() const
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=nullptr)
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.