From 5d49eb10b73821a3dc06a3d95baf905811edcb32 Mon Sep 17 00:00:00 2001 From: Vincent Rabaud Date: Sat, 23 Apr 2011 10:41:03 +0000 Subject: [PATCH] - generate uniformly chosen samples in a common manner: much cleaner code and linear time algorithm for sampling - make the SAC tests more robust to slight changes in implementation git-svn-id: svn+ssh://svn.pointclouds.org/pcl/trunk@681 a9d63959-f2ad-4865-b262-bf0e56cfafb6 --- .../impl/sac_model_circle.hpp | 71 +-- .../impl/sac_model_cylinder.hpp | 40 +- .../sample_consensus/impl/sac_model_line.hpp | 47 +- .../sample_consensus/impl/sac_model_plane.hpp | 70 +-- .../impl/sac_model_registration.hpp | 49 +- .../impl/sac_model_sphere.hpp | 81 +--- .../pcl/sample_consensus/model_types.h | 11 +- .../include/pcl/sample_consensus/sac_model.h | 42 +- .../pcl/sample_consensus/sac_model_circle.h | 10 +- .../pcl/sample_consensus/sac_model_cylinder.h | 10 +- .../pcl/sample_consensus/sac_model_line.h | 9 +- .../pcl/sample_consensus/sac_model_plane.h | 8 +- .../sample_consensus/sac_model_registration.h | 19 +- .../pcl/sample_consensus/sac_model_sphere.h | 10 +- test/test_sample_consensus.cpp | 437 ++++-------------- 15 files changed, 208 insertions(+), 706 deletions(-) diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle.hpp index 2df5d1d98..dbcfe501a 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle.hpp @@ -42,79 +42,22 @@ #include "pcl/common/concatenate.h" ////////////////////////////////////////////////////////////////////////// -template void -pcl::SampleConsensusModelCircle2D::getSamples (int &iterations, std::vector &samples) +template bool +pcl::SampleConsensusModelCircle2D::isSampleGood(const std::vector &samples) const { - // We're assuming that indices_ have already been set in the constructor - if (indices_->empty ()) - { - PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::getSamples] Empty set of indices given!"); - return; - } - - samples.resize (3); - double trand = indices_->size () / (RAND_MAX + 1.0); - - // Check if we have enough points - if (samples.size () > indices_->size ()) - { - PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::getSamples] Can not select %zu unique points out of %zu!", samples.size (), indices_->size ()); - // one of these will make it stop :) TODO static constant for each model that the method has to check - samples.clear (); - iterations = INT_MAX - 1; - return; - } - - // Get a random number between 1 and max_indices - int idx = (int)(rand () * trand); - // Get the index - samples[0] = (*indices_)[idx]; - - // Get a second point which is different than the first - do - { - idx = (int)(rand () * trand); - samples[1] = (*indices_)[idx]; - //iterations++; - } while (samples[1] == samples[0]); - //iterations--; - // Get the values at the two points Eigen::Array2d p0 (input_->points[samples[0]].x, input_->points[samples[0]].y); Eigen::Array2d p1 (input_->points[samples[1]].x, input_->points[samples[1]].y); + Eigen::Array2d p2 (input_->points[samples[2]].x, input_->points[samples[2]].y); // Compute the segment values (in 2d) between p1 and p0 p1 -= p0; + // Compute the segment values (in 2d) between p2 and p0 + p2 -= p0; - Eigen::Array2d dy1dy2; - int iter = 0; - do - { - // Get the third point, different from the first two - do - { - idx = (int)(rand () * trand); - samples[2] = (*indices_)[idx]; - //iterations++; - } while ( (samples[2] == samples[1]) || (samples[2] == samples[0]) ); - //iterations--; - - Eigen::Array2d p2 (input_->points[samples[2]].x, input_->points[samples[2]].y); - - // Compute the segment values (in 2d) between p2 and p0 - p2 -= p0; + Eigen::Array2d dy1dy2 = p1 / p2; - dy1dy2 = p1 / p2; - ++iter; - if (iter > MAX_ITERATIONS_COLLINEAR ) - { - PCL_DEBUG ("[pcl::SampleConsensusModelCircle2D::getSamples] WARNING: Could not select 3 non collinear points in %d iterations!", MAX_ITERATIONS_COLLINEAR); - break; - } - //iterations++; - } - while ( (dy1dy2[0] == dy1dy2[1]) ); - //iterations--; + return (dy1dy2[0] != dy1dy2[1]); } ////////////////////////////////////////////////////////////////////////// diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_cylinder.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_cylinder.hpp index 82d5d150f..c62506950 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_cylinder.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_cylinder.hpp @@ -44,46 +44,12 @@ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** \brief Get 2 random points with their normals as data samples and return them as point indices. - * \param iterations the internal number of iterations used by SAC methods * \param samples the resultant model samples - * \note assumes unique points! */ -template void -pcl::SampleConsensusModelCylinder::getSamples (int &iterations, std::vector &samples) +template bool +pcl::SampleConsensusModelCylinder::isSampleGood(const std::vector &samples) const { - // We're assuming that indices_ have already been set in the constructor - if (indices_->empty ()) - { - PCL_ERROR ("[pcl::SampleConsensusModelCylinder::getSamples] Empty set of indices given!"); - return; - } - - samples.resize (2); - double trand = indices_->size () / (RAND_MAX + 1.0); - - // Check if we have enough points - if (samples.size () > indices_->size ()) - { - PCL_ERROR ("[pcl::SampleConsensusModelCylinder::getSamples] Can not select %zu unique points out of %zu!", samples.size (), indices_->size ()); - // one of these will make it stop :) TODO static constant for each model that the method has to check - samples.clear (); - iterations = INT_MAX - 1; - return; - } - - // Get a random number between 1 and max_indices - int idx = (int)(rand () * trand); - // Get the index - samples[0] = (*indices_)[idx]; - - // Get a second point which is different than the first - do - { - idx = (int)(rand () * trand); - samples[1] = (*indices_)[idx]; - //iterations++; - } while (samples[1] == samples[0]); - //iterations--; + return true; } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_line.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_line.hpp index f44efa604..512e13065 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_line.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_line.hpp @@ -43,51 +43,10 @@ #include "pcl/common/concatenate.h" ////////////////////////////////////////////////////////////////////////// -template void -pcl::SampleConsensusModelLine::getSamples (int &iterations, std::vector &samples) +template bool +pcl::SampleConsensusModelLine::isSampleGood(const std::vector &samples) const { - // We're assuming that indices_ have already been set in the constructor - if (indices_->empty ()) - { - PCL_ERROR ("[pcl::SampleConsensusModelLine::getSamples] Empty set of indices given!"); - return; - } - - samples.resize (2); - double trand = indices_->size () / (RAND_MAX + 1.0); - - // Check if we have enough points - if (samples.size () > indices_->size ()) - { - PCL_ERROR ("[pcl::SampleConsensusModelLine::getSamples] Can not select %zu unique points out of %zu!", samples.size (), indices_->size ()); - // one of these will make it stop :) TODO static constant for each model that the method has to check - samples.clear (); - iterations = INT_MAX - 1; - return; - } - - // Get a random number between 1 and max_indices - int idx = (int)(rand () * trand); - // Get the index - samples[0] = (*indices_)[idx]; - - // Get a second point which is different than the first - int iter = 0; - do - { - idx = (int)(rand () * trand); - samples[1] = (*indices_)[idx]; - ++iter; - - if (iter > MAX_ITERATIONS_UNIQUE ) - { - PCL_DEBUG ("[pcl::SampleConsensusModelLine::getSamples] WARNING: Could not select 2 unique points in %d iterations (%zu indices)!", MAX_ITERATIONS_UNIQUE, indices_->size ()); - break; - } - //iterations++; - } - while (samples[1] == samples[0]); - //iterations--; + return true; } ////////////////////////////////////////////////////////////////////////// diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_plane.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_plane.hpp index bcfe7ac5e..0062e9093 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_plane.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_plane.hpp @@ -44,79 +44,23 @@ #include "pcl/common/concatenate.h" ////////////////////////////////////////////////////////////////////////// -template void -pcl::SampleConsensusModelPlane::getSamples (int &iterations, std::vector &samples) +template bool +pcl::SampleConsensusModelPlane::isSampleGood(const std::vector &samples) const { - // We're assuming that indices_ have already been set in the constructor - if (indices_->empty ()) - { - PCL_ERROR ("[pcl::SampleConsensusModelPlane::getSamples] Empty set of indices given!"); - return; - } - - samples.resize (3); - double trand = indices_->size () / (RAND_MAX + 1.0); - - // Check if we have enough points - if (samples.size () > indices_->size ()) - { - PCL_ERROR ("[pcl::SampleConsensusModelPlane::getSamples] Can not select %zu unique points out of %zu!", samples.size (), indices_->size ()); - // one of these will make it stop :) TODO static constant for each model that the method has to check - samples.clear (); - iterations = INT_MAX - 1; - return; - } - - // Get a random number between 1 and max_indices - int idx = (int)(rand () * trand); - // Get the index - samples[0] = (*indices_)[idx]; - - // Get a second point which is different than the first - do - { - idx = (int)(rand () * trand); - samples[1] = (*indices_)[idx]; - //iterations++; - } while (samples[1] == samples[0]); - //iterations--; - // Get the values at the two points pcl::Array4fMapConst p0 = input_->points[samples[0]].getArray4fMap (); pcl::Array4fMapConst p1 = input_->points[samples[1]].getArray4fMap (); + pcl::Array4fMapConst p2 = input_->points[samples[2]].getArray4fMap (); // Compute the segment values (in 3d) between p1 and p0 Eigen::Array4f p1p0 = p1 - p0; - Eigen::Array4f dy1dy2; - int iter = 0; - do - { - // Get the third point, different from the first two - do - { - idx = (int)(rand () * trand); - samples[2] = (*indices_)[idx]; - //iterations++; - } while ( (samples[2] == samples[1]) || (samples[2] == samples[0]) ); - //iterations--; - - pcl::Array4fMapConst p2 = input_->points[samples[2]].getArray4fMap (); + // Compute the segment values (in 3d) between p2 and p0 + Eigen::Array4f p2p0 = p2 - p0; - // Compute the segment values (in 3d) between p2 and p0 - Eigen::Array4f p2p0 = p2 - p0; + Eigen::Array4f dy1dy2 = p1p0 / p2p0; - dy1dy2 = p1p0 / p2p0; - ++iter; - if (iter > MAX_ITERATIONS_COLLINEAR ) - { - PCL_DEBUG ("[pcl::SampleConsensusModelPlane::getSamples] WARNING: Could not select 3 non collinear points in %d iterations!", MAX_ITERATIONS_COLLINEAR); - break; - } - //iterations++; - } - while ( (dy1dy2[0] == dy1dy2[1]) && (dy1dy2[2] == dy1dy2[1]) ); - //iterations--; + return ( (dy1dy2[0] != dy1dy2[1]) || (dy1dy2[2] != dy1dy2[1]) ); } ////////////////////////////////////////////////////////////////////////// diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration.hpp index abbdafa36..4882a7260 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration.hpp @@ -42,43 +42,22 @@ #include "pcl/common/rigid_transforms.h" ////////////////////////////////////////////////////////////////////////// -template void -pcl::SampleConsensusModelRegistration::getSamples (int &iterations, std::vector &samples) +template bool +pcl::SampleConsensusModelRegistration::isSampleGood(const std::vector &samples) const { - // We're assuming that indices_ have already been set in the constructor - if (indices_->size () < 3) - { - PCL_ERROR ("[pcl::SampleConsensusModelRegistration::getSamples] Can not select %zu unique points out of %zu!", samples.size (), indices_->size ()); - // one of these will make it stop :) TODO static constant for each model that the method has to check - samples.clear (); - iterations = INT_MAX - 1; - return; - } - - samples.resize (3); - - // Get a second point which is different than the first Eigen::Array4f p1p0, p2p0, p2p1; - for(unsigned int iter = 0; iter <= MAX_ITERATIONS_COLLINEAR; ++iter) - { - // Choose thre random indices - SampleConsensusModel::drawIndexSample (samples); - - // Get the values at the points - pcl::Array4fMapConst p0 = input_->points[samples[0]].getArray4fMap (); - pcl::Array4fMapConst p1 = input_->points[samples[1]].getArray4fMap (); - pcl::Array4fMapConst p2 = input_->points[samples[2]].getArray4fMap (); - - // Compute the segment values (in 3d) between p1 and p0 - p1p0 = p1 - p0; - p2p0 = p2 - p0; - p2p1 = p2 - p1; - if ((p1p0.matrix ().squaredNorm () > sample_dist_thresh_) && (p2p0.matrix ().squaredNorm () - > sample_dist_thresh_) && (p2p1.matrix ().squaredNorm () > sample_dist_thresh_)) - return; - } - PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::getSamples] WARNING: Could not select 3 non collinear points in %d iterations!", MAX_ITERATIONS_COLLINEAR); - samples.clear (); + + // Get the values at the points + pcl::Array4fMapConst p0 = input_->points[samples[0]].getArray4fMap(); + pcl::Array4fMapConst p1 = input_->points[samples[1]].getArray4fMap(); + pcl::Array4fMapConst p2 = input_->points[samples[2]].getArray4fMap(); + + // Compute the segment values (in 3d) between p1 and p0 + p1p0 = p1 - p0; + p2p0 = p2 - p0; + p2p1 = p2 - p1; + return ((p1p0.matrix().squaredNorm() > sample_dist_thresh_) && (p2p0.matrix().squaredNorm() > sample_dist_thresh_) + && (p2p1.matrix().squaredNorm() > sample_dist_thresh_)); } ////////////////////////////////////////////////////////////////////////// diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_sphere.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_sphere.hpp index 96af461f6..c756aa4e8 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_sphere.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_sphere.hpp @@ -41,89 +41,22 @@ #include "pcl/sample_consensus/sac_model_sphere.h" ////////////////////////////////////////////////////////////////////////// -template void -pcl::SampleConsensusModelSphere::getSamples (int &iterations, std::vector &samples) +template bool +pcl::SampleConsensusModelSphere::isSampleGood(const std::vector &samples) const { - // We're assuming that indices_ have already been set in the constructor - if (indices_->empty ()) - { - PCL_ERROR ("[pcl::SampleConsensusModelSphere::getSamples] Empty set of indices given!"); - return; - } - - samples.resize (4); - double trand = indices_->size () / (RAND_MAX + 1.0); - - // Check if we have enough points - if (samples.size () > indices_->size ()) - { - PCL_ERROR ("[pcl::SampleConsensusModelSphere::getSamples] Can not select %zu unique points out of %zu!", samples.size (), indices_->size ()); - // one of these will make it stop :) TODO static constant for each model that the method has to check - samples.clear (); - iterations = INT_MAX - 1; - return; - } - - // Get a random number between 1 and max_indices - int idx = (int)(rand () * trand); - // Get the index - samples[0] = (*indices_)[idx]; - - // Get a second point which is different than the first - do - { - idx = (int)(rand () * trand); - samples[1] = (*indices_)[idx]; - //iterations++; - } while (samples[1] == samples[0]); - //iterations--; - // Get the values at the two points pcl::Array4fMapConst p0 = input_->points[samples[0]].getArray4fMap (); pcl::Array4fMapConst p1 = input_->points[samples[1]].getArray4fMap (); + pcl::Array4fMapConst p2 = input_->points[samples[2]].getArray4fMap (); // Compute the segment values (in 3d) between p1 and p0 Eigen::Array4f p1p0 = p1 - p0; + // Compute the segment values (in 3d) between p2 and p0 + Eigen::Array4f p2p0 = p2 - p0; - Eigen::Array4f dy1dy2; - int iter = 0; - do - { - // Get the third point, different from the first two - do - { - idx = (int)(rand () * trand); - samples[2] = (*indices_)[idx]; - //iterations++; - } while ( (samples[2] == samples[1]) || (samples[2] == samples[0]) ); - //iterations--; - - pcl::Array4fMapConst p2 = input_->points[samples[2]].getArray4fMap (); - - // Compute the segment values (in 3d) between p2 and p0 - Eigen::Array4f p2p0 = p2 - p0; - - dy1dy2 = p1p0 / p2p0; - ++iter; - if (iter > MAX_ITERATIONS_COLLINEAR ) - { - PCL_DEBUG ("[pcl::SampleConsensusModelSphere::getSamples] WARNING: Could not select 3 non collinear points in %d iterations!", MAX_ITERATIONS_COLLINEAR); - break; - } - //iterations++; - } - while ( (dy1dy2[0] == dy1dy2[1]) && (dy1dy2[2] == dy1dy2[1]) ); - //iterations--; - - // Need to improve this: we need 4 points, 3 non-collinear always, and the 4th should not be in the same plane as the other 3 - // otherwise we can encounter degenerate cases - do - { - samples[3] = (int)(rand () * trand); - //iterations++; - } while ( (samples[3] == samples[2]) || (samples[3] == samples[1]) || (samples[3] == samples[0]) ); - //iterations--; + Eigen::Array4f dy1dy2 = p1p0 / p2p0; + return true; } ////////////////////////////////////////////////////////////////////////// diff --git a/sample_consensus/include/pcl/sample_consensus/model_types.h b/sample_consensus/include/pcl/sample_consensus/model_types.h index 24d2c860e..3e2be64a4 100644 --- a/sample_consensus/include/pcl/sample_consensus/model_types.h +++ b/sample_consensus/include/pcl/sample_consensus/model_types.h @@ -67,9 +67,18 @@ typedef std::map::value_type SampleSizeModel; const static SampleSizeModel sample_size_pairs[] = {SampleSizeModel (pcl::SACMODEL_PLANE, 3), SampleSizeModel (pcl::SACMODEL_LINE,2), SampleSizeModel (pcl::SACMODEL_CIRCLE2D, 3), + //SampleSizeModel (pcl::SACMODEL_CIRCLE3D, 3), SampleSizeModel (pcl::SACMODEL_SPHERE, 4), SampleSizeModel (pcl::SACMODEL_CYLINDER, 2), - SampleSizeModel (pcl::SACMODEL_REGISTRATION, 3)}; + //SampleSizeModel (pcl::SACMODEL_CONE, 2), + //SampleSizeModel (pcl::SACMODEL_TORUS, 2), + //SampleSizeModel (pcl::PARALLEL_LINE, 2), + //SampleSizeModel (pcl::PERPENDICULAR_LINE, 2), + //SampleSizeModel (pcl::PARALLEL_LINES, 2), + SampleSizeModel (pcl::SACMODEL_NORMAL_PLANE, 3), + SampleSizeModel (pcl::SACMODEL_REGISTRATION, 3), + SampleSizeModel (pcl::SACMODEL_PARALLEL_PLANE, 3), + SampleSizeModel (pcl::SACMODEL_NORMAL_PARALLEL_PLANE, 3)}; namespace pcl { diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model.h b/sample_consensus/include/pcl/sample_consensus/sac_model.h index 810ba0db2..d97374811 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model.h @@ -109,8 +109,33 @@ namespace pcl * \param iterations the internal number of iterations used by SAC methods * \param samples the resultant model samples */ - virtual void - getSamples (int &iterations, std::vector &samples) = 0; + void getSamples(int &iterations, std::vector &samples) + { + // We're assuming that indices_ have already been set in the constructor + if (indices_->size () < getSampleSize()) + { + PCL_ERROR ("[pcl::SampleConsensusModel::getSamples] Can not select %zu unique points out of %zu!", + samples.size (), indices_->size ()); + // one of these will make it stop :) + samples.clear (); + iterations = INT_MAX - 1; + return; + } + + // Get a second point which is different than the first + samples.resize(getSampleSize()); + for(unsigned int iter = 0; iter < max_sample_checks_; ++iter) + { + // Choose the random indices + SampleConsensusModel::drawIndexSample (samples); + + // If it's a good sample, stop here + if (isSampleGood(samples)) + return; + } + PCL_DEBUG ("[pcl::SampleConsensusModel::getSamples] WARNING: Could not select 3 non collinear points in %d iterations!", max_sample_checks_); + samples.clear(); + } /** \brief Check whether the given index samples can form a valid model, * compute the model coefficients from these samples and store them @@ -206,6 +231,7 @@ namespace pcl for (size_t i = 0; i < cloud->points.size (); ++i) (*indices_)[i] = i; } + shuffled_indices_ = *indices_; } /** \brief Get a pointer to the input point cloud dataset. */ @@ -272,7 +298,7 @@ namespace pcl /** \brief Fills a sample array with random samples from the indices_ vector * Sure, there are some swaps in there but it is linear in the size of the sample, no stupid while loop to * compare the elements between them - * \param model_coefficients the set of model coefficients + * \param sample the set of indices of target_ to analyze */ inline void drawIndexSample (std::vector & sample) @@ -292,12 +318,22 @@ namespace pcl virtual inline bool isModelValid (const Eigen::VectorXf &model_coefficients) = 0; + /** \brief Check if a sample of indices results in a good sample of points + * indices. Pure virtual. + * \param samples the resultant index samples + */ + virtual bool + isSampleGood(const std::vector &samples) const = 0; + /** \brief A boost shared pointer to the point cloud data array. */ PointCloudConstPtr input_; /** \brief A pointer to the vector of point indices to use. */ IndicesPtr indices_; + /** The maximum number of samples to try until we get a good one */ + static const unsigned int max_sample_checks_ = 1000; + /** \brief The minimum and maximum radius limits for the model. * Applicable to all models that estimate a radius. */ diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_circle.h b/sample_consensus/include/pcl/sample_consensus/sac_model_circle.h index dca43af8f..f36b12608 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_circle.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_circle.h @@ -137,6 +137,13 @@ namespace pcl bool isModelValid (const Eigen::VectorXf &model_coefficients); + /** \brief Check if a sample of indices results in a good sample of points + * indices. Pure virtual. + * \param samples the resultant index samples + */ + bool + isSampleGood(const std::vector &samples) const; + private: /** \brief Temporary boost mutex for \a tmp_inliers_ */ boost::mutex tmp_mutex_; @@ -144,9 +151,6 @@ namespace pcl /** \brief Temporary pointer to a list of given indices for optimizeModelCoefficients () */ const std::vector *tmp_inliers_; - /** \brief Define the maximum number of iterations for collinearity checks */ - const static int MAX_ITERATIONS_COLLINEAR = 1000; - /** \brief Cost function to be minimized * \param p a pointer to our data structure array * \param m the number of functions diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_cylinder.h b/sample_consensus/include/pcl/sample_consensus/sac_model_cylinder.h index 96e6094d0..2a41f73ce 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_cylinder.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_cylinder.h @@ -196,6 +196,13 @@ namespace pcl */ bool isModelValid (const Eigen::VectorXf &model_coefficients); + /** \brief Check if a sample of indices results in a good sample of points + * indices. Pure virtual. + * \param samples the resultant index samples + */ + bool + isSampleGood(const std::vector &samples) const; + private: /** \brief The axis along which we need to search for a plane perpendicular to. */ Eigen::Vector3f axis_; @@ -209,9 +216,6 @@ namespace pcl /** \brief temporary pointer to a list of given indices for optimizeModelCoefficients () */ const std::vector *tmp_inliers_; - /** \brief Define the maximum number of iterations for collinearity checks */ - const static int MAX_ITERATIONS_COLLINEAR = 1000; - /** \brief Cost function to be minimized * \param p a pointer to our data structure array * \param m the number of functions diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_line.h b/sample_consensus/include/pcl/sample_consensus/sac_model_line.h index c05bddfd9..6597936c4 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_line.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_line.h @@ -137,9 +137,12 @@ namespace pcl return (true); } - private: - /** \brief Define the maximum number of iterations for unique points search. */ - const static int MAX_ITERATIONS_UNIQUE = 1000; + /** \brief Check if a sample of indices results in a good sample of points + * indices. Pure virtual. + * \param samples the resultant index samples + */ + bool + isSampleGood(const std::vector &samples) const; }; } diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_plane.h b/sample_consensus/include/pcl/sample_consensus/sac_model_plane.h index e154f7d28..5e4fa4479 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_plane.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_plane.h @@ -193,8 +193,12 @@ namespace pcl } private: - /** \brief Define the maximum number of iterations for collinearity checks */ - const static int MAX_ITERATIONS_COLLINEAR = 1000; + /** \brief Check if a sample of indices results in a good sample of points + * indices. Pure virtual. + * \param samples the resultant index samples + */ + bool + isSampleGood(const std::vector &samples) const; }; } diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_registration.h b/sample_consensus/include/pcl/sample_consensus/sac_model_registration.h index 45a9c8ea5..8b144688d 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_registration.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_registration.h @@ -139,15 +139,6 @@ namespace pcl indices_tgt_.reset (new std::vector (indices_tgt)); } - /** \brief Get 3 random indices from a subset of given indices. - * \param iterations the internal number of iterations used by SAC methods - * \param samples the resultant model samples - * \note assumes unique points! - */ - void - getSamples (int &iterations, - std::vector &samples); - /** \brief Compute a 4x4 rigid transformation matrix from the samples given * \param samples the indices found as good candidates for creating a valid model * \param model_coefficients the resultant model coefficients @@ -213,6 +204,13 @@ namespace pcl return (true); } + /** \brief Check if a sample of indices results in a good sample of points + * indices. Pure virtual. + * \param samples the resultant index samples + */ + bool + isSampleGood(const std::vector &samples) const; + private: /** \brief A boost shared pointer to the target point cloud data array. */ PointCloudConstPtr target_; @@ -222,9 +220,6 @@ namespace pcl /** \brief Internal distance threshold used for the sample selection step. */ double sample_dist_thresh_; - - /** \brief Define the maximum number of iterations for collinearity checks */ - const static unsigned int MAX_ITERATIONS_COLLINEAR = 1000; }; } diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_sphere.h b/sample_consensus/include/pcl/sample_consensus/sac_model_sphere.h index a408334a4..402e44206 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_sphere.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_sphere.h @@ -163,6 +163,13 @@ namespace pcl return (true); } + /** \brief Check if a sample of indices results in a good sample of points + * indices. Pure virtual. + * \param samples the resultant index samples + */ + bool + isSampleGood(const std::vector &samples) const; + private: /** \brief Temporary boost mutex for \a tmp_inliers_ */ boost::mutex tmp_mutex_; @@ -170,9 +177,6 @@ namespace pcl /** \brief Temporary pointer to a list of given indices for optimizeModelCoefficients () */ const std::vector *tmp_inliers_; - /** \brief Define the maximum number of iterations for collinearity checks */ - const static int MAX_ITERATIONS_COLLINEAR = 1000; - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////( /** \brief Cost function to be minimized * \param p a pointer to our data structure array diff --git a/test/test_sample_consensus.cpp b/test/test_sample_consensus.cpp index 3658906c6..e4212b885 100644 --- a/test/test_sample_consensus.cpp +++ b/test/test_sample_consensus.cpp @@ -72,8 +72,58 @@ typedef SampleConsensusModelParallelPlane::Ptr SampleConsensusModelPar PointCloud::Ptr cloud_ (new PointCloud ()); PointCloud::Ptr normals_ (new PointCloud ()); vector indices_; +float plane_coeffs_[] = {-0.8964, -0.5868, -1.208}; -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +template +void verifyPlaneSac(ModelType & model, SacType & sac, unsigned int inlier_number = 2000, float tol = 1e-1, + float refined_tol = 1e-2, float proj_tol = 1e-3) +{ + // Algorithm tests + bool result = sac.computeModel (); + ASSERT_EQ (result, true); + + std::vector sample; + sac.getModel (sample); + EXPECT_EQ ((int)sample.size (), 3); + + std::vector inliers; + sac.getInliers (inliers); + EXPECT_GE ((int)inliers.size (), inlier_number); + + Eigen::VectorXf coeff; + sac.getModelCoefficients (coeff); + EXPECT_EQ ((int)coeff.size (), 4); + EXPECT_NEAR (coeff[0]/coeff[3], plane_coeffs_[0], tol); + EXPECT_NEAR (coeff[1]/coeff[3], plane_coeffs_[1], tol); + EXPECT_NEAR (coeff[2]/coeff[3], plane_coeffs_[2], tol); + + + Eigen::VectorXf coeff_refined; + model->optimizeModelCoefficients (inliers, coeff, coeff_refined); + EXPECT_EQ ((int)coeff_refined.size (), 4); + EXPECT_NEAR (coeff_refined[0]/coeff_refined[3], plane_coeffs_[0], refined_tol); + EXPECT_NEAR (coeff_refined[1]/coeff_refined[3], plane_coeffs_[1], refined_tol); + EXPECT_NEAR (coeff_refined[2]/coeff_refined[3], plane_coeffs_[2], refined_tol); + + // Projection tests + PointCloud proj_points; + model->projectPoints (inliers, coeff_refined, proj_points); + EXPECT_NEAR (proj_points.points[20].x, 1.1266, proj_tol); + EXPECT_NEAR (proj_points.points[20].y, 0.0152, proj_tol); + EXPECT_NEAR (proj_points.points[20].z, -0.0156, proj_tol); + + EXPECT_NEAR (proj_points.points[30].x, 1.1843, proj_tol); + EXPECT_NEAR (proj_points.points[30].y, -0.0635, proj_tol); + EXPECT_NEAR (proj_points.points[30].z, -0.0201, proj_tol); + + EXPECT_NEAR (proj_points.points[50].x, 1.0749, proj_tol); + EXPECT_NEAR (proj_points.points[50].y, -0.0586, proj_tol); + EXPECT_NEAR (proj_points.points[50].z, 0.0587, proj_tol); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// TEST (SampleConsensusModelPlane, Base) { // Create a shared plane model pointer directly @@ -128,51 +178,7 @@ TEST (RANSAC, SampleConsensusModelPlane) // Create the RANSAC object RandomSampleConsensus sac (model, 0.03); - // Algorithm tests - bool result = sac.computeModel (); - ASSERT_EQ (result, true); - - std::vector sample; - sac.getModel (sample); - EXPECT_EQ ((int)sample.size (), 3); - EXPECT_EQ (sample[0], 1818); - EXPECT_EQ (sample[1], 1567); - EXPECT_EQ (sample[2], 2064); - - std::vector inliers; - sac.getInliers (inliers); - EXPECT_EQ ((int)inliers.size (), 2525); - - Eigen::VectorXf coeff; - sac.getModelCoefficients (coeff); - EXPECT_EQ ((int)coeff.size (), 4); - EXPECT_NEAR (coeff[0], 0.5590537786, 1e-4); - EXPECT_NEAR (coeff[1], 0.3575230539, 1e-4); - EXPECT_NEAR (coeff[2], 0.7480883598, 1e-4); - EXPECT_NEAR (coeff[3], -0.622878551, 1e-4); - - Eigen::VectorXf coeff_refined; - model->optimizeModelCoefficients (inliers, coeff, coeff_refined); - EXPECT_EQ ((int)coeff_refined.size (), 4); - EXPECT_NEAR (coeff_refined[0], 0.5554245114, 1e-4); - EXPECT_NEAR (coeff_refined[1], 0.3644647598, 1e-4); - EXPECT_NEAR (coeff_refined[2], 0.747441709, 1e-4); - EXPECT_NEAR (coeff_refined[3], -0.619598031, 1e-4); - - // Projection tests - PointCloud proj_points; - model->projectPoints (inliers, coeff_refined, proj_points); - EXPECT_NEAR (proj_points.points[20].x, 1.12661, 1e-4); - EXPECT_NEAR (proj_points.points[20].y, 0.0152829, 1e-4); - EXPECT_NEAR (proj_points.points[20].z, -0.0156815, 1e-4); - - EXPECT_NEAR (proj_points.points[30].x, 1.18438, 1e-4); - EXPECT_NEAR (proj_points.points[30].y, -0.0635465, 1e-4); - EXPECT_NEAR (proj_points.points[30].z, -0.0201715, 1e-4); - - EXPECT_NEAR (proj_points.points[50].x, 1.07499, 1e-4); - EXPECT_NEAR (proj_points.points[50].y, -0.0586441, 1e-4); - EXPECT_NEAR (proj_points.points[50].z, 0.0587273, 1e-4); + verifyPlaneSac(model, sac); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -185,52 +191,7 @@ TEST (LMedS, SampleConsensusModelPlane) // Create the LMedS object LeastMedianSquares sac (model, 0.03); - // Algorithm tests - bool result = sac.computeModel (); - ASSERT_EQ (result, true); - - std::vector sample; - sac.getModel (sample); - EXPECT_EQ ((int)sample.size (), 3); - EXPECT_EQ (sample[0], 338); - EXPECT_EQ (sample[1], 413); - EXPECT_EQ (sample[2], 1626); - - std::vector inliers; - sac.getInliers (inliers); - EXPECT_EQ ((int)inliers.size (), 2490); - - Eigen::VectorXf coeff; - sac.getModelCoefficients (coeff); - EXPECT_EQ ((int)coeff.size (), 4); - EXPECT_NEAR (coeff[0], -0.5413796306, 1e-4); - EXPECT_NEAR (coeff[1], -0.3658693433, 1e-4); - EXPECT_NEAR (coeff[2], -0.756999135, 1e-4); - EXPECT_NEAR (coeff[3], 0.606127798, 1e-4); - - Eigen::VectorXf coeff_refined; - model->optimizeModelCoefficients (inliers, coeff, coeff_refined); - EXPECT_EQ ((int)coeff_refined.size (), 4); - EXPECT_NEAR (coeff_refined[0], 0.5517195463, 1e-4); - EXPECT_NEAR (coeff_refined[1], 0.3651787937, 1e-4); - EXPECT_NEAR (coeff_refined[2], 0.74983340, 1e-4); - EXPECT_NEAR (coeff_refined[3], -0.6159575582, 1e-4); - - // Projection tests - PointCloud proj_points; - model->projectPoints (inliers, coeff_refined, proj_points); - - EXPECT_NEAR (proj_points.points[20].x, 1.12694, 1e-4); - EXPECT_NEAR (proj_points.points[20].y, 0.0154845, 1e-4); - EXPECT_NEAR (proj_points.points[20].z, -0.0152713, 1e-4); - - EXPECT_NEAR (proj_points.points[30].x, 1.18486, 1e-4); - EXPECT_NEAR (proj_points.points[30].y, -0.0632408, 1e-4); - EXPECT_NEAR (proj_points.points[30].z, -0.0195459, 1e-4); - - EXPECT_NEAR (proj_points.points[50].x, 1.07512, 1e-4); - EXPECT_NEAR (proj_points.points[50].y, -0.0585533, 1e-4); - EXPECT_NEAR (proj_points.points[50].z, 0.058916, 1e-4); + verifyPlaneSac(model, sac); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -243,53 +204,7 @@ TEST (MSAC, SampleConsensusModelPlane) // Create the MSAC object MEstimatorSampleConsensus sac (model, 0.03); - // Algorithm tests - bool result = sac.computeModel (); - ASSERT_EQ (result, true); - - std::vector sample; - sac.getModel (sample); - EXPECT_EQ ((int)sample.size (), 3); - EXPECT_EQ (sample[0], 464); - EXPECT_EQ (sample[1], 1992); - EXPECT_EQ (sample[2], 53); - - std::vector inliers; - sac.getInliers (inliers); - EXPECT_EQ ((int)inliers.size (), 2505); - - Eigen::VectorXf coeff; - sac.getModelCoefficients (coeff); - EXPECT_EQ ((int)coeff.size (), 4); - - EXPECT_NEAR (coeff[0], -0.5485954285, 1e-4); - EXPECT_NEAR (coeff[1], -0.3597415686, 1e-4); - EXPECT_NEAR (coeff[2], -0.7547376752, 1e-4); - EXPECT_NEAR (coeff[3], 0.6130523086, 1e-4); - - Eigen::VectorXf coeff_refined; - model->optimizeModelCoefficients (inliers, coeff, coeff_refined); - EXPECT_EQ ((int)coeff_refined.size (), 4); - EXPECT_NEAR (coeff_refined[0], 0.5532695055, 1e-4); - EXPECT_NEAR (coeff_refined[1], 0.3648152649, 1e-4); - EXPECT_NEAR (coeff_refined[2], 0.7488676906, 1e-4); - EXPECT_NEAR (coeff_refined[3], -0.617486834, 1e-4); - - // Projection tests - PointCloud proj_points; - model->projectPoints (inliers, coeff_refined, proj_points); - - EXPECT_NEAR (proj_points.points[20].x, 1.12681, 1e-4); - EXPECT_NEAR (proj_points.points[20].y, 0.0154031, 1e-4); - EXPECT_NEAR (proj_points.points[20].z, -0.0154375, 1e-4); - - EXPECT_NEAR (proj_points.points[30].x, 1.18466, 1e-4); - EXPECT_NEAR (proj_points.points[30].y, -0.0633676, 1e-4); - EXPECT_NEAR (proj_points.points[30].z, -0.0198059, 1e-4); - - EXPECT_NEAR (proj_points.points[50].x, 1.07506, 1e-4); - EXPECT_NEAR (proj_points.points[50].y, -0.0585913, 1e-4); - EXPECT_NEAR (proj_points.points[50].z, 0.0588374, 1e-4); + verifyPlaneSac(model, sac); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -305,52 +220,7 @@ TEST (RRANSAC, SampleConsensusModelPlane) sac.setFractionNrPretest (10.0); ASSERT_EQ (sac.getFractionNrPretest (), 10.0); - // Algorithm tests - bool result = sac.computeModel (); - ASSERT_EQ (result, true); - - std::vector sample; - sac.getModel (sample); - EXPECT_EQ ((int)sample.size (), 3); - EXPECT_EQ (sample[0], 2758); - EXPECT_EQ (sample[1], 1294); - EXPECT_EQ (sample[2], 2570); - - std::vector inliers; - sac.getInliers (inliers); - EXPECT_EQ ((int)inliers.size (), 2482); - - Eigen::VectorXf coeff; - sac.getModelCoefficients (coeff); - EXPECT_EQ ((int)coeff.size (), 4); - EXPECT_NEAR (coeff[0], -0.5901594758, 1e-4); - EXPECT_NEAR (coeff[1], -0.3539851904, 1e-4); - EXPECT_NEAR (coeff[2], -0.725538671, 1e-4); - EXPECT_NEAR (coeff[3], 0.6641977429, 1e-4); - - Eigen::VectorXf coeff_refined; - model->optimizeModelCoefficients (inliers, coeff, coeff_refined); - EXPECT_EQ ((int)coeff_refined.size (), 4); - EXPECT_NEAR (fabs (coeff_refined[0]), 0.5598492622, 1e-4); - EXPECT_NEAR (fabs (coeff_refined[1]), 0.3632659912, 1e-4); - EXPECT_NEAR (fabs (coeff_refined[2]), 0.7447191477, 1e-4); - EXPECT_NEAR (fabs (coeff_refined[3]), 0.6246083975, 1e-4); - - // Projection tests - PointCloud proj_points; - model->projectPoints (inliers, coeff_refined, proj_points); - - EXPECT_NEAR (proj_points.points[20].x, 1.1266, 1e-4); - EXPECT_NEAR (proj_points.points[20].y, 0.0152881, 1e-4); - EXPECT_NEAR (proj_points.points[20].z, -0.0156696, 1e-4); - - EXPECT_NEAR (proj_points.points[30].x, 1.18417, 1e-4); - EXPECT_NEAR (proj_points.points[30].y, -0.0636751, 1e-4); - EXPECT_NEAR (proj_points.points[30].z, -0.0204345, 1e-4); - - EXPECT_NEAR (proj_points.points[50].x, 1.07519, 1e-4); - EXPECT_NEAR (proj_points.points[50].y, -0.0585223, 1e-4); - EXPECT_NEAR (proj_points.points[50].z, 0.0589761, 1e-4); + verifyPlaneSac(model, sac, 600, 1.0 , 1.0, 0.01); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -366,52 +236,7 @@ TEST (RMSAC, SampleConsensusModelPlane) sac.setFractionNrPretest (10.0); ASSERT_EQ (sac.getFractionNrPretest (), 10.0); - // Algorithm tests - bool result = sac.computeModel (); - ASSERT_EQ (result, true); - - std::vector sample; - sac.getModel (sample); - EXPECT_EQ ((int)sample.size (), 3); - EXPECT_EQ (sample[0], 2758); - EXPECT_EQ (sample[1], 1294); - EXPECT_EQ (sample[2], 2570); - - std::vector inliers; - sac.getInliers (inliers); - EXPECT_EQ ((int)inliers.size (), 2482); - - Eigen::VectorXf coeff; - sac.getModelCoefficients (coeff); - EXPECT_EQ ((int)coeff.size (), 4); - EXPECT_NEAR (coeff[0], -0.5901594758, 1e-4); - EXPECT_NEAR (coeff[1], -0.3539851904, 1e-4); - EXPECT_NEAR (coeff[2], -0.725538671, 1e-4); - EXPECT_NEAR (coeff[3], 0.6641977429, 1e-4); - - Eigen::VectorXf coeff_refined; - model->optimizeModelCoefficients (inliers, coeff, coeff_refined); - EXPECT_EQ ((int)coeff_refined.size (), 4); - EXPECT_NEAR (fabs (coeff_refined[0]), 0.5598492622, 1e-4); - EXPECT_NEAR (fabs (coeff_refined[1]), 0.3632659912, 1e-4); - EXPECT_NEAR (fabs (coeff_refined[2]), 0.7447191477, 1e-4); - EXPECT_NEAR (fabs (coeff_refined[3]), 0.6246083975, 1e-4); - - // Projection tests - PointCloud proj_points; - model->projectPoints (inliers, coeff_refined, proj_points); - - EXPECT_NEAR (proj_points.points[20].x, 1.1266, 1e-4); - EXPECT_NEAR (proj_points.points[20].y, 0.0152881, 1e-4); - EXPECT_NEAR (proj_points.points[20].z, -0.0156696, 1e-4); - - EXPECT_NEAR (proj_points.points[30].x, 1.18417, 1e-4); - EXPECT_NEAR (proj_points.points[30].y, -0.0636751, 1e-4); - EXPECT_NEAR (proj_points.points[30].z, -0.0204345, 1e-4); - - EXPECT_NEAR (proj_points.points[50].x, 1.07519, 1e-4); - EXPECT_NEAR (proj_points.points[50].y, -0.0585223, 1e-4); - EXPECT_NEAR (proj_points.points[50].z, 0.0589761, 1e-4); + verifyPlaneSac(model, sac, 600, 1.0, 1.0, 0.01); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -424,53 +249,7 @@ TEST (MLESAC, SampleConsensusModelPlane) // Create the MSAC object MaximumLikelihoodSampleConsensus sac (model, 0.03); - // Algorithm tests - bool result = sac.computeModel (); - ASSERT_EQ (result, true); - - std::vector sample; - sac.getModel (sample); - EXPECT_EQ ((int)sample.size (), 3); - EXPECT_EQ (sample[0], 2758); - EXPECT_EQ (sample[1], 1294); - EXPECT_EQ (sample[2], 2570); - - std::vector inliers; - sac.getInliers (inliers); - EXPECT_EQ ((int)inliers.size (), 2214); - - Eigen::VectorXf coeff; - sac.getModelCoefficients (coeff); - EXPECT_EQ ((int)coeff.size (), 4); - - EXPECT_NEAR (coeff[0], -0.5901594758, 1e-4); - EXPECT_NEAR (coeff[1], -0.3539851904, 1e-4); - EXPECT_NEAR (coeff[2], -0.725538671, 1e-4); - EXPECT_NEAR (coeff[3], 0.664197742, 1e-4); - - Eigen::VectorXf coeff_refined; - model->optimizeModelCoefficients (inliers, coeff, coeff_refined); - EXPECT_EQ ((int)coeff_refined.size (), 4); - EXPECT_NEAR (fabs (coeff_refined[0]), 0.5599190593, 1e-4); - EXPECT_NEAR (fabs (coeff_refined[1]), 0.3627234101, 1e-4); - EXPECT_NEAR (fabs (coeff_refined[2]), 0.7449311614, 1e-4); - EXPECT_NEAR (fabs (coeff_refined[3]), 0.625774502, 1e-4); - - // Projection tests - PointCloud proj_points; - model->projectPoints (inliers, coeff_refined, proj_points); - - EXPECT_NEAR (proj_points.points[20].x, 1.12721, 1e-4); - EXPECT_NEAR (proj_points.points[20].y, 0.01568, 1e-4); - EXPECT_NEAR (proj_points.points[20].z, -0.014851, 1e-4); - - EXPECT_NEAR (proj_points.points[30].x, 1.18476, 1e-4); - EXPECT_NEAR (proj_points.points[30].y, -0.063291, 1e-4); - EXPECT_NEAR (proj_points.points[30].z, -0.019650, 1e-4); - - EXPECT_NEAR (proj_points.points[50].x, 1.07578, 1e-4); - EXPECT_NEAR (proj_points.points[50].y, -0.058144, 1e-4); - EXPECT_NEAR (proj_points.points[50].z, 0.059756, 1e-4); + verifyPlaneSac(model, sac, 1000, 0.3, 0.2, 0.01); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -505,10 +284,6 @@ TEST (RANSAC, SampleConsensusModelSphere) std::vector sample; sac.getModel (sample); EXPECT_EQ ((int)sample.size (), 4); - EXPECT_EQ (sample[0], 1); - EXPECT_EQ (sample[1], 3); - EXPECT_EQ (sample[2], 7); - EXPECT_EQ (sample[3], 2); std::vector inliers; sac.getInliers (inliers); @@ -517,18 +292,16 @@ TEST (RANSAC, SampleConsensusModelSphere) Eigen::VectorXf coeff; sac.getModelCoefficients (coeff); EXPECT_EQ ((int)coeff.size (), 4); - EXPECT_NEAR (coeff[0], 2.00052, 1e-4); - EXPECT_NEAR (coeff[1], 1.9997, 1e-4); - EXPECT_NEAR (coeff[2], 2.00003, 1e-4); - EXPECT_NEAR (coeff[3], 0.999565, 1e-4); + EXPECT_NEAR (coeff[0]/coeff[3], 2, 1e-3); + EXPECT_NEAR (coeff[1]/coeff[3], 2, 1e-3); + EXPECT_NEAR (coeff[2]/coeff[3], 2, 1e-3); Eigen::VectorXf coeff_refined; model->optimizeModelCoefficients (inliers, coeff, coeff_refined); EXPECT_EQ ((int)coeff_refined.size (), 4); - EXPECT_NEAR (coeff_refined[0], 2.00023, 1e-3); - EXPECT_NEAR (coeff_refined[1], 1.99979, 1e-3); - EXPECT_NEAR (coeff_refined[2], 1.99979, 1e-3); - EXPECT_NEAR (coeff_refined[3], 0.999888, 1e-3); + EXPECT_NEAR (coeff_refined[0]/coeff_refined[3], 2, 1e-3); + EXPECT_NEAR (coeff_refined[1]/coeff_refined[3], 2, 1e-3); + EXPECT_NEAR (coeff_refined[2]/coeff_refined[3], 2, 1e-3); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -597,8 +370,6 @@ TEST (RANSAC, SampleConsensusModelCylinder) std::vector sample; sac.getModel (sample); EXPECT_EQ ((int)sample.size (), 2); - EXPECT_EQ (sample[0], 16); - EXPECT_EQ (sample[1], 7); std::vector inliers; sac.getInliers (inliers); @@ -607,14 +378,14 @@ TEST (RANSAC, SampleConsensusModelCylinder) Eigen::VectorXf coeff; sac.getModelCoefficients (coeff); EXPECT_EQ ((int)coeff.size (), 7); - EXPECT_NEAR (coeff[0], -0.500038, 1e-4); - EXPECT_NEAR (coeff[1], 1.69997, 1e-4); - EXPECT_NEAR (coeff[6], 0.499934, 1e-4); + EXPECT_NEAR (coeff[0], -0.5, 1e-3); + EXPECT_NEAR (coeff[1], 1.7, 1e-3); + EXPECT_NEAR (coeff[6], 0.5, 1e-3); Eigen::VectorXf coeff_refined; model->optimizeModelCoefficients (inliers, coeff, coeff_refined); EXPECT_EQ ((int)coeff_refined.size (), 7); - EXPECT_NEAR (coeff_refined[6], 0.499966, 1e-4); + EXPECT_NEAR (coeff_refined[6], 0.5, 1e-3); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -658,9 +429,6 @@ TEST (RANSAC, SampleConsensusModelCircle2D) std::vector sample; sac.getModel (sample); EXPECT_EQ ((int)sample.size (), 3); - EXPECT_EQ (sample[0], 15); - EXPECT_EQ (sample[1], 7); - EXPECT_EQ (sample[2], 14); std::vector inliers; sac.getInliers (inliers); @@ -669,16 +437,16 @@ TEST (RANSAC, SampleConsensusModelCircle2D) Eigen::VectorXf coeff; sac.getModelCoefficients (coeff); EXPECT_EQ ((int)coeff.size (), 3); - EXPECT_NEAR (coeff[0], 2.9988, 1e-4); - EXPECT_NEAR (coeff[1], -4.99885, 1e-4); - EXPECT_NEAR (coeff[2], 0.998406, 1e-4); + EXPECT_NEAR (coeff[0], 3, 1e-3); + EXPECT_NEAR (coeff[1], -5, 1e-3); + EXPECT_NEAR (coeff[2], 1, 1e-3); Eigen::VectorXf coeff_refined; model->optimizeModelCoefficients (inliers, coeff, coeff_refined); EXPECT_EQ ((int)coeff_refined.size (), 3); - EXPECT_NEAR (coeff_refined[0], 2.99999, 1e-4); - EXPECT_NEAR (coeff_refined[1], -5.00004, 1e-4); - EXPECT_NEAR (coeff_refined[2], 0.999962, 1e-4); + EXPECT_NEAR (coeff_refined[0], 3, 1e-3); + EXPECT_NEAR (coeff_refined[1], -5, 1e-3); + EXPECT_NEAR (coeff_refined[2], 1, 1e-3); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -714,8 +482,6 @@ TEST (RANSAC, SampleConsensusModelLine) std::vector sample; sac.getModel (sample); EXPECT_EQ ((int)sample.size (), 2); - EXPECT_EQ (sample[0], 1); - EXPECT_EQ (sample[1], 3); std::vector inliers; sac.getInliers (inliers); @@ -724,16 +490,14 @@ TEST (RANSAC, SampleConsensusModelLine) Eigen::VectorXf coeff; sac.getModelCoefficients (coeff); EXPECT_EQ ((int)coeff.size (), 6); - EXPECT_NEAR (coeff[3], 0.57735, 1e-4); - EXPECT_NEAR (coeff[4], 0.57735, 1e-4); - EXPECT_NEAR (coeff[5], 0.57735, 1e-4); + EXPECT_NEAR (coeff[4]/coeff[3], 1, 1e-4); + EXPECT_NEAR (coeff[5]/coeff[3], 1, 1e-4); Eigen::VectorXf coeff_refined; model->optimizeModelCoefficients (inliers, coeff, coeff_refined); EXPECT_EQ ((int)coeff_refined.size (), 6); - EXPECT_NEAR (coeff_refined[3], 0.57735, 1e-4); - EXPECT_NEAR (coeff_refined[4], 0.57735, 1e-4); - EXPECT_NEAR (coeff_refined[5], 0.57735, 1e-4); + EXPECT_NEAR (coeff[4]/coeff[3], 1, 1e-4); + EXPECT_NEAR (coeff[5]/coeff[3], 1, 1e-4); // Projection tests PointCloud proj_points; @@ -763,52 +527,7 @@ TEST (RANSAC, SampleConsensusModelNormalPlane) // Create the RANSAC object RandomSampleConsensus sac (model, 0.03); - // Algorithm tests - bool result = sac.computeModel (); - ASSERT_EQ (result, true); - - std::vector sample; - sac.getModel (sample); - EXPECT_EQ ((int)sample.size (), 3); - EXPECT_EQ (sample[0], 1818); - EXPECT_EQ (sample[1], 1567); - EXPECT_EQ (sample[2], 2064); - - std::vector inliers; - sac.getInliers (inliers); - EXPECT_EQ ((int)inliers.size (), 2440); - - Eigen::VectorXf coeff; - sac.getModelCoefficients (coeff); - EXPECT_EQ ((int)coeff.size (), 4); - EXPECT_NEAR (coeff[0], 0.5590537786, 1e-4); - EXPECT_NEAR (coeff[1], 0.3575230538, 1e-4); - EXPECT_NEAR (coeff[2], 0.7480884194, 1e-4); - EXPECT_NEAR (coeff[3], -0.6228785514, 1e-4); - - Eigen::VectorXf coeff_refined; - model->optimizeModelCoefficients (inliers, coeff, coeff_refined); - EXPECT_EQ ((int)coeff_refined.size (), 4); - EXPECT_NEAR (fabs (coeff_refined[0]), 0.552499, 1e-4); - EXPECT_NEAR (fabs (coeff_refined[1]), 0.364361, 1e-4); - EXPECT_NEAR (fabs (coeff_refined[2]), 0.749656, 1e-4); - EXPECT_NEAR (fabs (coeff_refined[3]), 0.617206, 1e-4); - - // Projection tests - PointCloud proj_points; - model->projectPoints (inliers, coeff_refined, proj_points); - - EXPECT_NEAR (proj_points.points[20].x, 1.12707, 1e-4); - EXPECT_NEAR (proj_points.points[20].y, 0.0155766, 1e-4); - EXPECT_NEAR (proj_points.points[20].z, -0.0149861, 1e-4); - - EXPECT_NEAR (proj_points.points[30].x, 1.185, 1e-4); - EXPECT_NEAR (proj_points.points[30].y, -0.063224, 1e-4); - EXPECT_NEAR (proj_points.points[30].z, -0.019343, 1e-4); - - EXPECT_NEAR (proj_points.points[50].x, 1.07528, 1e-4); - EXPECT_NEAR (proj_points.points[50].y, -0.0584513, 1e-4); - EXPECT_NEAR (proj_points.points[50].z, 0.0591309, 1e-4); + verifyPlaneSac(model, sac); } /* ---[ */ -- GitLab