diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_cylinder.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_cylinder.hpp index 6ae067ad3c4..a5ac5d59128 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_cylinder.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_cylinder.hpp @@ -94,29 +94,26 @@ pcl::SampleConsensusModelCylinder::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::computeModelCoefficients ( model_coefficients[4] = line_dir[1]; model_coefficients[5] = line_dir[2]; // cylinder radius - model_coefficients[6] = static_cast (sqrt (pcl::sqrPointToLineDistance (p1, line_pt, line_dir))); + model_coefficients[6] = static_cast ( + 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); diff --git a/test/sample_consensus/test_sample_consensus_quadric_models.cpp b/test/sample_consensus/test_sample_consensus_quadric_models.cpp index 75606bdcf17..17e54e8f231 100644 --- a/test/sample_consensus/test_sample_consensus_quadric_models.cpp +++ b/test/sample_consensus/test_sample_consensus_quadric_models.cpp @@ -1641,6 +1641,66 @@ TEST(SampleConsensusModelTorusSelfIntersectSpindle, RANSAC) EXPECT_NEAR(coeff[7], 0.25000000000000017, 1e-2); } +TEST(SampleConsensusModelCylinder, SampleIntersectingLines) +{ + PointCloud cloud; + PointCloud 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 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 cloud; + PointCloud 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 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) {