[sample_consensus] Fix inconsistent sample validation

sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle.hpp

@@ -54,7 +54,9 @@ pcl::SampleConsensusModelCircle2D<PointT>::isSampleGood(const Indices &samples)
     PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
     return (false);
   }
-  // Get the values at the two points
+
+  // Double precision here follows computeModelCoefficients, which means we
+  // can't use getVector3fMap-accessor to make our lives easier.
   Eigen::Array2d p0 ((*input_)[samples[0]].x, (*input_)[samples[0]].y);
   Eigen::Array2d p1 ((*input_)[samples[1]].x, (*input_)[samples[1]].y);
   Eigen::Array2d p2 ((*input_)[samples[2]].x, (*input_)[samples[2]].y);
@@ -64,19 +66,23 @@ pcl::SampleConsensusModelCircle2D<PointT>::isSampleGood(const Indices &samples)
   // Compute the segment values (in 2d) between p2 and p0
   p2 -= p0;
 
-  Eigen::Array2d dy1dy2 = p1 / p2;
+  // Check if the triangle area spanned by the three sample points is greater than 0
+  // https://en.wikipedia.org/wiki/Triangle#Using_vectors
+  if (std::abs (p1.x()*p2.y() - p2.x()*p1.y()) < Eigen::NumTraits<double>::dummy_precision ()) {
+    PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::isSampleGood] Sample points too similar or collinear!\n");
+    return (false);
+  }
 
-  return (dy1dy2[0] != dy1dy2[1]);
+  return (true);
 }
 
 //////////////////////////////////////////////////////////////////////////
 template <typename PointT> bool
 pcl::SampleConsensusModelCircle2D<PointT>::computeModelCoefficients (const Indices &samples, Eigen::VectorXf &model_coefficients) const
 {
-  // Need 3 samples
-  if (samples.size () != sample_size_)
+  if (!isSampleGood (samples))
   {
-    PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
+    PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::computeModelCoefficients] Invalid set of samples given!\n");
     return (false);
   }
 

sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle3d.hpp

@@ -55,23 +55,29 @@ pcl::SampleConsensusModelCircle3D<PointT>::isSampleGood (
     PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
     return (false);
   }
-  // Get the values at the three points
+
+  // Double precision here follows computeModelCoefficients, which means we
+  // can't use getVector3fMap-accessor to make our lives easier.
   Eigen::Vector3d p0 ((*input_)[samples[0]].x, (*input_)[samples[0]].y, (*input_)[samples[0]].z);
   Eigen::Vector3d p1 ((*input_)[samples[1]].x, (*input_)[samples[1]].y, (*input_)[samples[1]].z);
   Eigen::Vector3d p2 ((*input_)[samples[2]].x, (*input_)[samples[2]].y, (*input_)[samples[2]].z);
 
-  // calculate vectors between points
-  p1 -= p0;
-  p2 -= p0;
+  // Check if the squared norm of the cross-product is non-zero, otherwise
+  // common_helper_vec, which plays an important role in computeModelCoefficients,
+  // would likely be ill-formed.
+  if ((p1 - p0).cross(p1 - p2).squaredNorm() < Eigen::NumTraits<float>::dummy_precision ())
+  {
+    PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::isSampleGood] Sample points too similar or collinear!\n");
+    return (false);
+  }
 
-  return (p1.dot (p2) < 0.000001);
+  return (true);
 }
 
 //////////////////////////////////////////////////////////////////////////
 template <typename PointT> bool
 pcl::SampleConsensusModelCircle3D<PointT>::computeModelCoefficients (const Indices &samples, Eigen::VectorXf &model_coefficients) const
 {
-  // Need 3 samples
   if (samples.size () != sample_size_)
   {
     PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
@@ -94,6 +100,14 @@ pcl::SampleConsensusModelCircle3D<PointT>::computeModelCoefficients (const Indic
 
   Eigen::Vector3d common_helper_vec = helper_vec01.cross (helper_vec12);
 
+  // The same check is implemented in isSampleGood, so be sure to look there too
+  // if you find the need to change something here.
+  if (common_helper_vec.squaredNorm() < Eigen::NumTraits<float>::dummy_precision ())
+  {
+    PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::computeModelCoefficients] Sample points too similar or collinear!\n");
+    return (false);
+  }
+
   double commonDividend = 2.0 * common_helper_vec.squaredNorm ();
 
   double alpha = (helper_vec12.squaredNorm () * helper_vec01.dot (helper_vec02)) / commonDividend;

sample_consensus/include/pcl/sample_consensus/impl/sac_model_cone.hpp

@@ -61,10 +61,10 @@ template <typename PointT, typename PointNT> bool
 pcl::SampleConsensusModelCone<PointT, PointNT>::computeModelCoefficients (
     const Indices &samples, Eigen::VectorXf &model_coefficients) const
 {
-  // Need 3 samples
-  if (samples.size () != sample_size_)
+  // Make sure that the samples are valid
+  if (!isSampleGood (samples))
   {
-    PCL_ERROR ("[pcl::SampleConsensusModelCone::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
+    PCL_ERROR ("[pcl::SampleConsensusModelCone::computeModelCoefficients] Invalid set of samples given\n");
     return (false);
   }
 

sample_consensus/include/pcl/sample_consensus/impl/sac_model_cylinder.hpp

@@ -55,6 +55,19 @@ pcl::SampleConsensusModelCylinder<PointT, PointNT>::isSampleGood (const Indices
     PCL_ERROR ("[pcl::SampleConsensusModelCylinder::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
     return (false);
   }
+
+  // Make sure that the two sample points are not identical
+  if (
+      std::abs ((*input_)[samples[0]].x - (*input_)[samples[1]].x) <= std::numeric_limits<float>::epsilon ()
+    &&
+      std::abs ((*input_)[samples[0]].y - (*input_)[samples[1]].y) <= std::numeric_limits<float>::epsilon ()
+    &&
+      std::abs ((*input_)[samples[0]].z - (*input_)[samples[1]].z) <= std::numeric_limits<float>::epsilon ())
+  {
+    PCL_ERROR ("[pcl::SampleConsensusModelCylinder::isSampleGood] The two sample points are (almost) identical!\n");
+    return (false);
+  }
+
   return (true);
 }
 
@@ -63,10 +76,10 @@ template <typename PointT, typename PointNT> bool
 pcl::SampleConsensusModelCylinder<PointT, PointNT>::computeModelCoefficients (
       const Indices &samples, Eigen::VectorXf &model_coefficients) const
 {
-  // Need 2 samples
-  if (samples.size () != sample_size_)
+  // Make sure that the samples are valid
+  if (!isSampleGood (samples))
   {
-    PCL_ERROR ("[pcl::SampleConsensusModelCylinder::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
+    PCL_ERROR ("[pcl::SampleConsensusModelCylinder::computeModelCoefficients] Invalid set of samples given!\n");
     return (false);
   }
 
@@ -76,13 +89,6 @@ pcl::SampleConsensusModelCylinder<PointT, PointNT>::computeModelCoefficients (
     return (false);
   }
 
-  if (std::abs ((*input_)[samples[0]].x - (*input_)[samples[1]].x) <= std::numeric_limits<float>::epsilon () && 
-      std::abs ((*input_)[samples[0]].y - (*input_)[samples[1]].y) <= std::numeric_limits<float>::epsilon () && 
-      std::abs ((*input_)[samples[0]].z - (*input_)[samples[1]].z) <= std::numeric_limits<float>::epsilon ()) 
-  {
-    return (false);
-  }
-
   Eigen::Vector4f p1 ((*input_)[samples[0]].x, (*input_)[samples[0]].y, (*input_)[samples[0]].z, 0.0f);
   Eigen::Vector4f p2 ((*input_)[samples[1]].x, (*input_)[samples[1]].y, (*input_)[samples[1]].z, 0.0f);
 

sample_consensus/include/pcl/sample_consensus/impl/sac_model_line.hpp

@@ -55,36 +55,31 @@ pcl::SampleConsensusModelLine<PointT>::isSampleGood (const Indices &samples) con
     PCL_ERROR ("[pcl::SampleConsensusModelLine::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
     return (false);
   }
+
   // Make sure that the two sample points are not identical
   if (
-      ((*input_)[samples[0]].x != (*input_)[samples[1]].x)
-    ||
-      ((*input_)[samples[0]].y != (*input_)[samples[1]].y)
-    ||
-      ((*input_)[samples[0]].z != (*input_)[samples[1]].z))
+      std::abs ((*input_)[samples[0]].x - (*input_)[samples[1]].x) <= std::numeric_limits<float>::epsilon ()
+    &&
+      std::abs ((*input_)[samples[0]].y - (*input_)[samples[1]].y) <= std::numeric_limits<float>::epsilon ()
+    &&
+      std::abs ((*input_)[samples[0]].z - (*input_)[samples[1]].z) <= std::numeric_limits<float>::epsilon ())
   {
-    return (true);
+    PCL_ERROR ("[pcl::SampleConsensusModelLine::isSampleGood] The two sample points are (almost) identical!\n");
+    return (false);
   }
 
-  return (false);
+  return (true);
 }
 
 //////////////////////////////////////////////////////////////////////////
 template <typename PointT> bool
 pcl::SampleConsensusModelLine<PointT>::computeModelCoefficients (
       const Indices &samples, Eigen::VectorXf &model_coefficients) const
 {
-  // Need 2 samples
-  if (samples.size () != sample_size_)
-  {
-    PCL_ERROR ("[pcl::SampleConsensusModelLine::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
-    return (false);
-  }
-
-  if (std::abs ((*input_)[samples[0]].x - (*input_)[samples[1]].x) <= std::numeric_limits<float>::epsilon () && 
-      std::abs ((*input_)[samples[0]].y - (*input_)[samples[1]].y) <= std::numeric_limits<float>::epsilon () && 
-      std::abs ((*input_)[samples[0]].z - (*input_)[samples[1]].z) <= std::numeric_limits<float>::epsilon ())
+  // Make sure that the samples are valid
+  if (!isSampleGood (samples))
   {
+    PCL_ERROR ("[pcl::SampleConsensusModelLine::computeModelCoefficients] Invalid set of samples given!\n");
     return (false);
   }
 
@@ -97,6 +92,9 @@ pcl::SampleConsensusModelLine<PointT>::computeModelCoefficients (
   model_coefficients[4] = (*input_)[samples[1]].y - model_coefficients[1];
   model_coefficients[5] = (*input_)[samples[1]].z - model_coefficients[2];
 
+  // This precondition should hold if the samples have been found to be good
+  assert (model_coefficients.template tail<3> ().squaredNorm () > 0.0f);
+
   model_coefficients.template tail<3> ().normalize ();
   PCL_DEBUG ("[pcl::SampleConsensusModelLine::computeModelCoefficients] Model is (%g,%g,%g,%g,%g,%g).\n",
              model_coefficients[0], model_coefficients[1], model_coefficients[2],

sample_consensus/include/pcl/sample_consensus/impl/sac_model_plane.hpp

@@ -55,58 +55,62 @@ pcl::SampleConsensusModelPlane<PointT>::isSampleGood (const Indices &samples) co
     PCL_ERROR ("[pcl::SampleConsensusModelPlane::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
     return (false);
   }
-  // Get the values at the two points
-  pcl::Array4fMapConst p0 = (*input_)[samples[0]].getArray4fMap ();
-  pcl::Array4fMapConst p1 = (*input_)[samples[1]].getArray4fMap ();
-  pcl::Array4fMapConst p2 = (*input_)[samples[2]].getArray4fMap ();
 
-  Eigen::Array4f p2p0 = p2 - p0;
-  Eigen::Array4f dy1dy2 = (p1-p0) / p2p0;
+  // Check if the sample points are collinear
+  // Similar checks are implemented as precaution in computeModelCoefficients,
+  // so if you find the need to fix something in here, look there, too.
+  pcl::Vector3fMapConst p0 = (*input_)[samples[0]].getVector3fMap ();
+  pcl::Vector3fMapConst p1 = (*input_)[samples[1]].getVector3fMap ();
+  pcl::Vector3fMapConst p2 = (*input_)[samples[2]].getVector3fMap ();
+
+  // Check if the norm of the cross-product would be non-zero, otherwise
+  // normalization will fail. One could also interpret this as kind of check
+  // if the triangle spanned by those three points would have an area greater
+  // than zero.
+  if ((p1 - p0).cross(p2 - p0).stableNorm() < Eigen::NumTraits<float>::dummy_precision ())
+  {
+    PCL_ERROR ("[pcl::SampleConsensusModelPlane::isSampleGood] Sample points too similar or collinear!\n");
+    return (false);
+  }
 
-  return ( ((dy1dy2[0] != dy1dy2[1]) || (dy1dy2[2] != dy1dy2[1])) && (!p2p0.isZero()) );
+  return (true);
 }
 
 //////////////////////////////////////////////////////////////////////////
 template <typename PointT> bool
 pcl::SampleConsensusModelPlane<PointT>::computeModelCoefficients (
       const Indices &samples, Eigen::VectorXf &model_coefficients) const
 {
-  // Need 3 samples
+  // The checks are redundant with isSampleGood above, but since most of the
+  // computed values are also used to compute the model coefficients, this might
+  // be a situation where this duplication is acceptable.
   if (samples.size () != sample_size_)
   {
     PCL_ERROR ("[pcl::SampleConsensusModelPlane::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
     return (false);
   }
 
-  pcl::Array4fMapConst p0 = (*input_)[samples[0]].getArray4fMap ();
-  pcl::Array4fMapConst p1 = (*input_)[samples[1]].getArray4fMap ();
-  pcl::Array4fMapConst p2 = (*input_)[samples[2]].getArray4fMap ();
+  pcl::Vector3fMapConst p0 = (*input_)[samples[0]].getVector3fMap ();
+  pcl::Vector3fMapConst p1 = (*input_)[samples[1]].getVector3fMap ();
+  pcl::Vector3fMapConst p2 = (*input_)[samples[2]].getVector3fMap ();
 
-  // 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;
+  const Eigen::Vector3f cross = (p1 - p0).cross(p2 - p0);
+  const float crossNorm = cross.stableNorm();
 
-  // Avoid some crashes by checking for collinearity here
-  Eigen::Array4f dy1dy2 = p1p0 / p2p0;
-  if ( p2p0.isZero() || ((dy1dy2[0] == dy1dy2[1]) && (dy1dy2[2] == dy1dy2[1])) )          // Check for collinearity
+  // Checking for collinearity here
+  if (crossNorm < Eigen::NumTraits<float>::dummy_precision ())
   {
+    PCL_ERROR ("[pcl::SampleConsensusModelPlane::computeModelCoefficients] Chosen samples are collinear!\n");
     return (false);
   }
 
   // Compute the plane coefficients from the 3 given points in a straightforward manner
   // calculate the plane normal n = (p2-p1) x (p3-p1) = cross (p2-p1, p3-p1)
   model_coefficients.resize (model_size_);
-  model_coefficients[0] = p1p0[1] * p2p0[2] - p1p0[2] * p2p0[1];
-  model_coefficients[1] = p1p0[2] * p2p0[0] - p1p0[0] * p2p0[2];
-  model_coefficients[2] = p1p0[0] * p2p0[1] - p1p0[1] * p2p0[0];
-  model_coefficients[3] = 0.0f;
-
-  // Normalize
-  model_coefficients.normalize ();
+  model_coefficients.template head<3>() = cross / crossNorm;
 
   // ... + d = 0
-  model_coefficients[3] = -1.0f * (model_coefficients.template head<4>().dot (p0.matrix ()));
+  model_coefficients[3] = -1.0f * (model_coefficients.template head<3>().dot (p0));
 
   PCL_DEBUG ("[pcl::SampleConsensusModelPlane::computeModelCoefficients] Model is (%g,%g,%g,%g).\n",
              model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3]);

test/sample_consensus/test_sample_consensus_line_models.cpp

@@ -149,6 +149,89 @@ TEST (SampleConsensusModelLine, OnGroundPlane)
   EXPECT_NEAR (0, coeff[5], 1e-4);
 }
 
+TEST (SampleConsensusModelLine, SampleValidationPointsEqual)
+{
+  PointCloud<PointXYZ> cloud;
+  cloud.resize (3);
+
+  // The "cheat point" makes it possible to find a set of valid samples and
+  // therefore avoids the log message of an unsuccessful sample validation
+  // being printed a 1000 times without any chance of success.
+  // The order is chosen such that with a known, fixed rng-state/-seed all
+  // validation steps are actually exercised.
+  const pcl::index_t firstKnownEqualPoint = 0;
+  const pcl::index_t secondKnownEqualPoint = 1;
+  const pcl::index_t cheatPointIndex = 2;
+
+  cloud[firstKnownEqualPoint].getVector3fMap () <<  0.1f,  0.0f,  0.0f;
+  cloud[secondKnownEqualPoint].getVector3fMap () <<  0.1f,  0.0f,  0.0f;
+  cloud[cheatPointIndex].getVector3fMap () <<  0.0f,  0.1f,  0.0f; // <-- cheat point
+
+  // Create a shared line model pointer directly and explicitly disable the
+  // random seed for the reasons mentioned above
+  SampleConsensusModelLinePtr model (
+    new SampleConsensusModelLine<PointXYZ> (cloud.makeShared (), /* random = */ false));
+
+  // Algorithm tests
+  pcl::Indices samples;
+  int iterations = 0;
+  model->getSamples(iterations, samples);
+  EXPECT_EQ (samples.size(), 2);
+  // The "cheat point" has to be part of the sample, otherwise something is wrong.
+  // The best option would be to assert on stderr output here, but that doesn't
+  // seem to be that simple.
+  EXPECT_TRUE (std::find(samples.begin (), samples.end (), cheatPointIndex) != samples.end ());
+
+  pcl::Indices forcedSamples = {firstKnownEqualPoint, secondKnownEqualPoint};
+  Eigen::VectorXf modelCoefficients;
+  EXPECT_FALSE (model->computeModelCoefficients (forcedSamples, modelCoefficients));
+}
+
+TEST (SampleConsensusModelLine, SampleValidationPointsValid)
+{
+  PointCloud<PointXYZ> cloud;
+  cloud.resize (2);
+
+  // These two points only differ in one coordinate so this also acts as a
+  // regression test for 36c2bd6209f87dc7c6f56e2c0314e19f9cab95ec
+  cloud[0].getVector3fMap () <<  0.0f,  0.0f,  0.0f;
+  cloud[1].getVector3fMap () <<  0.1f,  0.0f,  0.0f;
+
+  // Create a shared line model pointer directly
+  SampleConsensusModelLinePtr model (new SampleConsensusModelLine<PointXYZ> (cloud.makeShared ()));
+
+  // Algorithm tests
+  pcl::Indices samples;
+  int iterations = 0;
+  model->getSamples(iterations, samples);
+  EXPECT_EQ (samples.size(), 2);
+
+  pcl::Indices forcedSamples = {0, 1};
+  Eigen::VectorXf modelCoefficients;
+  EXPECT_TRUE (model->computeModelCoefficients (forcedSamples, modelCoefficients));
+}
+
+TEST (SampleConsensusModelLine, SampleValidationNotEnoughSamples)
+{
+  PointCloud<PointXYZ> cloud;
+  cloud.resize (1);
+
+  cloud[0].getVector3fMap () <<  0.1f,  0.0f,  0.0f;
+
+  // Create a shared line model pointer directly
+  SampleConsensusModelLinePtr model (new SampleConsensusModelLine<PointXYZ> (cloud.makeShared ()));
+
+  // Algorithm tests
+  pcl::Indices samples;
+  int iterations = 0;
+  model->getSamples(iterations, samples);
+  EXPECT_EQ (samples.size(), 0);
+
+  pcl::Indices forcedSamples = {0,};
+  Eigen::VectorXf modelCoefficients;
+  EXPECT_FALSE (model->computeModelCoefficients (forcedSamples, modelCoefficients));
+}
+
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 TEST (SampleConsensusModelParallelLine, RANSAC)
 {