Fix bug in SampleModelConsensusCylinder::projectPoints()

[fl0ydj]

As proposed by @kunaltyagi in #3876 .

I am unsure about the math behind it but this change does it for me.
However, if copy_data_fields is false, this change does not help.

[kunaltyagi]

The issue lies in dir = p - pp

• p has w == 1 (which is good for most cases)
• pp doesn't have w == 1 because the copy was made per-field-list

[kunaltyagi]

Please add a test-case to ensure no regression

[fl0ydj]

Ok, I have implemented your proposed cheanges.

Testcase (from the cylinder model segmentation)

Use the pointcloud provided in the cylinder model segmentation.

#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>
#include <pcl/features/normal_3d.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
typedef pcl::PointXYZ PointT;

int
main (int argc, char** argv)
{
  // All the objects needed
  pcl::PCDReader reader;
  pcl::PassThrough<PointT> pass;
  pcl::NormalEstimation<PointT, pcl::Normal> ne;
  pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg; 
  pcl::PCDWriter writer;
  pcl::ExtractIndices<PointT> extract;
  pcl::ExtractIndices<pcl::Normal> extract_normals;
  pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());

  // Datasets
  pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
  pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT>);
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
  pcl::PointCloud<PointT>::Ptr cloud_filtered2 (new pcl::PointCloud<PointT>);
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal>);
  pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients), coefficients_cylinder (new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices), inliers_cylinder (new pcl::PointIndices);

  // Read in the cloud data
  reader.read ("table_scene_mug_stereo_textured.pcd", *cloud);
  std::cerr << "PointCloud has: " << cloud->points.size () << " data points." << std::endl;

  // Build a passthrough filter to remove spurious NaNs
  pass.setInputCloud (cloud);
  pass.setFilterFieldName ("z");
  pass.setFilterLimits (0, 1.5);
  pass.filter (*cloud_filtered);
  std::cerr << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl;

  // Estimate point normals
  ne.setSearchMethod (tree);
  ne.setInputCloud (cloud_filtered);
  ne.setKSearch (50);
  ne.compute (*cloud_normals);

  // Create the segmentation object for the planar model and set all the parameters
  seg.setOptimizeCoefficients (true);
  seg.setModelType (pcl::SACMODEL_NORMAL_PLANE);
  seg.setNormalDistanceWeight (0.1);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setMaxIterations (100);
  seg.setDistanceThreshold (0.03);
  seg.setInputCloud (cloud_filtered);
  seg.setInputNormals (cloud_normals);
  // Obtain the plane inliers and coefficients
  seg.segment (*inliers_plane, *coefficients_plane);
  std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl;

  // Extract the planar inliers from the input cloud
  extract.setInputCloud (cloud_filtered);
  extract.setIndices (inliers_plane);
  extract.setNegative (false);

  // Write the planar inliers to disk
  pcl::PointCloud<PointT>::Ptr cloud_plane (new pcl::PointCloud<PointT> ());
  extract.filter (*cloud_plane);
  std::cerr << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
  writer.write ("table_scene_mug_stereo_textured_plane.pcd", *cloud_plane, false);

  // Remove the planar inliers, extract the rest
  extract.setNegative (true);
  extract.filter (*cloud_filtered2);
  extract_normals.setNegative (true);
  extract_normals.setInputCloud (cloud_normals);
  extract_normals.setIndices (inliers_plane);
  extract_normals.filter (*cloud_normals2);

  // Create the segmentation object for cylinder segmentation and set all the parameters
  seg.setOptimizeCoefficients (true);
  seg.setModelType (pcl::SACMODEL_CYLINDER);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setNormalDistanceWeight (0.1);
  seg.setMaxIterations (10000);
  seg.setDistanceThreshold (0.05);
  seg.setRadiusLimits (0, 0.1);
  seg.setInputCloud (cloud_filtered2);
  seg.setInputNormals (cloud_normals2);

  // Obtain the cylinder inliers and coefficients
  seg.segment (*inliers_cylinder, *coefficients_cylinder);
  std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl;
  // Write the cylinder inliers to disk
  extract.setInputCloud (cloud_filtered2);
  extract.setIndices (inliers_cylinder);
  extract.setNegative (false);
  pcl::PointCloud<PointT>::Ptr cloud_cylinder (new pcl::PointCloud<PointT> ());
  extract.filter (*cloud_cylinder);
  if (cloud_cylinder->points.empty ()) 
    std::cerr << "Can't find the cylindrical component." << std::endl;
  else
  {
          pcl::ProjectInliers<PointT> proj;
          proj.setModelType(pcl::SACMODEL_CYLINDER);
          proj.setInputCloud(cloud_cylinder);
          proj.setModelCoefficients(coefficients_cylinder);
          proj.filter(*cloud_cylinder);
	  std::cerr << "PointCloud representing the cylindrical component: " << cloud_cylinder->points.size () << " data points." << std::endl;
	  writer.write ("table_scene_mug_stereo_textured_cylinder.pcd", *cloud_cylinder, false);
  }
  return (0);
}

I hope thats ok :) If you need anything more, I'll be happy to help.

[kunaltyagi]

I hope thats ok

I was looking more for a test in the tests/sample_consensus/tests_sample_consensus_cylinder.cpp (See the plane model test file for example).

The test doesn't need that many moving pieces. @SergioRAgostinho could you help to whittle down the pieces needed to test this?

[fl0ydj]

Oh ok sorry ._. I am pretty new to all of this! I will try coding something as well.

[kunaltyagi]

No issues, we can walk you through the process of adding the test.

Can you verify that this resolves the issue in all cases? If so, we can try to see if replacing Vector4 with Vector3 has exactly the same results (because we don't seem to be using the 4th component, which is giving the issues)

[fl0ydj]

Mmh, so I am not quite sure, when you actually would use copy_data_fields. Is it for organized point clouds?
But if 'copy_data_fields' is false, our code works for me.
I will try something with 3 dimensions.

[kunaltyagi]

Needs a newline as the last character of the text file

[kunaltyagi]

3d based code compiles. If it works, we can start with the tests. 😄

[fl0ydj]

Excuse my late reply. Yeah it works :)

[kunaltyagi]

The test can be pretty simple. In your example, the relevant lines are:

pcl::ProjectInliers<PointT> proj;
proj.setModelType(pcl::SACMODEL_CYLINDER);
proj.setInputCloud(cloud_cylinder);
proj.setModelCoefficients(coefficients_cylinder);
proj.filter(*cloud_cylinder);
std::cerr << "PointCloud representing the cylindrical component: " << cloud_cylinder->points.size () << " data points." << std::endl;

What you need to do:

1. Create a file and add the boiler plate
2. Create a model coefficients_cylinder (or a list over which to test)
3. Create a fake cloud cloud_cylinder which is spread over a range of parameters (for ease of testing, concentric with the model)
4. Confirm that the output (aka cloud_cylinder) matches the expectation

Details on the testing framework GTest are here. You can also use other test files as an example by throwing away code you don't need

[stale]

Marking this as stale due to 30 days of inactivity. Commenting or adding a new commit to the pull request will revert this.