Extend tests of sample validation in sample consensus models

Special emphasis is placed on tests with collinear inputs.

Author: alexvorndran

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)
 {

test/sample_consensus/test_sample_consensus_plane_models.cpp

@@ -134,6 +134,97 @@ TEST (SampleConsensusModelPlane, Base)
   ASSERT_EQ (indices_.size (), indices->size ());
 }
 
+TEST (SampleConsensusModelPlane, SampleValidationPointsCollinear)
+{
+  PointCloud<PointXYZ> cloud;
+  cloud.resize (4);
+
+  // 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 firstCollinearPointIndex = 0;
+  const pcl::index_t secondCollinearPointIndex = 1;
+  const pcl::index_t thirdCollinearPointIndex = 2;
+  const pcl::index_t cheatPointIndex = 3;
+
+  cloud[firstCollinearPointIndex].getVector3fMap () <<  0.1f,  0.1f,  0.1f;
+  cloud[secondCollinearPointIndex].getVector3fMap () <<  0.2f,  0.2f,  0.2f;
+  cloud[thirdCollinearPointIndex].getVector3fMap () <<  0.3f,  0.3f,  0.3f;
+  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
+  SampleConsensusModelPlanePtr model (
+    new SampleConsensusModelPlane<PointXYZ> (cloud.makeShared (), /* random = */ false));
+
+  // Algorithm tests
+  pcl::Indices samples;
+  int iterations = 0;
+  model->getSamples(iterations, samples);
+  EXPECT_EQ (samples.size(), 3);
+  // 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 = {firstCollinearPointIndex, secondCollinearPointIndex, thirdCollinearPointIndex};
+  Eigen::VectorXf modelCoefficients;
+  EXPECT_FALSE (model->computeModelCoefficients (forcedSamples, modelCoefficients));
+}
+
+TEST (SampleConsensusModelPlane, SampleValidationPointsValid)
+{
+  PointCloud<PointXYZ> cloud;
+  cloud.resize (3);
+
+  cloud[0].getVector3fMap () <<  0.1f,  0.0f,  0.0f;
+  cloud[1].getVector3fMap () <<  0.0f,  0.1f,  0.0f;
+  cloud[2].getVector3fMap () <<  0.0f,  0.0f,  0.1f;
+
+  // Create a shared line model pointer directly
+  SampleConsensusModelPlanePtr model (
+    new SampleConsensusModelPlane<PointXYZ> (cloud.makeShared ()));
+
+  // Algorithm tests
+  pcl::Indices samples;
+  int iterations = 0;
+  model->getSamples(iterations, samples);
+  EXPECT_EQ (samples.size(), 3);
+
+  Eigen::VectorXf modelCoefficients;
+  EXPECT_TRUE (model->computeModelCoefficients (samples, modelCoefficients));
+}
+
+TEST (SampleConsensusModelPlane, SampleValidationNotEnoughSamples)
+{
+  PointCloud<PointXYZ> cloud;
+  cloud.resize (2);
+
+  cloud[0].getVector3fMap () <<  0.1f,  0.0f,  0.0f;
+  cloud[1].getVector3fMap () <<  0.0f,  0.1f,  0.0f;
+
+  std::vector<pcl::Indices> testIndices = {{}, {0,}, {0, 1}};
+
+  for( const auto& indices : testIndices) {
+    PointCloud<PointXYZ> subCloud {cloud, indices};
+
+    // Create a shared line model pointer directly
+    SampleConsensusModelPlanePtr model (
+      new SampleConsensusModelPlane<PointXYZ> (subCloud.makeShared ()));
+
+    // Algorithm tests
+    pcl::Indices samples;
+    int iterations = 0;
+    model->getSamples(iterations, samples);
+    EXPECT_EQ (samples.size(), 0);
+
+    Eigen::VectorXf modelCoefficients;
+    EXPECT_FALSE (model->computeModelCoefficients (indices, modelCoefficients));
+  }
+}
+
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 TEST (SampleConsensusModelPlane, RANSAC)
 {

test/sample_consensus/test_sample_consensus_quadric_models.cpp

@@ -520,6 +520,89 @@ TEST (SampleConsensusModelCircle2D, RANSAC)
   EXPECT_NEAR ( 1, coeff_refined[2], 1e-3);
 }
 
+///////////////////////////////////////////////////////////////////////////////
+
+TEST (SampleConsensusModelCircle2D, SampleValidationPointsValid)
+{
+  PointCloud<PointXYZ> cloud;
+  cloud.resize (3);
+
+  cloud[0].getVector3fMap () <<  1.0f,  2.0f,  0.0f;
+  cloud[1].getVector3fMap () <<  0.0f,  1.0f,  0.0f;
+  cloud[2].getVector3fMap () <<  1.5f,  0.0f,  0.0f;
+
+  // Create a shared line model pointer directly
+  SampleConsensusModelCircle2DPtr model (
+    new SampleConsensusModelCircle2D<PointXYZ> (cloud.makeShared ()));
+
+  // Algorithm tests
+  pcl::Indices samples;
+  int iterations = 0;
+  model->getSamples(iterations, samples);
+  EXPECT_EQ (samples.size(), 3);
+
+  Eigen::VectorXf modelCoefficients;
+  EXPECT_TRUE (model->computeModelCoefficients (samples, modelCoefficients));
+
+  EXPECT_NEAR (modelCoefficients[0], 1.05f, 1e-3);   // center x
+  EXPECT_NEAR (modelCoefficients[1], 0.95f, 1e-3);   // center y
+  EXPECT_NEAR (modelCoefficients[2], std::sqrt (1.105f), 1e-3);  // radius
+}
+
+using PointVector = std::vector<PointXYZ>;
+class SampleConsensusModelCircle2DCollinearTest
+  : public testing::TestWithParam<PointVector> {
+  protected:
+    void SetUp() override {
+      pointCloud_ = make_shared<PointCloud<PointXYZ>>();
+      for(const auto& point : GetParam()) {
+        pointCloud_->emplace_back(point);
+      }
+    }
+
+    PointCloud<PointXYZ>::Ptr pointCloud_ = nullptr;
+};
+
+// Parametric tests clearly show the input for which they failed and provide
+// clearer feedback if something is changed in the future.
+TEST_P (SampleConsensusModelCircle2DCollinearTest, SampleValidationPointsInvalid)
+{
+  ASSERT_NE (pointCloud_, nullptr);
+
+  SampleConsensusModelCircle2DPtr model (
+      new SampleConsensusModelCircle2D<PointXYZ> (pointCloud_));
+  ASSERT_GE (model->getInputCloud ()->size (), model->getSampleSize ());
+
+  // Algorithm tests
+  // (Maybe) TODO: try to implement the "cheat point" trick from
+  //               test_sample_consensus_line_models.cpp and friends here, too.
+  // pcl::Indices samples;
+  // int iterations = 0;
+  // model->getSamples(iterations, samples);
+  // EXPECT_EQ (samples.size(), 0);
+
+  // Explicitly enforce sample order so they can act as designed
+  pcl::Indices forcedSamples = {0, 1, 2};
+  Eigen::VectorXf modelCoefficients;
+  EXPECT_FALSE (model->computeModelCoefficients (forcedSamples, modelCoefficients));
+}
+
+INSTANTIATE_TEST_SUITE_P (CollinearInputs, SampleConsensusModelCircle2DCollinearTest,
+  testing::Values( // Throw in some negative coordinates and "asymmetric" points to cover more cases
+    PointVector {{ 0.0f,  0.0f, 0.0f}, { 1.0f,  0.0f, 0.0f}, { 2.0f,  0.0f, 0.0f}}, // collinear along x-axis
+    PointVector {{-1.0f,  0.0f, 0.0f}, { 1.0f,  0.0f, 0.0f}, { 2.0f,  0.0f, 0.0f}},
+    PointVector {{ 0.0f, -1.0f, 0.0f}, { 0.0f,  1.0f, 0.0f}, { 0.0f,  2.0f, 0.0f}}, // collinear along y-axis
+    PointVector {{ 0.0f, -1.0f, 0.0f}, { 0.0f,  1.0f, 0.0f}, { 0.0f,  2.0f, 0.0f}},
+    PointVector {{ 0.0f,  0.0f, 0.0f}, { 1.0f,  1.0f, 0.0f}, { 2.0f,  2.0f, 0.0f}}, // collinear along diagonal
+    PointVector {{ 0.0f,  0.0f, 0.0f}, {-1.0f, -1.0f, 0.0f}, {-2.0f, -2.0f, 0.0f}},
+    PointVector {{-3.0f, -6.5f, 0.0f}, {-2.0f, -0.5f, 0.0f}, {-1.5f,  2.5f, 0.0f}}, // other collinear input
+    PointVector {{ 2.0f,  2.0f, 0.0f}, { 0.0f,  0.0f, 0.0f}, { 0.0f,  0.0f, 0.0f}}, // two points equal
+    PointVector {{ 0.0f,  0.0f, 0.0f}, { 2.0f,  2.0f, 0.0f}, { 0.0f,  0.0f, 0.0f}},
+    PointVector {{ 0.0f,  0.0f, 0.0f}, { 0.0f,  0.0f, 0.0f}, { 2.0f,  2.0f, 0.0f}},
+    PointVector {{ 1.0f,  1.0f, 0.0f}, { 1.0f,  1.0f, 0.0f}, { 1.0f,  1.0f, 0.0f}}  // all points equal
+  )
+);
+
 //////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT>
 class SampleConsensusModelCircle2DTest : private SampleConsensusModelCircle2D<PointT>