Skip to content

Lab 32: Point cloud segmentation via clustering

Jump to bottom
Vinitha Ranganeni edited this page May 3, 2022 · 13 revisions

In this lab, we will see how to analyze point clouds to understand where objects are in the environment. As we have seen, point clouds are just a list of XYZRGB points. Segmentation is the process of categorizing the points into semantic groups. For example, we will figure out which points represent the shelf and the different objects on the shelf.

We will first use the Euclidean Clustering algorithm for segmentation, as described in the PCL tutorials. Euclidean clustering essentially groups points that are close together. You must set a "closeness" threshold, such that points within this threshold are considered to be part of the same cluster. Another useful feature is that you can set a minimum and a maximum number of points that a cluster can contain. This helps to filter out noise (isolated single points).

We will be implementing our segmentation algorithms in segmentation.cpp and segmentation.h.

Setup

Create src/segmentation.cpp and include/perception/segmentation.h. Add a perception_segmentation library to your CMakeLists.txt as we did in the previous tutorials.

include/perception/segmentation.h

#include "pcl/PointIndices.h"
#include "pcl/point_cloud.h"
#include "pcl/point_types.h"
#include "ros/ros.h"
#include "sensor_msgs/PointCloud2.h"
#include "pcl/segmentation/extract_clusters.h"

namespace perception {

// Add function definitions here later

class Segmenter {
 public:
  Segmenter(const ros::Publisher& points_pub);
  void Callback(const sensor_msgs::PointCloud2& msg);

 private:
  ros::Publisher points_pub_;
};
}  // namespace perception

src/segmentation.cpp

#include "perception/segmentation.h"

#include "pcl/PointIndices.h"
#include "pcl/point_cloud.h"
#include "pcl/point_types.h"
#include "pcl_conversions/pcl_conversions.h"
#include "ros/ros.h"
#include "sensor_msgs/PointCloud2.h"

typedef pcl::PointXYZRGB PointC;
typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloudC;

namespace perception {
void SegmentBinObjects(PointCloudC::Ptr cloud, std::vector<pcl::PointIndices>* indices) {
}

Segmenter::Segmenter(const ros::Publisher& points_pub)
    : points_pub_(points_pub) {}

void Segmenter::Callback(const sensor_msgs::PointCloud2& msg) {
  PointCloudC::Ptr cloud(new PointCloudC());
  pcl::fromROSMsg(msg, *cloud);

}
}  // namespace perception

Edit your point_cloud_demo.cpp to use the segmenter:

src/point_cloud_demo.cpp

#include "perception/segmentation.h"
#include "ros/ros.h"
#include "sensor_msgs/PointCloud2.h"

int main(int argc, char** argv) {
  ros::init(argc, argv, "point_cloud_demo");
  ros::NodeHandle nh;
  ros::Publisher segment_pub =
      nh.advertise<sensor_msgs::PointCloud2>("segment_cloud", 1, true);
  perception::Segmenter segmenter(segment_pub);
  ros::Subscriber sub =
      nh.subscribe("cloud_in", 1, &perception::Segmenter::Callback, &segmenter);
  ros::spin();
  return 0;
}

Pause here and make sure that your code compiles.

PointIndices

A new data structure we are using is PointIndices. PointIndices are used to represent a subset of a point cloud, using a list of indices in the PointCloud's points field. Some PCL algorithms make use of PointIndices, while others don't. PointIndices is a wrapper around a std::vector<int>, which you can get using the indices field, like so:

PointCloudC::Ptr cloud(new PointCloudC);
pcl::PointIndices indices;
for (size_t i=0; i<indices.indices->size(); ++i) {
  int index = indices.indices[i];
  const PointC& pt = cloud->points[index];
}

To reify a point cloud from its indices, use pcl::ExtractIndices:

#include "pcl/filters/extract_indices.h"

// Given these data types:
PointCloudC::Ptr original_cloud(new PointCloudC);
pcl::PointIndices indices;
PointCloudC::Ptr subset_cloud(new PointCloudC);

// Extract subset of original_cloud into subset_cloud:
pcl::ExtractIndices<PointC> extract;
extract.setInputCloud(original_cloud);
extract.setIndices(indices);
extract.filter(*subset_cloud);

Crop the scene

Reinstate the smart cropper from Lab 30 so that we are working with a much smaller point cloud corresponding to one of the bins on the shelf:

Add object segmentation function

Add this function to segmentation.h and segmentation.cpp. Notice that we are representing objects not as miniature point clouds, but as PointIndices in the original point cloud. This technique is more efficient. It also gives you more flexibility, since it's always possible to reify the point cloud at a later time.

void SegmentBinObjects(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,
                           std::vector<pcl::PointIndices>* indices);

Now, in SegmentBinObjects, you can implement the clustering:

double cluster_tolerance;
int min_cluster_size, max_cluster_size;
ros::param::param("ec_cluster_tolerance", cluster_tolerance, 0.01);
ros::param::param("ec_min_cluster_size", min_cluster_size, 10);
ros::param::param("ec_max_cluster_size", max_cluster_size, 10000);

pcl::EuclideanClusterExtraction<PointC> euclid;
euclid.setInputCloud(cloud);
// euclid.setIndices(inside_bin_indices); // << TODO: is cloud already cropped or do we get indices for the crop?
euclid.setClusterTolerance(cluster_tolerance);
euclid.setMinClusterSize(min_cluster_size);
euclid.setMaxClusterSize(max_cluster_size);
euclid.extract(*indices);

// Find the size of the smallest and the largest object,
// where size = number of points in the cluster
size_t min_size = std::numeric_limits<size_t>::max();
size_t max_size = std::numeric_limits<size_t>::min();
for (size_t i = 0; i < indices->size(); ++i) {
  // TODO: implement this
  size_t cluster_size = ???;
}

ROS_INFO("Found %ld objects, min size: %ld, max size: %ld",
         indices->size(), min_size, max_size);

In the for loop, implement some code to figure out how many points are in the smallest cluster and how many points are in the largest cluster. This will be useful for tweaking your ec_min_cluster_size and ec_max_cluster_size params later.

In your Segmenter::Callback, call SegmentBinObjects:

std::vector<pcl::PointIndices> object_indices;
SegmentBinObjects(cloud, &object_indices);

Visualize the results

First, make sure to visualize the cropped point cloud corresponding to the bin of interest. By viewing only this point cloud in RViz, you can see what input you are giving to the Euclidean clustering algorithm: image

First implement the following function:

#include "geometry_msgs/Pose.h"
...
void GetAxisAlignedBoundingBox(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,
                               geometry_msgs::Pose* pose,
                               geometry_msgs::Vector3* dimensions)

This function should update the pose and dimensions of the bounding box.

Now, let's draw boxes around each of the objects.

#include "visualization_msgs/Marker.h"
#include <pcl/common/common.h>
...
for (size_t i = 0; i < object_indices.size(); ++i) {
  // Reify indices into a point cloud of the object.
  pcl::PointIndices::Ptr indices(new pcl::PointIndices);
  *indices = object_indices[i];
  PointCloudC::Ptr object_cloud(new PointCloudC());
  // TODO: fill in object_cloud using indices

  // Publish a bounding box around it.
  visualization_msgs::Marker object_marker;
  object_marker.ns = "objects";
  object_marker.id = i;
  object_marker.header.frame_id = "base_link";
  object_marker.type = visualization_msgs::Marker::CUBE;
  GetAxisAlignedBoundingBox(object_cloud, &object_marker.pose,
                            &object_marker.scale);
  object_marker.color.g = 1;
  object_marker.color.a = 0.3;
  marker_pub_.publish(object_marker);
}

Note that we have to create a unique ID for each object.

You should see something like this: image

Some of the objects are actually just noise in the data. Luckily, you can filter them out by increasing the minimum size of a cluster:

rosparam set ec_min_cluster_size 20

One issue that will occur is that if you change a parameter and the number of objects decreases, the box for the objects that disappeared will still be there. This is because RViz markers are "added" to the scene and are not deleted unless you specify action=DELETE. You can auto-delete them by setting the lifetime field on the marker, or you can refresh the RViz display by toggling the Marker display off and on.

As you adjust the minimum cluster size, you can look at the output of your node to see what the smallest cluster size is:

/point_cloud_demo SegmentBinObjects:102: Found 10 objects, min size: 11, max size: 5306
# Adjusted ec_min_cluster_size to 20
/point_cloud_demo SegmentBinObjects:102: Found 7 objects, min size: 27, max size: 5306

If you have objects that are close together, you can experiment with adjusting the ec_cluster_tolerance param:

rosparam set ec_cluster_tolerance 0.05

There might not be one parameter setting that correctly separates different objects while joining all parts of the same object together. This is a limitation of the Euclidean clustering algorithm. image

As you work on your projects, you should think about what perception challenges you have. Although the segmentation pipeline presented in this lab could be useful, you may also need to make tweaks or design your own algorithm.

Color and geometry-based clustering

Euclidean clustering works only if there is a gap in between the objects (as specified by ec_cluster_tolerance). If there is no gap between objects (e.g., two books placed next to each other in a shelf), then you may need to use a more advanced clustering algorithm. Next you will implement two other clustering algorithms and compare them to Euclidean clustering:

Both of these clustering algorithms work similarly to Euclidean clustering. However, they also give you the option to specify thresholds on the smoothness of the normals or on the average color. For example, the color-based region-growing segmentation can be used to segment two differently-colored books placed against each other.

Clone this wiki locally