Removing outliers using a custom non-destructive condition

This document demonstrates how to use the FunctionFilter class to remove points from a PointCloud that do not satisfy a custom criteria. This is a cleaner and faster appraoch compared to ConditionalRemoval filter or a custom Condition class.

Advanced users can use the FunctorFilter class that can provide a small but measurable speedup when used with a lambda.

The code

First, create a file, let’s say, sphere_removal.cpp in you favorite editor, and place the following inside it:

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
#include <pcl/common/generate.h>
#include <pcl/filters/experimental/functor_filter.h>
#include <pcl/point_types.h>

#include <iostream>

int
main(int argc, char** argv)
{
  using XYZCloud = pcl::PointCloud<pcl::PointXYZ>;
  const auto cloud = pcl::make_shared<XYZCloud>();
  const auto filtered_cloud = pcl::make_shared<XYZCloud>();

  // Create a random generator to fill in the cloud
  pcl::common::CloudGenerator<pcl::PointXYZ, pcl::common::UniformGenerator<float>>
      generator{{-2.0, 2, 1234}};
  generator.fill(10, 1, *cloud);

  std::cerr << "Cloud before filtering: " << std::endl;
  for (const auto& pt : *cloud)
    std::cerr << "    " << pt.x << " " << pt.y << " " << pt.z << std::endl;

  // Setup a condition to reject points inside a filter
  const Eigen::Vector3f center{0, 0, 2};
  const float radius = 2;

  pcl::experimental::FilterFunction<pcl::PointXYZ> filter;
  filter = [=](const XYZCloud& cloud, pcl::index_t idx) {
    return ((cloud[idx].getVector3fMap() - center).norm() >= radius);
  };

  // build the filter
  pcl::experimental::FunctionFilter<pcl::PointXYZ> func_filter(filter);
  func_filter.setInputCloud(cloud);

  // apply filter
  func_filter.filter(*filtered_cloud);

  // display pointcloud after filtering
  std::cerr << "Cloud after filtering: " << std::endl;
  for (const auto& pt : *filtered_cloud)
    std::cerr << "    " << pt.x << " " << pt.y << " " << pt.z << std::endl;

  return (0);
}

The explanation

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

In the following lines, we define the PointCloud structures, fill in the input cloud, and display its content to screen.

Then, we create the condition which a given point must satisfy so that it remains in our PointCloud. To do this we create a std::function which accepts a PointCloud by const reference and an index, and returns true only if the point lies inside a sphere. This is then used to build the filter

This last bit of code just applies the filter to our original PointCloud, and removes all of the points that do not satisfy the conditions we specified. Then it outputs all of the points remaining in the PointCloud.

  // apply filter
  func_filter.filter(*filtered_cloud);

  // display pointcloud after filtering
  std::cerr << "Cloud after filtering: " << std::endl;
  for (const auto& pt : *filtered_cloud)
    std::cerr << "    " << pt.x << " " << pt.y << " " << pt.z << std::endl;

Compiling and running the program

Add the following lines to your CMakeLists.txt file:

1
2
3
4
5
6
7
8
9
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

project(function_filter)

find_package(PCL 1.11.1.99 REQUIRED)

add_executable (sphere_removal sphere_removal.cpp)
target_link_libraries (sphere_removal ${PCL_LIBRARIES})
add_definitions(${PCL_DEFINITIONS})

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

$ ./sphere_removal

You will see something similar to:

Cloud before filtering:
    -1.23392 1.81505 -0.968005
    -0.00934529 1.36497 0.158734
    0.488435 1.96851 -0.0534078
    1.27135 1.16404 -1.00462
    -0.249089 -0.0815883 1.13229
    0.448447 1.48914 1.78378
    1.14143 1.77363 1.68965
    1.08544 -1.01664 -1.13041
    1.1199 0.9951 -1.13308
    1.44268 -1.44434 -0.391739
Cloud after filtering:
    -1.23392 1.81505 -0.968005
    -0.00934529 1.36497 0.158734
    0.488435 1.96851 -0.0534078
    1.27135 1.16404 -1.00462
    1.14143 1.77363 1.68965
    1.08544 -1.01664 -1.13041
    1.1199 0.9951 -1.13308
    1.44268 -1.44434 -0.391739