#include <visp3/core/vpConfig.h>
#ifdef VISP_HAVE_CATCH2
#define CATCH_CONFIG_RUNNER
#include <catch.hpp>
#include <algorithm>
#include <iomanip>
#include <map>
#include <visp3/core/vpGaussRand.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpPoint.h>
#include <visp3/vision/vpPose.h>
#ifdef ENABLE_VISP_NAMESPACE
#endif
namespace
{
#if (VISP_HAVE_DATASET_VERSION >= 0x030300)
{
}
int checkInlierIndex(const std::vector<unsigned int> &vectorOfFoundInlierIndex,
const std::vector<bool> &vectorOfOutlierFlags)
{
int nbInlierIndexOk = 0;
for (std::vector<unsigned int>::const_iterator it = vectorOfFoundInlierIndex.begin();
it != vectorOfFoundInlierIndex.end(); ++it) {
if (!vectorOfOutlierFlags[*it]) {
nbInlierIndexOk++;
}
}
return nbInlierIndexOk;
}
bool checkInlierPoints(const std::vector<vpPoint> &vectorOfFoundInlierPoints,
const std::vector<unsigned int> &vectorOfFoundInlierIndex,
const std::vector<vpPoint> &bunnyModelPoints_noisy)
{
for (size_t i = 0; i < vectorOfFoundInlierPoints.size(); i++) {
if (!samePoints(vectorOfFoundInlierPoints[i], bunnyModelPoints_noisy[vectorOfFoundInlierIndex[i]])) {
std::cerr << "Problem with the inlier index and the corresponding "
"inlier point!"
<< std::endl;
std::cerr << "Returned inliers: oX=" << std::setprecision(std::numeric_limits<double>::max_digits10)
<< vectorOfFoundInlierPoints[i].get_oX() << ", oY=" << vectorOfFoundInlierPoints[i].get_oY()
<< ", oZ=" << vectorOfFoundInlierPoints[i].get_oZ() << " ; x=" << vectorOfFoundInlierPoints[i].get_x()
<< ", y=" << vectorOfFoundInlierPoints[i].get_y() << std::endl;
const vpPoint &pt = bunnyModelPoints_noisy[vectorOfFoundInlierIndex[i]];
std::cerr <<
"Object points: oX=" << std::setprecision(std::numeric_limits<double>::max_digits10) << pt.
get_oX()
<< std::endl;
return false;
}
}
return true;
}
void readBunnyModelPoints(const std::string &filename, std::vector<vpPoint> &bunnyModelPoints,
std::vector<vpPoint> &bunnyModelPoints_noisy)
{
std::ifstream file(filename);
if (!file.is_open()) {
return;
}
double oX = 0, oY = 0, oZ = 0;
while (file >> oX >> oY >> oZ) {
bunnyModelPoints.push_back(pt);
bunnyModelPoints_noisy.push_back(pt);
}
std::cout << "The raw model contains " << bunnyModelPoints.size() << " points." << std::endl;
std::cout << "cMo_groundTruth=\n" << cMo_groundTruth << std::endl << std::endl;
}
bool testRansac(const std::vector<vpPoint> &bunnyModelPoints_original,
const std::vector<vpPoint> &bunnyModelPoints_noisy_original, size_t nb_model_points,
bool test_duplicate, bool test_degenerate)
{
std::vector<vpPoint> bunnyModelPoints = bunnyModelPoints_original;
std::vector<vpPoint> bunnyModelPoints_noisy = bunnyModelPoints_noisy_original;
if (nb_model_points > 0) {
bunnyModelPoints.
resize(nb_model_points);
bunnyModelPoints_noisy.resize(nb_model_points);
}
vpPose ground_truth_pose, real_pose;
ground_truth_pose.
addPoints(bunnyModelPoints);
std::cout << "\ncMo estimated using VVS on data with small gaussian noise:\n" << cMo_estimated << std::endl;
std::cout << "Corresponding residual: " << r_vvs << std::endl;
size_t nbOutliers = (size_t)(0.35 * bunnyModelPoints_noisy.size());
std::vector<bool> vectorOfOutlierFlags(bunnyModelPoints_noisy.size(), false);
for (size_t i = 0; i < nbOutliers; i++) {
bunnyModelPoints_noisy[i].set_x(bunnyModelPoints_noisy[i].get_x() + noise());
bunnyModelPoints_noisy[i].set_y(bunnyModelPoints_noisy[i].get_y() + noise());
vectorOfOutlierFlags[i] = true;
}
if (test_duplicate) {
size_t nbDuplicatePoints = 100;
for (size_t i = 0; i < nbDuplicatePoints; i++) {
size_t index = (size_t)rand() % bunnyModelPoints_noisy.size();
vpPoint duplicatePoint = bunnyModelPoints_noisy[index];
bunnyModelPoints_noisy.push_back(duplicatePoint);
vectorOfOutlierFlags.push_back(true);
}
}
if (test_degenerate) {
size_t nbDegeneratePoints = 100;
double degenerate_tolerence = 9.999e-7;
std::vector<vpPoint> listOfDegeneratePoints;
for (size_t i = 0; i < nbDegeneratePoints; i++) {
size_t index = (size_t)rand() % bunnyModelPoints_noisy.size();
vpPoint degeneratePoint = bunnyModelPoints_noisy[index];
degeneratePoint.
set_oX(degeneratePoint.
get_oX() + degenerate_tolerence);
degeneratePoint.
set_oY(degeneratePoint.
get_oY() + degenerate_tolerence);
degeneratePoint.
set_oZ(degeneratePoint.
get_oZ() - degenerate_tolerence);
listOfDegeneratePoints.push_back(degeneratePoint);
index = (size_t)rand() % bunnyModelPoints_noisy.size();
degeneratePoint = bunnyModelPoints_noisy[index];
degeneratePoint.
set_x(degeneratePoint.
get_x() + degenerate_tolerence);
degeneratePoint.
set_y(degeneratePoint.
get_y() - degenerate_tolerence);
listOfDegeneratePoints.push_back(degeneratePoint);
}
for (std::vector<vpPoint>::const_iterator it_degenerate = listOfDegeneratePoints.begin();
it_degenerate != listOfDegeneratePoints.end(); ++it_degenerate) {
bunnyModelPoints_noisy.push_back(*it_degenerate);
vectorOfOutlierFlags.push_back(true);
}
}
std::vector<size_t> vectorOfIndex(bunnyModelPoints_noisy.size());
for (size_t i = 0; i < vectorOfIndex.size(); i++) {
vectorOfIndex[i] = i;
}
std::random_device rng;
std::mt19937 urng(rng());
std::shuffle(vectorOfIndex.begin(), vectorOfIndex.end(), urng);
std::vector<vpPoint> bunnyModelPoints_noisy_tmp = bunnyModelPoints_noisy;
bunnyModelPoints_noisy.clear();
std::vector<bool> vectorOfOutlierFlags_tmp = vectorOfOutlierFlags;
vectorOfOutlierFlags.clear();
for (std::vector<size_t>::const_iterator it = vectorOfIndex.begin(); it != vectorOfIndex.end(); ++it) {
bunnyModelPoints_noisy.push_back(bunnyModelPoints_noisy_tmp[*it]);
vectorOfOutlierFlags.push_back(vectorOfOutlierFlags_tmp[*it]);
}
vpPose pose_ransac, pose_ransac2;
vpPose pose_ransac_parallel, pose_ransac_parallel2;
for (std::vector<vpPoint>::const_iterator it = bunnyModelPoints_noisy.begin(); it != bunnyModelPoints_noisy.end();
++it) {
}
pose_ransac.
addPoints(bunnyModelPoints_noisy);
pose_ransac2.
addPoints(bunnyModelPoints_noisy);
pose_ransac_parallel.
addPoints(bunnyModelPoints_noisy);
pose_ransac_parallel2.
addPoints(bunnyModelPoints_noisy);
std::cout << "\nNumber of model points in the noisy data vector: " << bunnyModelPoints_noisy.size() << " points."
<< std::endl
<< std::endl;
unsigned int nbInlierToReachConsensus = (unsigned int)(60.0 * (double)(bunnyModelPoints_noisy.size()) / 100.0);
double threshold = 0.001;
std::cout << "Number of RANSAC iterations to ensure p=0.99 and epsilon=0.4: " << ransac_iterations << std::endl;
std::cout << "\ncMo estimated with RANSAC (1000 iterations) on noisy data:\n" << cMo_estimated_RANSAC << std::endl;
std::cout <<
"Computation time: " << chrono_RANSAC.
getDurationMs() <<
" ms" << std::endl;
double r_RANSAC_estimated = ground_truth_pose.
computeResidual(cMo_estimated_RANSAC);
std::cout << "Corresponding residual (1000 iterations): " << r_RANSAC_estimated << std::endl;
std::cout << "\ncMo estimated with RANSAC (" << ransac_iterations << " iterations) on noisy data:\n"
<< cMo_estimated_RANSAC_2 << std::endl;
std::cout <<
"Computation time: " << chrono_RANSAC.
getDurationMs() <<
" ms" << std::endl;
double r_RANSAC_estimated_2 = ground_truth_pose.
computeResidual(cMo_estimated_RANSAC_2);
std::cout << "Corresponding residual (" << ransac_iterations << " iterations): " << r_RANSAC_estimated_2 << std::endl;
std::cout << "\ncMo estimated with only VVS on noisy data:\n" << cMo_estimated << std::endl;
std::cout << "Corresponding residual: " << r_estimated << std::endl;
chrono_RANSAC_parallel.
start();
chrono_RANSAC_parallel.
stop();
std::cout << "\ncMo estimated with parallel RANSAC (1000 iterations) on "
"noisy data:\n"
<< cMo_estimated_RANSAC_parallel << std::endl;
std::cout <<
"Computation time: " << chrono_RANSAC_parallel.
getDurationMs() <<
" ms" << std::endl;
double r_RANSAC_estimated_parallel = ground_truth_pose.
computeResidual(cMo_estimated_RANSAC_parallel);
std::cout << "Corresponding residual (1000 iterations): " << r_RANSAC_estimated_parallel << std::endl;
chrono_RANSAC_parallel2.
start();
chrono_RANSAC_parallel2.
stop();
std::cout << "\ncMo estimated with parallel RANSAC (" << ransac_iterations << " iterations) on noisy data:\n"
<< cMo_estimated_RANSAC_parallel2 << std::endl;
std::cout <<
"Computation time: " << chrono_RANSAC_parallel2.
getDurationMs() <<
" ms" << std::endl;
double r_RANSAC_estimated_parallel2 = ground_truth_pose.
computeResidual(cMo_estimated_RANSAC_parallel2);
std::cout << "Corresponding residual (" << ransac_iterations << " iterations): " << r_RANSAC_estimated_parallel2
<< std::endl;
int nbInlierIndexOk = checkInlierIndex(vectorOfFoundInlierIndex, vectorOfOutlierFlags);
int nbTrueInlierIndex = (int)std::count(vectorOfOutlierFlags.begin(), vectorOfOutlierFlags.end(), false);
std::cout << "\nThere are " << nbInlierIndexOk << " true inliers found, " << vectorOfFoundInlierIndex.size()
<< " inliers returned and " << nbTrueInlierIndex << " true inliers." << std::endl;
std::vector<vpPoint> vectorOfFoundInlierPoints = pose_ransac.
getRansacInliers();
if (vectorOfFoundInlierPoints.size() != vectorOfFoundInlierIndex.size()) {
std::cerr << "The number of inlier index is different from the number of "
"inlier points!"
<< std::endl;
return false;
}
if (!checkInlierPoints(vectorOfFoundInlierPoints, vectorOfFoundInlierIndex, bunnyModelPoints_noisy)) {
return false;
}
std::cout << "\nCheck for RANSAC iterations: " << ransac_iterations << std::endl;
nbInlierIndexOk = checkInlierIndex(vectorOfFoundInlierIndex_2, vectorOfOutlierFlags);
std::cout << "There are " << nbInlierIndexOk << " true inliers found, " << vectorOfFoundInlierIndex_2.size()
<< " inliers returned and " << nbTrueInlierIndex << " true inliers." << std::endl;
std::vector<vpPoint> vectorOfFoundInlierPoints_2 = pose_ransac2.
getRansacInliers();
if (vectorOfFoundInlierPoints_2.size() != vectorOfFoundInlierIndex_2.size()) {
std::cerr << "The number of inlier index is different from the number of "
"inlier points!"
<< std::endl;
return false;
}
if (!checkInlierPoints(vectorOfFoundInlierPoints_2, vectorOfFoundInlierIndex_2, bunnyModelPoints_noisy)) {
return false;
}
std::cout << "\nCheck for parallel RANSAC (1000 iterations)" << std::endl;
std::vector<unsigned int> vectorOfFoundInlierIndex_parallel = pose_ransac_parallel.
getRansacInlierIndex();
nbInlierIndexOk = checkInlierIndex(vectorOfFoundInlierIndex_parallel, vectorOfOutlierFlags);
std::cout << "There are " << nbInlierIndexOk << " true inliers found, " << vectorOfFoundInlierIndex_parallel.size()
<< " inliers returned and " << nbTrueInlierIndex << " true inliers." << std::endl;
std::vector<vpPoint> vectorOfFoundInlierPoints_parallel = pose_ransac_parallel.
getRansacInliers();
if (vectorOfFoundInlierPoints_parallel.size() != vectorOfFoundInlierIndex_parallel.size()) {
std::cerr << "The number of inlier index is different from the number "
"of inlier points!"
<< std::endl;
return false;
}
if (!checkInlierPoints(vectorOfFoundInlierPoints_parallel, vectorOfFoundInlierIndex_parallel,
bunnyModelPoints_noisy)) {
return false;
}
std::cout << "\nCheck for parallel RANSAC (" << ransac_iterations << " iterations)" << std::endl;
std::vector<unsigned int> vectorOfFoundInlierIndex_parallel2 = pose_ransac_parallel2.
getRansacInlierIndex();
nbInlierIndexOk = checkInlierIndex(vectorOfFoundInlierIndex_parallel2, vectorOfOutlierFlags);
std::cout << "There are " << nbInlierIndexOk << " true inliers found, " << vectorOfFoundInlierIndex_parallel2.size()
<< " inliers returned and " << nbTrueInlierIndex << " true inliers." << std::endl;
std::vector<vpPoint> vectorOfFoundInlierPoints_parallel2 = pose_ransac_parallel2.
getRansacInliers();
if (vectorOfFoundInlierPoints_parallel2.size() != vectorOfFoundInlierIndex_parallel2.size()) {
std::cerr << "The number of inlier index is different from the number "
"of inlier points!"
<< std::endl;
return false;
}
if (!checkInlierPoints(vectorOfFoundInlierPoints_parallel2, vectorOfFoundInlierIndex_parallel2,
bunnyModelPoints_noisy)) {
return false;
}
if (r_RANSAC_estimated > threshold ) {
std::cerr << "The pose estimated with the RANSAC method is badly estimated!" << std::endl;
std::cerr << "r_RANSAC_estimated=" << r_RANSAC_estimated << std::endl;
std::cerr << "threshold=" << threshold << std::endl;
return false;
}
else {
if (r_RANSAC_estimated_parallel > threshold) {
std::cerr << "The pose estimated with the parallel RANSAC method is "
"badly estimated!"
<< std::endl;
std::cerr << "r_RANSAC_estimated_parallel=" << r_RANSAC_estimated_parallel << std::endl;
std::cerr << "threshold=" << threshold << std::endl;
return false;
}
std::cout << "The pose estimated with the RANSAC method is well estimated!" << std::endl;
}
return true;
}
#endif
}
TEST_CASE("Print RANSAC number of iterations", "[ransac_pose]")
{
const int sample_sizes[] = { 2, 3, 4, 5, 6, 7, 8 };
const double epsilon[] = { 0.05, 0.1, 0.2, 0.25, 0.3, 0.4, 0.5 };
const std::string spacing = " ";
std::cout << spacing << " outliers percentage\n"
<< "nb pts\\";
for (int cpt2 = 0; cpt2 < 7; cpt2++) {
std::cout << std::setfill(' ') << std::setw(5) << epsilon[cpt2] << " ";
}
std::cout << std::endl;
std::cout << std::setfill(' ') << std::setw(7) << "+";
for (int cpt2 = 0; cpt2 < 6; cpt2++) {
std::cout << std::setw(7) << "-------";
}
std::cout << std::endl;
for (int cpt1 = 0; cpt1 < 7; cpt1++) {
std::cout << std::setfill(' ') << std::setw(6) << sample_sizes[cpt1] << "|";
for (int cpt2 = 0; cpt2 < 7; cpt2++) {
std::cout << std::setfill(' ') << std::setw(6) << ransac_iters;
}
std::cout << std::endl;
}
std::cout << std::endl;
}
#if (VISP_HAVE_DATASET_VERSION >= 0x030300)
TEST_CASE("RANSAC pose estimation tests", "[ransac_pose]")
{
const std::vector<size_t> model_sizes = { 10, 20, 50, 100, 200, 500, 1000, 0, 0 };
const std::vector<bool> duplicates = { false, false, false, false, false, false, false, false, true };
const std::vector<bool> degenerates = { false, false, false, false, false, false, true, true, true };
std::vector<vpPoint> bunnyModelPoints, bunnyModelPoints_noisy_original;
readBunnyModelPoints(model_filename, bunnyModelPoints, bunnyModelPoints_noisy_original);
CHECK(bunnyModelPoints.size() == bunnyModelPoints_noisy_original.size());
for (size_t i = 0; i < model_sizes.size(); i++) {
std::cout << "\n\n===============================================================================" << std::endl;
if (model_sizes[i] == 0) {
std::cout << "Test on " << bunnyModelPoints_noisy_original.size() << " model points." << std::endl;
}
else {
std::cout << "Test on " << model_sizes[i] << " model points." << std::endl;
}
std::cout << "Test duplicate: " << duplicates[i] << " ; Test degenerate: " << degenerates[i] << std::endl;
CHECK(testRansac(bunnyModelPoints, bunnyModelPoints_noisy_original, model_sizes[i], duplicates[i], degenerates[i]));
}
}
#endif
int main(int argc, char *argv[])
{
#if defined(__mips__) || defined(__mips) || defined(mips) || defined(__MIPS__)
return EXIT_SUCCESS;
#endif
#if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
Catch::Session session;
session.applyCommandLine(argc, argv);
int numFailed = session.run();
return numFailed;
#else
std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
return EXIT_SUCCESS;
#endif
}
#else
int main() { return EXIT_SUCCESS; }
#endif
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.
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true)
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
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, FuncCheckValidityPose func=nullptr)
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
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.