PCL Tutorial

This tutorial will demonstrate the use of PCL for the problem of downsampling a PointCloud2 dataset to a sparser representation.

In addition, in step 2, we will attempt to segment the largest planar component present in the downsampled point cloud.

Step 1 : run the following

Please create a package in your ROS_PACKAGE_PATH using:

$ roscreate-pkg pcl_tutorial pcl

Edit CMakeLists.txt and add

rosbuild_add_executable (pcl_tutorial pcl_tutorial.cpp)

Add pcl_tutorial.cpp to your newly created package:

#include <pcl/filters/voxel_grid.h>
#include <pcl/point_types.h>

int
  main (int argc, char **argv)
{
  // init ROS
  ros::init (argc, argv, "pcl_demo");
  ros::NodeHandle nh;

  // Publishers
  point_cloud::Publisher<pcl::PointXYZ> pub_downsampled;
  pub_downsampled.advertise (nh, "downsampled", 1);
  
  // PCL objects
  pcl::PointCloud<pcl::PointXYZ> cloud, cloud_downsampled;
  pcl::VoxelGrid<pcl::PointXYZ> grid;
  grid.setFilterFieldName ("z");
  grid.setLeafSize (0.01, 0.01, 0.01);
  grid.setFilterLimits (0.4, 1.6);

  while (nh.ok ()) 
  {
    // Spin until we get a PointCloud2 message
    sensor_msgs::PointCloud2ConstPtr cloud2_blob_ptr = ros::topic::waitForMessage<sensor_msgs::PointCloud2>("/narrow_stereo_textured/points2");

    // Convert to PCL
    point_cloud::fromMsg (*cloud2_blob_ptr, cloud);

    // Downsample the data    
    grid.setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(cloud));
    grid.filter (cloud_downsampled);
    
    // Publish the data
    pub_downsampled.publish (cloud_downsampled);
  }
}

Step 2 : edit the above source file

Please edit the above source file and include the following code in the right places:

#include <pcl/filters/project_inliers.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/PointIndices.h>
#include <pcl/ModelCoefficients.h>

  // ...

point_cloud::Publisher<pcl::PointXYZ> pub_plane;
pub_plane.advertise (nh, "plane", 1);

  // ...

pcl::PointIndices plane_inliers;
pcl::ModelCoefficients plane_coefficients;
pcl::PointCloud<pcl::PointXYZ> cloud_plane;

  // ...

pcl::SACSegmentation<pcl::PointXYZ> seg;
seg.setDistanceThreshold (0.05);
seg.setMaxIterations (1000);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);

  // ...

pcl::ProjectInliers<pcl::PointXYZ> proj;
proj.setModelType (pcl::SACMODEL_PLANE);

  // ...

// Find the largest plane
seg.setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZ> > (cloud_downsampled));
seg.segment (plane_inliers, plane_coefficients);

  // ...

// Extract the inliers indices as a separate point cloud
proj.setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZ> > (cloud_downsampled));
proj.setIndices (boost::make_shared<pcl::PointIndices> (plane_inliers));
proj.setModelCoefficients (boost::make_shared<pcl::ModelCoefficients> (plane_coefficients));
proj.filter (cloud_plane);

  // ...

pub_plane.publish (cloud_plane);