Better Cylinder Model Sampling (#6342)

* Add cylinder sampling test with intersecting lines

* Compute axis direction using cross product

* Add symmetry cylinder sampling test

* Estimate cylinder radius using both points

Author: akretz

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

@@ -94,29 +94,26 @@ pcl::SampleConsensusModelCylinder<PointT, PointNT>::computeModelCoefficients (
   Eigen::Vector4f n1 ((*normals_)[samples[0]].normal[0], (*normals_)[samples[0]].normal[1], (*normals_)[samples[0]].normal[2], 0.0f);
   Eigen::Vector4f n2 ((*normals_)[samples[1]].normal[0], (*normals_)[samples[1]].normal[1], (*normals_)[samples[1]].normal[2], 0.0f);
   Eigen::Vector4f w = n1 + p1 - p2;
+  Eigen::Vector4f line_dir = n1.cross3 (n2);
 
-  float a = n1.dot (n1);
   float b = n1.dot (n2);
   float c = n2.dot (n2);
   float d = n1.dot (w);
   float e = n2.dot (w);
-  float denominator = a*c - b*b;
-  float sc, tc;
+  float denominator = line_dir.squaredNorm ();
+  float sc;
   // Compute the line parameters of the two closest points
   if (denominator < 1e-8)          // The lines are almost parallel
   {
     sc = 0.0f;
-    tc = (b > c ? d / b : e / c);  // Use the largest denominator
   }
   else
   {
     sc = (b*e - c*d) / denominator;
-    tc = (a*e - b*d) / denominator;
   }
 
   // point_on_axis, axis_direction
   Eigen::Vector4f line_pt  = p1 + n1 + sc * n1;
-  Eigen::Vector4f line_dir = p2 + tc * n2 - line_pt;
   line_dir.normalize ();
 
   model_coefficients.resize (model_size_);
@@ -129,7 +126,9 @@ pcl::SampleConsensusModelCylinder<PointT, PointNT>::computeModelCoefficients (
   model_coefficients[4] = line_dir[1];
   model_coefficients[5] = line_dir[2];
   // cylinder radius
-  model_coefficients[6] = static_cast<float> (sqrt (pcl::sqrPointToLineDistance (p1, line_pt, line_dir)));
+  model_coefficients[6] = static_cast<float> (
+      0.5 * (sqrt (pcl::sqrPointToLineDistance (p1, line_pt, line_dir)) +
+             sqrt (pcl::sqrPointToLineDistance (p2, line_pt, line_dir))));
 
   if (model_coefficients[6] > radius_max_ || model_coefficients[6] < radius_min_)
     return (false);

test/sample_consensus/test_sample_consensus_quadric_models.cpp

@@ -1660,6 +1660,66 @@ TEST(SampleConsensusModelTorusSelfIntersectSpindle, RANSAC)
   EXPECT_NEAR(coeff[7], 0.25000000000000017, 1e-2);
 }
 
+TEST(SampleConsensusModelCylinder, SampleIntersectingLines)
+{
+  PointCloud<PointXYZ> cloud;
+  PointCloud<Normal> normals;
+  cloud.resize(2);
+  normals.resize(2);
+  cloud[0].getVector3fMap() << 0.0f, 1.0f, 0.0f;
+  cloud[1].getVector3fMap() << 1.0f, 0.0f, 0.0f;
+  normals[0].getNormalVector3fMap() << 0.0f, 1.0f, 0.0f;
+  normals[1].getNormalVector3fMap() << 1.0f, 0.0f, 0.0f;
+
+  SampleConsensusModelCylinder<PointXYZ, Normal> model(cloud.makeShared());
+  model.setInputNormals(normals.makeShared());
+
+  Eigen::VectorXf model_coefficients;
+  model.computeModelCoefficients({0, 1}, model_coefficients);
+
+  ASSERT_EQ(7, model_coefficients.size());
+
+  EXPECT_NEAR(0.0f, model_coefficients[0], 1e-5f);
+  EXPECT_NEAR(0.0f, model_coefficients[1], 1e-5f);
+  EXPECT_NEAR(0.0f, model_coefficients[3], 1e-5f);
+  EXPECT_NEAR(0.0f, model_coefficients[4], 1e-5f);
+  EXPECT_LT(0.0f, std::abs(model_coefficients[5]));
+  EXPECT_NEAR(1.0f, model_coefficients[6], 1e-5f);
+}
+
+TEST(SampleConsensusModelCylinder, SampleModelsSymmetric)
+{
+  PointCloud<PointXYZ> cloud;
+  PointCloud<Normal> normals;
+  cloud.resize(2);
+  normals.resize(2);
+  cloud[0].getVector3fMap() << 0.1f, 1.3f, 0.4f;
+  cloud[1].getVector3fMap() << 1.2f, -0.2f, -0.1f;
+  normals[0].getNormalVector3fMap() << 0.0f, 0.9f, -0.1f;
+  normals[1].getNormalVector3fMap() << 1.1f, -0.2f, 0.0f;
+
+  SampleConsensusModelCylinder<PointXYZ, Normal> model(cloud.makeShared());
+  model.setInputNormals(normals.makeShared());
+
+  Eigen::VectorXf model_coefficients, model_coefficients_swapped;
+  model.computeModelCoefficients({0, 1}, model_coefficients);
+  model.computeModelCoefficients({1, 0}, model_coefficients_swapped);
+
+  ASSERT_EQ(7, model_coefficients.size());
+  ASSERT_EQ(7, model_coefficients_swapped.size());
+
+  Eigen::Vector4f pt1(model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.f);
+  Eigen::Vector4f dir1(model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.f);
+  Eigen::Vector4f pt2(model_coefficients_swapped[0], model_coefficients_swapped[1], model_coefficients_swapped[2], 0.f);
+  Eigen::Vector4f dir2(model_coefficients_swapped[3], model_coefficients_swapped[4], model_coefficients_swapped[5], 0.f);
+
+  // Check if both cylinder axes are the same
+  EXPECT_GT(1e-5, sqrPointToLineDistance(pt2, pt1, dir1));
+  EXPECT_GT(1e-5f, (dir1 * (dir2.x() / dir1.x()) - dir2).squaredNorm());
+
+  EXPECT_NEAR(model_coefficients[6], model_coefficients_swapped[6], 1e-5f);
+}
+
 int
 main(int argc, char** argv)
 {