Add a test for SAC model parallel line

Additionaly rename test_sample_consensus_line_model to reflect the fact that now it tests multiple line models.
PointCloudLibrary · Oct 23, 2014 · bd3ae1e · bd3ae1e
1 parent 8c848e5
commit bd3ae1e
Show file tree

Hide file tree

Showing 2 changed files with 64 additions and 4 deletions.
diff --git a/test/sample_consensus/CMakeLists.txt b/test/sample_consensus/CMakeLists.txt
@@ -11,6 +11,6 @@ PCL_ADD_TEST(sample_consensus_quadric_models test_sample_consensus_quadric_model
              FILES test_sample_consensus_quadric_models.cpp
              LINK_WITH pcl_gtest pcl_sample_consensus)
 
-PCL_ADD_TEST(sample_consensus_line_model test_sample_consensus_line_model
-             FILES test_sample_consensus_line_model.cpp
+PCL_ADD_TEST(sample_consensus_line_models test_sample_consensus_line_models
+             FILES test_sample_consensus_line_models.cpp
              LINK_WITH pcl_gtest pcl_sample_consensus)
diff --git a/...nsus/test_sample_consensus_line_model.cpp → ...sus/test_sample_consensus_line_models.cpp b/...nsus/test_sample_consensus_line_model.cpp → ...sus/test_sample_consensus_line_models.cpp
@@ -42,16 +42,16 @@
 
 #include <pcl/sample_consensus/ransac.h>
 #include <pcl/sample_consensus/sac_model_line.h>
+#include <pcl/sample_consensus/sac_model_parallel_line.h>
 
 using namespace pcl;
 
 typedef SampleConsensusModelLine<PointXYZ>::Ptr SampleConsensusModelLinePtr;
+typedef SampleConsensusModelParallelLine<PointXYZ>::Ptr SampleConsensusModelParallelLinePtr;
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 TEST (SampleConsensusModelLine, RANSAC)
 {
-  srand (0);
-
   // Use a custom point cloud for these tests until we need something better
   PointCloud<PointXYZ> cloud;
   cloud.points.resize (10);
@@ -106,6 +106,66 @@ TEST (SampleConsensusModelLine, RANSAC)
   EXPECT_XYZ_NEAR (PointXYZ (16.0, 17.0, 18.0), proj_points.points[5], 1e-4);
 }
 
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+TEST (SampleConsensusModelParallelLine, RANSAC)
+{
+  PointCloud<PointXYZ> cloud (16, 1);
+
+  // Line 1
+  cloud.points[0].getVector3fMap () <<  1.0f,  2.00f,  3.00f;
+  cloud.points[1].getVector3fMap () <<  4.0f,  5.00f,  6.00f;
+  cloud.points[2].getVector3fMap () <<  7.0f,  8.00f,  9.00f;
+  cloud.points[3].getVector3fMap () << 10.0f, 11.00f, 12.00f;
+  cloud.points[4].getVector3fMap () << 13.0f, 14.00f, 15.00f;
+  cloud.points[5].getVector3fMap () << 16.0f, 17.00f, 18.00f;
+  cloud.points[6].getVector3fMap () << 19.0f, 20.00f, 21.00f;
+  cloud.points[7].getVector3fMap () << 22.0f, 23.00f, 24.00f;
+  // Random points
+  cloud.points[8].getVector3fMap () << -5.0f,  1.57f,  0.75f;
+  cloud.points[9].getVector3fMap () <<  4.0f,  2.00f,  3.00f;
+  // Line 2 (parallel to the Z axis)
+  cloud.points[10].getVector3fMap () << -1.00f,  5.00f,  0.0f;
+  cloud.points[11].getVector3fMap () << -1.05f,  5.01f,  1.0f;
+  cloud.points[12].getVector3fMap () << -1.01f,  5.05f,  2.0f;
+  cloud.points[13].getVector3fMap () << -1.05f,  5.01f,  3.0f;
+  cloud.points[14].getVector3fMap () << -1.01f,  5.05f,  4.0f;
+  cloud.points[15].getVector3fMap () << -1.05f,  5.01f,  5.0f;
+
+  // Create a shared line model pointer directly
+  SampleConsensusModelParallelLinePtr model (new SampleConsensusModelParallelLine<PointXYZ> (cloud.makeShared ()));
+  model->setAxis (Eigen::Vector3f (0, 0, 1));
+  model->setEpsAngle (0.1);
+
+  // Create the RANSAC object
+  RandomSampleConsensus<PointXYZ> sac (model, 0.1);
+
+  // Algorithm tests
+  bool result = sac.computeModel ();
+  ASSERT_TRUE (result);
+
+  std::vector<int> sample;
+  sac.getModel (sample);
+  EXPECT_EQ (2, sample.size ());
+
+  std::vector<int> inliers;
+  sac.getInliers (inliers);
+  EXPECT_EQ (6, inliers.size ());
+
+  Eigen::VectorXf coeff;
+  sac.getModelCoefficients (coeff);
+  EXPECT_EQ (6, coeff.size ());
+  EXPECT_NEAR (0, coeff[3], 1e-4);
+  EXPECT_NEAR (0, coeff[4], 1e-4);
+  EXPECT_NEAR (1, coeff[5], 1e-4);
+
+  // Projection tests
+  PointCloud<PointXYZ> proj_points;
+  model->projectPoints (inliers, coeff, proj_points);
+
+  EXPECT_XYZ_NEAR (PointXYZ (-1.05, 5.05, 3.0), proj_points.points[13], 0.1);
+  EXPECT_XYZ_NEAR (PointXYZ (-1.05, 5.05, 4.0), proj_points.points[14], 0.1);
+}
+
 int
 main (int argc, char** argv)
 {