Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
testPoseRansac2.cpp
1/*
2 * ViSP, open source Visual Servoing Platform software.
3 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4 *
5 * This software is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or
8 * (at your option) any later version.
9 * See the file LICENSE.txt at the root directory of this source
10 * distribution for additional information about the GNU GPL.
11 *
12 * For using ViSP with software that can not be combined with the GNU
13 * GPL, please contact Inria about acquiring a ViSP Professional
14 * Edition License.
15 *
16 * See https://visp.inria.fr for more information.
17 *
18 * This software was developed at:
19 * Inria Rennes - Bretagne Atlantique
20 * Campus Universitaire de Beaulieu
21 * 35042 Rennes Cedex
22 * France
23 *
24 * If you have questions regarding the use of this file, please contact
25 * Inria at visp@inria.fr
26 *
27 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29 *
30 * Description:
31 * Test RANSAC 3D pose estimation method.
32 */
33
34#include <visp3/core/vpConfig.h>
35
36#ifdef VISP_HAVE_CATCH2
37#define CATCH_CONFIG_RUNNER
38#include <catch.hpp>
39
40#include <algorithm>
41#include <iomanip>
42#include <map>
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>
49
50namespace
51{
52#if (VISP_HAVE_DATASET_VERSION >= 0x030300)
53bool samePoints(const vpPoint &pt1, const vpPoint &pt2)
54{
55 return vpMath::equal(pt1.get_oX(), pt2.get_oX(), std::numeric_limits<double>::epsilon()) &&
56 vpMath::equal(pt1.get_oY(), pt2.get_oY(), std::numeric_limits<double>::epsilon()) &&
57 vpMath::equal(pt1.get_oZ(), pt2.get_oZ(), std::numeric_limits<double>::epsilon()) &&
58 vpMath::equal(pt1.get_x(), pt2.get_x(), std::numeric_limits<double>::epsilon()) &&
59 vpMath::equal(pt1.get_y(), pt2.get_y(), std::numeric_limits<double>::epsilon());
60}
61
62int checkInlierIndex(const std::vector<unsigned int> &vectorOfFoundInlierIndex,
63 const std::vector<bool> &vectorOfOutlierFlags)
64{
65 int nbInlierIndexOk = 0;
66
67 for (std::vector<unsigned int>::const_iterator it = vectorOfFoundInlierIndex.begin();
68 it != vectorOfFoundInlierIndex.end(); ++it) {
69 if (!vectorOfOutlierFlags[*it]) {
70 nbInlierIndexOk++;
71 }
72 }
73
74 return nbInlierIndexOk;
75}
76
77bool checkInlierPoints(const std::vector<vpPoint> &vectorOfFoundInlierPoints,
78 const std::vector<unsigned int> &vectorOfFoundInlierIndex,
79 const std::vector<vpPoint> &bunnyModelPoints_noisy)
80{
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 "
84 "inlier point!"
85 << std::endl;
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()
92 << ", oY=" << pt.get_oY() << ", oZ=" << pt.get_oZ() << " ; x=" << pt.get_x() << ", y=" << pt.get_y()
93 << std::endl;
94 return false;
95 }
96 }
97
98 return true;
99}
100
101void readBunnyModelPoints(const std::string &filename, std::vector<vpPoint> &bunnyModelPoints,
102 std::vector<vpPoint> &bunnyModelPoints_noisy)
103{
104 // Read the model
105 std::ifstream file(filename);
106 if (!file.is_open()) {
107 return;
108 }
109
110 // ground truth cMo
111 const vpTranslationVector translation(-0.14568, 0.154567, 1.4462);
112 const vpRzyxVector zyxVector(vpMath::rad(12.4146f), vpMath::rad(-75.5478f), vpMath::rad(138.5607f));
113 vpHomogeneousMatrix cMo_groundTruth(translation, vpThetaUVector(zyxVector));
114
115 vpGaussRand gaussian_noise(0.0002, 0.0);
116 double oX = 0, oY = 0, oZ = 0;
117
118 while (file >> oX >> oY >> oZ) {
119 vpPoint pt(oX, oY, oZ);
120 pt.project(cMo_groundTruth);
121 bunnyModelPoints.push_back(pt);
122
123 // Add a small gaussian noise to the data
124 pt.set_x(pt.get_x() + gaussian_noise());
125 pt.set_y(pt.get_y() + gaussian_noise());
126 bunnyModelPoints_noisy.push_back(pt);
127 }
128
129 // Print the number of model points
130 std::cout << "The raw model contains " << bunnyModelPoints.size() << " points." << std::endl;
131 std::cout << "cMo_groundTruth=\n" << cMo_groundTruth << std::endl << std::endl;
132}
133
134bool 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)
137{
138 std::vector<vpPoint> bunnyModelPoints = bunnyModelPoints_original;
139 std::vector<vpPoint> bunnyModelPoints_noisy = bunnyModelPoints_noisy_original;
140 // Resize
141 if (nb_model_points > 0) {
142 bunnyModelPoints.resize(nb_model_points);
143 bunnyModelPoints_noisy.resize(nb_model_points);
144 }
145
146 vpPose ground_truth_pose, real_pose;
147 vpHomogeneousMatrix cMo_estimated;
148 ground_truth_pose.addPoints(bunnyModelPoints);
149 real_pose.addPoints(bunnyModelPoints_noisy);
150 real_pose.computePose(vpPose::DEMENTHON_LAGRANGE_VIRTUAL_VS, cMo_estimated);
151 double r_vvs = ground_truth_pose.computeResidual(cMo_estimated);
152
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;
155
156 size_t nbOutliers = (size_t)(0.35 * bunnyModelPoints_noisy.size());
157 vpGaussRand noise(0.01, 0.008);
158 // Vector that indicates if the point is an outlier or not
159 std::vector<bool> vectorOfOutlierFlags(bunnyModelPoints_noisy.size(), false);
160 // Generate outliers points
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;
165 }
166
167 if (test_duplicate) {
168 // Add some duplicate points
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);
175 }
176 }
177
178 if (test_degenerate) {
179 // Add some degenerate points
180 size_t nbDegeneratePoints = 100;
181 double degenerate_tolerence = 9.999e-7; // 1e-6 is used in the code to
182 // detect if a point is degenerate
183 // or not
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];
188
189 // Object point is degenerate
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);
193
194 // Add duplicate 3D points
195 listOfDegeneratePoints.push_back(degeneratePoint);
196
197 // Image point is degenerate
198 index = (size_t)rand() % bunnyModelPoints_noisy.size();
199 degeneratePoint = bunnyModelPoints_noisy[index];
200
201 degeneratePoint.set_x(degeneratePoint.get_x() + degenerate_tolerence);
202 degeneratePoint.set_y(degeneratePoint.get_y() - degenerate_tolerence);
203
204 // Add duplicate 2D points
205 listOfDegeneratePoints.push_back(degeneratePoint);
206 }
207
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);
212 }
213 }
214
215 // Shuffle the data vector
216 std::vector<size_t> vectorOfIndex(bunnyModelPoints_noisy.size());
217 for (size_t i = 0; i < vectorOfIndex.size(); i++) {
218 vectorOfIndex[i] = i;
219 }
220
221 // std::random_shuffle(vectorOfIndex.begin(), vectorOfIndex.end()); // std::random_shuffle is deprecated in C++14
222 std::random_device rng;
223 std::mt19937 urng(rng());
224 std::shuffle(vectorOfIndex.begin(), vectorOfIndex.end(), urng);
225
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]);
233 }
234
235 // Add data to vpPose
236 vpPose pose;
237 vpPose pose_ransac, pose_ransac2;
238
239 vpPose pose_ransac_parallel, pose_ransac_parallel2;
240 pose_ransac_parallel.setUseParallelRansac(true);
241 pose_ransac_parallel2.setUseParallelRansac(true);
242
247 for (std::vector<vpPoint>::const_iterator it = bunnyModelPoints_noisy.begin(); it != bunnyModelPoints_noisy.end();
248 ++it) {
249 pose.addPoint(*it);
250 }
251 // Test addPoints
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);
256
257 // Print the number of points in the final data vector
258 std::cout << "\nNumber of model points in the noisy data vector: " << bunnyModelPoints_noisy.size() << " points."
259 << std::endl
260 << std::endl;
261
262 unsigned int nbInlierToReachConsensus = (unsigned int)(60.0 * (double)(bunnyModelPoints_noisy.size()) / 100.0);
263 double threshold = 0.001;
264
265 // RANSAC with 1000 iterations
266 pose_ransac.setRansacNbInliersToReachConsensus(nbInlierToReachConsensus);
267 pose_ransac.setRansacThreshold(threshold);
268 pose_ransac.setRansacMaxTrials(1000);
269 pose_ransac_parallel.setRansacNbInliersToReachConsensus(nbInlierToReachConsensus);
270 pose_ransac_parallel.setRansacThreshold(threshold);
271 pose_ransac_parallel.setRansacMaxTrials(1000);
272
273 pose_ransac_parallel2.setRansacNbInliersToReachConsensus(nbInlierToReachConsensus);
274 pose_ransac_parallel2.setRansacThreshold(threshold);
275 pose_ransac_parallel2.setRansacMaxTrials(vpPose::computeRansacIterations(0.99, 0.4, 4, -1));
276
277 // RANSAC with p=0.99, epsilon=0.4
278 pose_ransac2.setRansacNbInliersToReachConsensus(nbInlierToReachConsensus);
279 pose_ransac2.setRansacThreshold(threshold);
280 int ransac_iterations = vpPose::computeRansacIterations(0.99, 0.4, 4, -1);
281 pose_ransac2.setRansacMaxTrials(ransac_iterations);
282 std::cout << "Number of RANSAC iterations to ensure p=0.99 and epsilon=0.4: " << ransac_iterations << std::endl;
283
284 vpHomogeneousMatrix cMo_estimated_RANSAC;
285 vpChrono chrono_RANSAC;
286 chrono_RANSAC.start();
287 pose_ransac.computePose(vpPose::RANSAC, cMo_estimated_RANSAC);
288 chrono_RANSAC.stop();
289
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;
292
293 double r_RANSAC_estimated = ground_truth_pose.computeResidual(cMo_estimated_RANSAC);
294 std::cout << "Corresponding residual (1000 iterations): " << r_RANSAC_estimated << std::endl;
295
296 vpHomogeneousMatrix cMo_estimated_RANSAC_2;
297 chrono_RANSAC.start();
298 pose_ransac2.computePose(vpPose::RANSAC, cMo_estimated_RANSAC_2);
299 chrono_RANSAC.stop();
300
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;
304
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;
307
309 std::cout << "\ncMo estimated with only VVS on noisy data:\n" << cMo_estimated << std::endl;
310
311 double r_estimated = ground_truth_pose.computeResidual(cMo_estimated);
312 std::cout << "Corresponding residual: " << r_estimated << std::endl;
313
314 vpHomogeneousMatrix cMo_estimated_RANSAC_parallel;
315 vpChrono chrono_RANSAC_parallel;
316 chrono_RANSAC_parallel.start();
317 pose_ransac_parallel.computePose(vpPose::RANSAC, cMo_estimated_RANSAC_parallel);
318 chrono_RANSAC_parallel.stop();
319
320 std::cout << "\ncMo estimated with parallel RANSAC (1000 iterations) on "
321 "noisy data:\n"
322 << cMo_estimated_RANSAC_parallel << std::endl;
323 std::cout << "Computation time: " << chrono_RANSAC_parallel.getDurationMs() << " ms" << std::endl;
324
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;
327
328 vpHomogeneousMatrix cMo_estimated_RANSAC_parallel2;
329 vpChrono chrono_RANSAC_parallel2;
330 chrono_RANSAC_parallel2.start();
331 pose_ransac_parallel2.computePose(vpPose::RANSAC, cMo_estimated_RANSAC_parallel2);
332 chrono_RANSAC_parallel2.stop();
333
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;
337
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
340 << std::endl;
341
342 // Check inlier index
343 std::vector<unsigned int> vectorOfFoundInlierIndex = pose_ransac.getRansacInlierIndex();
344 int nbInlierIndexOk = checkInlierIndex(vectorOfFoundInlierIndex, vectorOfOutlierFlags);
345
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;
349
350 // Check inlier points returned
351 std::vector<vpPoint> vectorOfFoundInlierPoints = pose_ransac.getRansacInliers();
352
353 if (vectorOfFoundInlierPoints.size() != vectorOfFoundInlierIndex.size()) {
354 std::cerr << "The number of inlier index is different from the number of "
355 "inlier points!"
356 << std::endl;
357 return false;
358 }
359 if (!checkInlierPoints(vectorOfFoundInlierPoints, vectorOfFoundInlierIndex, bunnyModelPoints_noisy)) {
360 return false;
361 }
362
363 // Check for RANSAC with p=0.99, epsilon=0.4
364 // Check inlier index
365 std::cout << "\nCheck for RANSAC iterations: " << ransac_iterations << std::endl;
366 std::vector<unsigned int> vectorOfFoundInlierIndex_2 = pose_ransac2.getRansacInlierIndex();
367 nbInlierIndexOk = checkInlierIndex(vectorOfFoundInlierIndex_2, vectorOfOutlierFlags);
368
369 std::cout << "There are " << nbInlierIndexOk << " true inliers found, " << vectorOfFoundInlierIndex_2.size()
370 << " inliers returned and " << nbTrueInlierIndex << " true inliers." << std::endl;
371
372 // Check inlier points returned
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 "
376 "inlier points!"
377 << std::endl;
378 return false;
379 }
380 if (!checkInlierPoints(vectorOfFoundInlierPoints_2, vectorOfFoundInlierIndex_2, bunnyModelPoints_noisy)) {
381 return false;
382 }
383
384 // Check for parallel RANSAC
385 // Check inlier index
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);
389
390 std::cout << "There are " << nbInlierIndexOk << " true inliers found, " << vectorOfFoundInlierIndex_parallel.size()
391 << " inliers returned and " << nbTrueInlierIndex << " true inliers." << std::endl;
392
393 // Check inlier points returned
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 "
397 "of inlier points!"
398 << std::endl;
399 return false;
400 }
401 if (!checkInlierPoints(vectorOfFoundInlierPoints_parallel, vectorOfFoundInlierIndex_parallel,
402 bunnyModelPoints_noisy)) {
403 return false;
404 }
405
406 // Check for parallel RANSAC 2
407 // Check inlier index
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);
411
412 std::cout << "There are " << nbInlierIndexOk << " true inliers found, " << vectorOfFoundInlierIndex_parallel2.size()
413 << " inliers returned and " << nbTrueInlierIndex << " true inliers." << std::endl;
414
415 // Check inlier points returned
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 "
419 "of inlier points!"
420 << std::endl;
421 return false;
422 }
423 if (!checkInlierPoints(vectorOfFoundInlierPoints_parallel2, vectorOfFoundInlierIndex_parallel2,
424 bunnyModelPoints_noisy)) {
425 return false;
426 }
427
428 if (r_RANSAC_estimated > threshold /*|| r_RANSAC_estimated_2 > 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;
432 return false;
433 } else {
434 if (r_RANSAC_estimated_parallel > threshold) {
435 std::cerr << "The pose estimated with the parallel RANSAC method is "
436 "badly estimated!"
437 << std::endl;
438 std::cerr << "r_RANSAC_estimated_parallel=" << r_RANSAC_estimated_parallel << std::endl;
439 std::cerr << "threshold=" << threshold << std::endl;
440 return false;
441 }
442 std::cout << "The pose estimated with the RANSAC method is well estimated!" << std::endl;
443 }
444
445 return true;
446}
447#endif
448} // namespace
449
450TEST_CASE("Print RANSAC number of iterations", "[ransac_pose]")
451{
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};
454
455 // Format output
456 const std::string spacing = " ";
457
458 std::cout << spacing << " outliers percentage\n"
459 << "nb pts\\";
460 for (int cpt2 = 0; cpt2 < 7; cpt2++) {
461 std::cout << std::setfill(' ') << std::setw(5) << epsilon[cpt2] << " ";
462 }
463 std::cout << std::endl;
464
465 std::cout << std::setfill(' ') << std::setw(7) << "+";
466 for (int cpt2 = 0; cpt2 < 6; cpt2++) {
467 std::cout << std::setw(7) << "-------";
468 }
469 std::cout << std::endl;
470
471 for (int cpt1 = 0; cpt1 < 7; cpt1++) {
472 std::cout << std::setfill(' ') << std::setw(6) << sample_sizes[cpt1] << "|";
473
474 for (int cpt2 = 0; cpt2 < 7; cpt2++) {
475 int ransac_iters = vpPose::computeRansacIterations(0.99, epsilon[cpt2], sample_sizes[cpt1], -1);
476 std::cout << std::setfill(' ') << std::setw(6) << ransac_iters;
477 }
478 std::cout << std::endl;
479 }
480 std::cout << std::endl;
481}
482
483#if (VISP_HAVE_DATASET_VERSION >= 0x030300)
484TEST_CASE("RANSAC pose estimation tests", "[ransac_pose]")
485{
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};
489
490 std::string visp_input_images = vpIoTools::getViSPImagesDataPath();
491 std::string model_filename = vpIoTools::createFilePath(visp_input_images, "3dmodel/bunny/bunny.xyz");
492 CHECK(vpIoTools::checkFilename(model_filename));
493
494 std::vector<vpPoint> bunnyModelPoints, bunnyModelPoints_noisy_original;
495 readBunnyModelPoints(model_filename, bunnyModelPoints, bunnyModelPoints_noisy_original);
496 CHECK(bunnyModelPoints.size() == bunnyModelPoints_noisy_original.size());
497
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;
502 } else {
503 std::cout << "Test on " << model_sizes[i] << " model points." << std::endl;
504 }
505 std::cout << "Test duplicate: " << duplicates[i] << " ; Test degenerate: " << degenerates[i] << std::endl;
506
507 CHECK(testRansac(bunnyModelPoints, bunnyModelPoints_noisy_original, model_sizes[i], duplicates[i], degenerates[i]));
508 }
509}
510#endif
511
512int main(int argc, char *argv[])
513{
514#if defined(__mips__) || defined(__mips) || defined(mips) || defined(__MIPS__)
515 // To avoid Debian test timeout
516 return EXIT_SUCCESS;
517#endif
518
519#if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
520
521 Catch::Session session; // There must be exactly one instance
522
523 // Let Catch (using Clara) parse the command line
524 session.applyCommandLine(argc, argv);
525
526 int numFailed = session.run();
527
528 // numFailed is clamped to 255 as some unices only use the lower 8 bits.
529 // This clamping has already been applied, so just return it here
530 // You can also do any post run clean-up here
531 return numFailed;
532#else
533 std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
534 return EXIT_SUCCESS;
535#endif
536}
537#else
538int main() { return EXIT_SUCCESS; }
539#endif
void start(bool reset=true)
Definition vpTime.cpp:397
void stop()
Definition vpTime.cpp:412
double getDurationMs()
Definition vpTime.cpp:386
Class for generating random number with normal probability density.
Implementation of an homogeneous matrix and operations on such kind of matrices.
static std::string getViSPImagesDataPath()
static bool checkFilename(const std::string &filename)
static std::string createFilePath(const std::string &parent, const std::string &child)
static double rad(double deg)
Definition vpMath.h:116
static bool equal(double x, double y, double threshold=0.001)
Definition vpMath.h:369
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition vpPoint.h:77
double get_oX() const
Get the point oX coordinate in the object frame.
Definition vpPoint.cpp:458
void set_x(double x)
Set the point x coordinate in the image plane.
Definition vpPoint.cpp:508
double get_y() const
Get the point y coordinate in the image plane.
Definition vpPoint.cpp:469
double get_oZ() const
Get the point oZ coordinate in the object frame.
Definition vpPoint.cpp:462
void set_oY(double oY)
Set the point oY coordinate in the object frame.
Definition vpPoint.cpp:501
double get_x() const
Get the point x coordinate in the image plane.
Definition vpPoint.cpp:467
void set_oZ(double oZ)
Set the point oZ coordinate in the object frame.
Definition vpPoint.cpp:503
void set_oX(double oX)
Set the point oX coordinate in the object frame.
Definition vpPoint.cpp:499
double get_oY() const
Get the point oY coordinate in the object frame.
Definition vpPoint.cpp:460
void set_y(double y)
Set the point y coordinate in the image plane.
Definition vpPoint.cpp:510
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition vpPose.h:81
void setRansacMaxTrials(const int &rM)
Definition vpPose.h:256
static int computeRansacIterations(double probability, double epsilon, const int sampleSize=4, int maxIterations=2000)
void addPoint(const vpPoint &P)
Definition vpPose.cpp:140
void setRansacNbInliersToReachConsensus(const unsigned int &nbC)
Definition vpPose.h:245
std::vector< unsigned int > getRansacInlierIndex() const
Definition vpPose.h:258
@ RANSAC
Definition vpPose.h:91
@ DEMENTHON_LAGRANGE_VIRTUAL_VS
Definition vpPose.h:102
std::vector< vpPoint > getRansacInliers() const
Definition vpPose.h:259
void addPoints(const std::vector< vpPoint > &lP)
Definition vpPose.cpp:155
double computeResidual(const vpHomogeneousMatrix &cMo) const
Compute and return the sum of squared residuals expressed in meter^2 for the pose matrix cMo.
Definition vpPose.cpp:369
void setRansacFilterFlag(const RANSAC_FILTER_FLAGS &flag)
Definition vpPose.h:298
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition vpPose.cpp:469
@ PREFILTER_DEGENERATE_POINTS
Definition vpPose.h:110
void setUseParallelRansac(bool use)
Definition vpPose.h:329
void setRansacThreshold(const double &t)
Definition vpPose.h:246
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.