From f0274f571bb49f9254ab375313d2a8390098d49a Mon Sep 17 00:00:00 2001 From: Sergey Alexandrov Date: Thu, 10 Jan 2019 18:02:36 +0100 Subject: [PATCH] Add a new test for SampleConsensusModelLine All input points are on the ground plane. This test passes after #2767 and fails before. --- .../test_sample_consensus_line_models.cpp | 42 +++++++++++++++++++ 1 file changed, 42 insertions(+) diff --git a/test/sample_consensus/test_sample_consensus_line_models.cpp b/test/sample_consensus/test_sample_consensus_line_models.cpp index 6ef73dd4ee2..c6a59a9708f 100644 --- a/test/sample_consensus/test_sample_consensus_line_models.cpp +++ b/test/sample_consensus/test_sample_consensus_line_models.cpp @@ -107,6 +107,48 @@ TEST (SampleConsensusModelLine, RANSAC) EXPECT_XYZ_NEAR (PointXYZ (16.0, 17.0, 18.0), proj_points.points[5], 1e-4); } +TEST (SampleConsensusModelLine, OnGroundPlane) +{ + PointCloud cloud; + cloud.points.resize (10); + + // All the points are on the ground plane (z=0). + // The line is parallel to the x axis, so all the inlier points have the same z and y coordinates. + cloud.points[0].getVector3fMap () << 0.0f, 0.0f, 0.0f; + cloud.points[1].getVector3fMap () << 1.0f, 0.0f, 0.0f; + cloud.points[2].getVector3fMap () << 2.0f, 0.0f, 0.0f; + cloud.points[3].getVector3fMap () << 3.0f, 0.0f, 0.0f; + cloud.points[4].getVector3fMap () << 4.0f, 0.0f, 0.0f; + cloud.points[5].getVector3fMap () << 5.0f, 0.0f, 0.0f; + // Outliers + cloud.points[6].getVector3fMap () << 2.1f, 2.0f, 0.0f; + cloud.points[7].getVector3fMap () << 5.0f, 4.1f, 0.0f; + cloud.points[8].getVector3fMap () << 0.4f, 1.3f, 0.0f; + cloud.points[9].getVector3fMap () << 3.3f, 0.1f, 0.0f; + + // Create a shared line model pointer directly + SampleConsensusModelLinePtr model (new SampleConsensusModelLine (cloud.makeShared ())); + + // Create the RANSAC object + RandomSampleConsensus sac (model, 0.001); + + // Algorithm tests + bool result = sac.computeModel (); + ASSERT_TRUE (result); + + std::vector inliers; + sac.getInliers (inliers); + EXPECT_EQ (6, inliers.size ()); + + Eigen::VectorXf coeff; + sac.getModelCoefficients (coeff); + EXPECT_EQ (6, coeff.size ()); + + EXPECT_NE (0, coeff[3]); + EXPECT_NEAR (0, coeff[4], 1e-4); + EXPECT_NEAR (0, coeff[5], 1e-4); +} + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// TEST (SampleConsensusModelParallelLine, RANSAC) {