Merge pull request #5062 from mvieth/revert-2562

Revert "Implement `SampleConsensusModelSphere<PointT>::projectPoints`…

Author: kunaltyagi

sample_consensus/include/pcl/sample_consensus/impl/sac_model_sphere.hpp

@@ -366,84 +366,24 @@ pcl::SampleConsensusModelSphere<PointT>::optimizeModelCoefficients (
 //////////////////////////////////////////////////////////////////////////
 template <typename PointT> void
 pcl::SampleConsensusModelSphere<PointT>::projectPoints (
-      const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const
+      const Indices &, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool) const
 {
-  // Needs a valid set of model coefficients
+  // Needs a valid model coefficients
   if (!isModelValid (model_coefficients))
   {
     PCL_ERROR ("[pcl::SampleConsensusModelSphere::projectPoints] Given model is invalid!\n");
     return;
   }
 
+  // Allocate enough space and copy the basics
+  projected_points.resize (input_->size ());
   projected_points.header   = input_->header;
+  projected_points.width    = input_->width;
+  projected_points.height   = input_->height;
   projected_points.is_dense = input_->is_dense;
 
-  // C : sphere center
-  const Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
-  // r : radius
-  const double r = model_coefficients[3];
-
-  // Copy all the data fields from the input cloud to the projected one?
-  if (copy_data_fields)
-  {
-    // Allocate enough space and copy the basics
-    projected_points.resize (input_->size ());
-    projected_points.width    = input_->width;
-    projected_points.height   = input_->height;
-
-    using FieldList = typename pcl::traits::fieldList<PointT>::type;
-    // Iterate over each point
-    for (std::size_t i = 0; i < projected_points.points.size (); ++i)
-      // Iterate over each dimension
-      pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));
-
-    // Iterate through the 3d points and calculate the distances from them to the sphere
-    for (std::size_t i = 0; i < inliers.size (); ++i)
-    {
-      // what i have:
-      // P : Sample Point
-      const Eigen::Vector3d P (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z);
-
-      const Eigen::Vector3d direction = (P - C).normalized();
-
-      // K : Point on Sphere
-      const Eigen::Vector3d K = C + r * direction;
-
-      projected_points.points[inliers[i]].x = static_cast<float> (K[0]);
-      projected_points.points[inliers[i]].y = static_cast<float> (K[1]);
-      projected_points.points[inliers[i]].z = static_cast<float> (K[2]);
-    }
-  }
-  else
-  {
-    // Allocate enough space and copy the basics
-    projected_points.resize (inliers.size ());
-    projected_points.width    = static_cast<uint32_t> (inliers.size ());
-    projected_points.height   = 1;
-
-    using FieldList = typename pcl::traits::fieldList<PointT>::type;
-    // Iterate over each point
-    for (std::size_t i = 0; i < inliers.size (); ++i)
-      // Iterate over each dimension
-      pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i]));
-
-    // Iterate through the 3d points and calculate the distances from them to the plane
-    for (std::size_t i = 0; i < inliers.size (); ++i)
-    {
-      // what i have:
-      // P : Sample Point
-      const Eigen::Vector3d P (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z);
-
-      const Eigen::Vector3d direction = (P - C).normalized();
-
-      // K : Point on Sphere
-      const Eigen::Vector3d K = C + r * direction;
-
-      projected_points.points[i].x = static_cast<float> (K[0]);
-      projected_points.points[i].y = static_cast<float> (K[1]);
-      projected_points.points[i].z = static_cast<float> (K[2]);
-    }
-  }
+  PCL_WARN ("[pcl::SampleConsensusModelSphere::projectPoints] Not implemented yet.\n");
+  projected_points.points = input_->points;
 }
 
 //////////////////////////////////////////////////////////////////////////

test/sample_consensus/test_sample_consensus_quadric_models.cpp

@@ -651,50 +651,6 @@ TEST (SampleConsensusModelCircle3D, RANSAC)
   EXPECT_NEAR ( 0.0, coeff_refined[6], 1e-3);
 }
 
-TEST (SampleConsensusModelSphere, projectPoints)
-{
-  Eigen::VectorXf model_coefficients(4);
-  model_coefficients << -0.32, -0.89, 0.37, 0.12; // center and radius
-
-  pcl::PointCloud<pcl::PointXYZ>::Ptr input(new pcl::PointCloud<pcl::PointXYZ>);
-  input->emplace_back(-0.259754, -0.950873,  0.318377); // inlier, dist from center=0.10
-  input->emplace_back( 0.595892,  0.455094,  0.025545); // outlier, not projected
-  input->emplace_back(-0.221871, -0.973718,  0.353817); // inlier, dist from center=0.13
-  input->emplace_back(-0.332269, -0.848851,  0.437499); // inlier, dist from center=0.08
-  input->emplace_back(-0.242308, -0.561036, -0.365535); // outlier, not projected
-  input->emplace_back(-0.327668, -0.800009,  0.290988); // inlier, dist from center=0.12
-  input->emplace_back(-0.173948, -0.883831,  0.403625); // inlier, dist from center=0.15
-  input->emplace_back(-0.033891,  0.624537, -0.606994); // outlier, not projected
-
-  pcl::SampleConsensusModelSphere<pcl::PointXYZ> model(input);
-  pcl::Indices inliers = {0, 2, 3, 5, 6};
-
-  pcl::PointCloud<pcl::PointXYZ> projected_truth;
-  projected_truth.emplace_back(-0.247705, -0.963048, 0.308053);
-  projected_truth.emplace_back(-0.229419, -0.967278, 0.355062);
-  projected_truth.emplace_back(-0.338404, -0.828276, 0.471249);
-  projected_truth.emplace_back(-0.327668, -0.800009, 0.290988);
-  projected_truth.emplace_back(-0.203158, -0.885065, 0.396900);
-
-  pcl::PointCloud<pcl::PointXYZ> projected_points;
-  model.projectPoints(inliers, model_coefficients, projected_points, false);
-  EXPECT_EQ(projected_points.size(), 5);
-  for(int i=0; i<5; ++i)
-    EXPECT_XYZ_NEAR(projected_points[i], projected_truth[i], 1e-5);
-
-  pcl::PointCloud<pcl::PointXYZ> projected_points_all;
-  model.projectPoints(inliers, model_coefficients, projected_points_all, true);
-  EXPECT_EQ(projected_points_all.size(), 8);
-  EXPECT_XYZ_NEAR(projected_points_all[0], projected_truth[0], 1e-5);
-  EXPECT_XYZ_NEAR(projected_points_all[1],        (*input)[1], 1e-5);
-  EXPECT_XYZ_NEAR(projected_points_all[2], projected_truth[1], 1e-5);
-  EXPECT_XYZ_NEAR(projected_points_all[3], projected_truth[2], 1e-5);
-  EXPECT_XYZ_NEAR(projected_points_all[4],        (*input)[4], 1e-5);
-  EXPECT_XYZ_NEAR(projected_points_all[5], projected_truth[3], 1e-5);
-  EXPECT_XYZ_NEAR(projected_points_all[6], projected_truth[4], 1e-5);
-  EXPECT_XYZ_NEAR(projected_points_all[7],        (*input)[7], 1e-5);
-}
-
 TEST (SampleConsensusModelCylinder, projectPoints)
 {
   Eigen::VectorXf model_coefficients(7);