Point Cloud Registration – Part 1: Filter and Feature Estimation

This part of the registration tutorial will cover the CustomCloud class, which includes all the processing of point clouds before ICP.

CustomCloud

The entire code for CustomCloud class is shown below:

CustomCloud.h

#ifndef CUSTOMCLOUD_H
#define CUSTOMCLOUD_H

#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/shot_omp.h>
#include <pcl/filters/passthrough.h>
#include <pcl/io/pcd_io.h>
#include <pcl/keypoints/uniform_sampling.h>

typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> PointCloud;
typedef pcl::PointXYZRGBNormal PointNormalT;
typedef pcl::PointCloud<PointNormalT> PointCloudWithNormals;
typedef pcl::SHOT352 FeatureT;
// typedef pcl::SHOT1344 FeatureT;
typedef pcl::PointCloud<FeatureT> FeatureCloud;
typedef pcl::SHOTEstimationOMP<PointT, PointNormalT, FeatureT> FeatureEstimationT;
// typedef pcl::SHOTColorEstimationOMP<PointT, PointNormalT, FeatureT> FeatureEstimationT;

class CustomCloud
{
  public:

    CustomCloud ();

    CustomCloud (float rl, float ls1, float ls2, float k, float fr, u_int i);

    void setParameters (float rl, float ls1, float ls2, float k, 
        float fr, u_int i);

    void setInputCloud (PointCloud::ConstPtr xyz);

    void loadInputCloud (const std::string& pcd_file);
    
    u_int identity ();

    PointCloud::ConstPtr getPointCloud () const;

    PointCloud::Ptr getFilteredPointCloud () const;

    PointCloudWithNormals::Ptr getPointCloudWithNormals () const;

    FeatureCloud::Ptr getFeatureCloud () const;

  private:

    void processInput ();

    void filter ();

    void computePointCloudWithNormals ();

    void computeFeatureCloud ();

    // Point cloud data
    PointCloud::Ptr cloud_;
    PointCloud::Ptr f_cloud_;

    PointCloudWithNormals::Ptr normals_;

    FeatureCloud::Ptr features_;  

    // Parameters
    float range_limit_z;  
    float leaf_size_1_;
    float leaf_size_2_;
    float normal_k_;
    float feature_radius_;

    u_int identity_;
};

#endif

CustomCloud.cpp

#include "CustomCloud.h"

CustomCloud::CustomCloud() : 
  range_limit_z (1.5), 
  leaf_size_1_ (0.01), 
  leaf_size_2_ (0.03),
  normal_k_ (20),
  feature_radius_ (0.1),
  identity_ (0)
{}  

CustomCloud::CustomCloud(float rl, float ls1, float ls2, float k, float fr, u_int i) 
{
  setParameters(rl, ls1, ls2, k, fr, i);
}  

void CustomCloud::setParameters (float rl, float ls1, float ls2, float k, 
  float fr, u_int i)
{
  range_limit_z = rl; 
  leaf_size_1_ = ls1;
  leaf_size_2_ = ls2;
  normal_k_ = k;
  feature_radius_ = fr;
  identity_ = i;
}

// Process the given cloud
void CustomCloud::setInputCloud(PointCloud::ConstPtr xyz)
{
  cloud_ = PointCloud::Ptr(new PointCloud);
  copyPointCloud(*xyz, *cloud_);
  ++ identity_; 
  processInput();
}  

// Load and process the cloud in the given PCD file
void CustomCloud::loadInputCloud(const std::string& pcd_file)
{
  cloud_ = PointCloud::Ptr(new PointCloud);
  pcl::io::loadPCDFile(pcd_file, *cloud_);
  processInput();
}

u_int CustomCloud::identity()
{
  return (identity_);
}  

// Get a pointer to the cloud 3D points
PointCloud::ConstPtr CustomCloud::getPointCloud() const
{
  return (cloud_);
}  

// Get a pointer to the filtered cloud 3D points
PointCloud::Ptr CustomCloud::getFilteredPointCloud() const
{
  return (f_cloud_);
}  

// Get a pointer to the cloud of 3D surface normals
PointCloudWithNormals::Ptr CustomCloud::getPointCloudWithNormals() const
{
  return (normals_);
}    

// Get a pointer to the cloud of feature descriptors
FeatureCloud::Ptr CustomCloud::getFeatureCloud() const
{
  return (features_);
}

// Compute the surface normals and local features
void CustomCloud::processInput()
{
  filter();
  computePointCloudWithNormals();
  computeFeatureCloud();
}    

// Remove points beyond a certain z limit
void CustomCloud::filter()
{
  PointCloud::Ptr temp(new PointCloud);
  f_cloud_ = PointCloud::Ptr(new PointCloud);        

  pcl::PassThrough<PointT> range_filter;
  // Filter out all points with Z values not in the [0 - range_limit_z] range.
  range_filter.setFilterFieldName("z");
  range_filter.setFilterLimits(0, range_limit_z);
  range_filter.setInputCloud(cloud_);
  range_filter.filter(*temp);      

  // Uniform sampling object.
  pcl::UniformSampling<PointT> uniform_filter;
  uniform_filter.setInputCloud(temp);
  // We set the size of every voxel to be leaf_size_1_
  // (only one point per every (leaf_size_1_^3 cubic meter will survive).
  uniform_filter.setRadiusSearch(leaf_size_1_);
  // We need an additional object to store the indices of surviving points.
  pcl::PointCloud<int> keypointIndices;
  uniform_filter.compute(keypointIndices);
  copyPointCloud(*temp, keypointIndices.points, *f_cloud_);
}  

// Compute the surface normals
void CustomCloud::computePointCloudWithNormals()
{
  normals_ = PointCloudWithNormals::Ptr(new PointCloudWithNormals);
  pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);      

  pcl::NormalEstimationOMP<PointT, PointNormalT> norm_est;
  norm_est.setInputCloud(f_cloud_);
  norm_est.setSearchMethod(tree);
  norm_est.setKSearch(normal_k_);
  norm_est.compute(*normals_);      

  copyPointCloud(*f_cloud_, *normals_);
}      

// Compute the local feature descriptors
void CustomCloud::computeFeatureCloud()
{
  PointCloud::Ptr temp (new PointCloud);

  // Uniform sampling object.
  pcl::UniformSampling<PointT> uniform_filter;
  uniform_filter.setInputCloud(f_cloud_);
  // We set the size of every voxel to be leaf_size_2_
  // (only one point per every (leaf_size_2_)^3 cubic meter will survive).
  uniform_filter.setRadiusSearch(leaf_size_2_);
  // We need an additional object to store the indices of surviving points.
  pcl::PointCloud<int> keypointIndices;
  uniform_filter.compute(keypointIndices);
  copyPointCloud(*f_cloud_, keypointIndices.points, *temp);

  features_ = FeatureCloud::Ptr(new FeatureCloud);
  pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);      

  FeatureEstimationT feat_est;
  feat_est.setSearchSurface (f_cloud_);
  feat_est.setInputCloud (temp);
  feat_est.setInputNormals (normals_);
  feat_est.setSearchMethod (tree);
  feat_est.setRadiusSearch (feature_radius_);
  feat_est.compute (*features_);

  f_cloud_.swap(temp);
}

Notice that we used several typedef’s in the CustomCloud class. The typedef’s simplify the code we need to write and make changing data structures easier.

setParameters

void CustomCloud::setParameters (float rl, float ls1, float ls2, float k, 
  float fr, u_int i)
{
  range_limit_z = rl; 
  leaf_size_1_ = ls1;
  leaf_size_2_ = ls2;
  normal_k_ = k;
  feature_radius_ = fr;
  identity_ = i;
}

In order to allow the user to easily change the parameters on filtering and normal estimation, we’ve added a setParameters function that allows the user to change parameter from the main method. The parameters passed are then stored in private variables. The parameters, in order, are range limit, first leaf size, second leaf size, normal search k value, feature estimation radius, and identity. T

Filter

In the filter function, we will use two filters to downsample our code and remove possible noise and clutter from the point cloud.

The first filter is a PassThrough filter, which can remove any points whose coordinates do not fall in a certain range. In this case, we limit the coordinates in the z-direction:

range_filter.setFilterFieldName("z");
range_filter.setFilterLimits(0, range_limit_z)

This will filter out all points with z-values not in the 0 to range_limit_z range.

range_filter.setInputCloud(cloud_);
range_filter.filter(*temp);

The filtered cloud is passed into a temp PointCloud, since we don’t want to pass the filtered cloud back into our original cloud.

Next filter is UniformSampling, which downsamples points based on voxels, little cube-shaped regions with size defined by the user. The UniformSampling class is the same as the VoxelGrid class except it outputs the indices of the points that survived the process.

We load the input cloud into the filter and set the leaf size, which defines the size of the voxel. Then we use a PointCloud of ints (which basically is the same as a std::vector<int>) to store the indices of the surviving points.

uniform_filter.setInputCloud(temp);
// We set the size of every voxel to be leaf_size_1_
// (only one point per every (leaf_size_1_^3 cubic meter will survive).
uniform_filter.setRadiusSearch(leaf_size_1_);
// We need an additional object to store the indices of surviving points.
pcl::PointCloud<int> keypointIndices;
uniform_filter.compute(keypointIndices);

Using the copyPointCloud function, we copy the surviving points into the f_cloud_ based on the keypointIndices.

copyPointCloud(*temp, keypointIndices.points, *f_cloud_);

Normal Estimation

Normals are one of the most common and basic features used in PCL. To compute normals, we must first make an object that stores the normals in a Point Cloud.

normals_ = PointCloudWithNormals::Ptr(new PointCloudWithNormals);

Notice that PointCloudWithNormals is actually pcl::PointCloud <pcl::PointXYZRGBNormal> >. The XYZ and RGB values in normal can be used in case the user wants to use curvature as part of their registration requirements in ICP.

We also make a KdTree object, which organizes a set of points in a k-dimensional space that makes range search operations very efficient. The KdTree will be passed later to normal estimation as our search method.

pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);

Then we set the input cloud, the search method, and the normal k value used in the normal estimation and compute the normal cloud:

pcl::NormalEstimationOMP<PointT, PointNormalT> norm_est;
norm_est.setInputCloud(f_cloud_);
norm_est.setSearchMethod(tree);
norm_est.setKSearch(normal_k_);
norm_est.compute(*normals_);

Notice that we use the OpenMP version of the NormalEstimation class, which speeds up the computation process through multi-threading.

Finally, we copy the XYZRGB values from f_cloud_ into normals_.

copyPointCloud(*f_cloud_, *normals_)

Feature Estimation

Although normals are features in the PCL library, they are too simple for registration and easily skewed by noise and clutter. Instead, we use the normals to compute higher-level features. In this case, we are using SHOT, which stands for Signature of Histograms of Orientations. It encodes information about the topology (surface within a spherical support structure). SHOT makes use of a local reference frame, making it rotation invariant. It is also robust to noise and clutter. For more information, please refer to this academic paper: http://vision.deis.unibo.it/fede/papers/eccv10.pdf

Before we start the feature estimation, we must downsample the cloud a second time. This is needed because using all the points to calculate features and conduct ICP will greatly slow down the program.

  // Uniform sampling object.
  pcl::UniformSampling<PointT> uniform_filter;
  uniform_filter.setInputCloud(f_cloud_);
  // We set the size of every voxel to be leaf_size_2_
  // (only one point per every (leaf_size_2_)^3 cubic meter will survive).
  uniform_filter.setRadiusSearch(leaf_size_2_);
  // We need an additional object to store the indices of surviving points.
  pcl::PointCloud<int> keypointIndices;
  uniform_filter.compute(keypointIndices);
  copyPointCloud(*f_cloud_, keypointIndices.points, *temp);

Similar to normal estimation, we must create a point cloud that stores our features and a search method object used in the feature estimation.

  features_ = FeatureCloud::Ptr(new FeatureCloud);
  pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);

Then we pass in the input clouds, the KdTree, and the normals computed from the last function.

  FeatureEstimationT feat_est;
  feat_est.setSearchSurface (f_cloud_);
  feat_est.setInputCloud (temp);
  feat_est.setInputNormals (normals_);
  feat_est.setSearchMethod (tree);
  feat_est.setRadiusSearch (feature_radius_);

Notice that besides using setInputCloud, we also use setSearchSurface. The setSearchSurface method lets the user to set an optional high-resolution cloud to improve feature estimation’s accuracy. In this case, we pass in the first downsampled cloud, but we only calculate features at points from second downsampled cloud.

Finally, we compute the features and swap the first downsampled cloud with the second downsampled cloud for registration.

  feat_est.compute (*features_);
  f_cloud_.swap(temp);

Helpful Resources:

http://robotica.unileon.es/mediawiki/index.php/PCL/OpenNI_tutorial_2:_Cloud_processing_%28basic%29: This tutorial from University of Leon contains good information on various types of filters and normal estimation.

http://robotica.unileon.es/mediawiki/index.php/PCL/OpenNI_tutorial_4:_3D_object_recognition_%28descriptors%29: This is another tutorial page from University of Leon that explains the various types of features in PCL.

http://pointclouds.org/documentation/tutorials/how_features_work.php: This is a tutorial page from PCL that explains how normal and estimation and feature estimation work.