Downsampling a PointCloud using a VoxelGrid filter

In this tutorial we will learn how to downsample – that is, reduce the number of points – a point cloud dataset, using a voxelized grid approach.

The VoxelGrid class that we’re about to present creates a 3D voxel grid (think about a voxel grid as a set of tiny 3D boxes in space) over the input point cloud data. Then, in each voxel (i.e., 3D box), all the points present will be approximated (i.e., downsampled) with their centroid. This approach is a bit slower than approximating them with the center of the voxel, but it represents the underlying surface more accurately.

The code

First, download the dataset table_scene_lms400.pcd and save it somewhere to disk.

Then, create a file, let’s say, voxel_grid.cpp in your favorite editor, and place the following inside it:

 1#include <iostream>
 2#include <pcl/io/pcd_io.h>
 3#include <pcl/point_types.h>
 4#include <pcl/filters/voxel_grid.h>
 5
 6int
 7main ()
 8{
 9  pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2 ());
10  pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());
11
12  // Fill in the cloud data
13  pcl::PCDReader reader;
14  // Replace the path below with the path where you saved your file
15  reader.read ("table_scene_lms400.pcd", *cloud); // Remember to download the file first!
16
17  std::cerr << "PointCloud before filtering: " << cloud->width * cloud->height 
18       << " data points (" << pcl::getFieldsList (*cloud) << ")." << std::endl;
19
20  // Create the filtering object
21  pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
22  sor.setInputCloud (cloud);
23  sor.setLeafSize (0.01f, 0.01f, 0.01f);
24  sor.filter (*cloud_filtered);
25
26  std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height 
27       << " data points (" << pcl::getFieldsList (*cloud_filtered) << ")." << std::endl;
28
29  pcl::PCDWriter writer;
30  writer.write ("table_scene_lms400_downsampled.pcd", *cloud_filtered, 
31         Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), false);
32
33  return (0);
34}

The explanation

Now, let’s break down the code piece by piece.

The following lines of code will read the point cloud data from disk.

  // Fill in the cloud data
  pcl::PCDReader reader;
  // Replace the path below with the path where you saved your file
  reader.read ("table_scene_lms400.pcd", *cloud); // Remember to download the file first!

Then, a pcl::VoxelGrid filter is created with a leaf size of 1cm, the input data is passed, and the output is computed and stored in cloud_filtered.

  pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
  sor.setInputCloud (cloud);
  sor.setLeafSize (0.01f, 0.01f, 0.01f);
  sor.filter (*cloud_filtered);

Finally, the data is written to disk for later inspection.

  pcl::PCDWriter writer;
  writer.write ("table_scene_lms400_downsampled.pcd", *cloud_filtered, 
         Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), false);

Compiling and running the program

Add the following lines to your CMakeLists.txt file:

 1cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 2
 3project(voxel_grid)
 4
 5find_package(PCL 1.2 REQUIRED)
 6
 7include_directories(${PCL_INCLUDE_DIRS})
 8link_directories(${PCL_LIBRARY_DIRS})
 9add_definitions(${PCL_DEFINITIONS})
10
11add_executable (voxel_grid voxel_grid.cpp)
12target_link_libraries (voxel_grid ${PCL_LIBRARIES})

After you have made the executable, you can run it. Simply do:

$ ./voxel_grid

You will see something similar to:

PointCloud before filtering: 460400 data points (x y z intensity distance sid).
PointCloud after filtering: 41049 data points (x y z intensity distance sid).