Removing outliers using a ConditionalRemoval filter

This document demonstrates how to use the ConditionalRemoval filter to remove points from a PointCloud that do not satisfy a specific or multiple conditions.

The code

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

 1#include <iostream>
 2#include <pcl/point_types.h>
 3#include <pcl/filters/conditional_removal.h>
 4
 5int
 6 main ()
 7{
 8  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
 9  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
10
11  // Fill in the cloud data
12  cloud->width  = 5;
13  cloud->height = 1;
14  cloud->points.resize (cloud->width * cloud->height);
15
16  for (std::size_t i = 0; i < cloud->size (); ++i)
17  {
18    (*cloud)[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
19    (*cloud)[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
20    (*cloud)[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
21  }
22
23  std::cerr << "Cloud before filtering: " << std::endl;
24  for (std::size_t i = 0; i < cloud->size (); ++i)
25    std::cerr << "    " << (*cloud)[i].x << " "
26                        << (*cloud)[i].y << " "
27                        << (*cloud)[i].z << std::endl;
28  // build the condition
29  pcl::ConditionAnd<pcl::PointXYZ>::Ptr range_cond (new
30                  pcl::ConditionAnd<pcl::PointXYZ> ());
31  range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
32      pcl::FieldComparison<pcl::PointXYZ> ("z", pcl::ComparisonOps::GT, 0.0)));
33  range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
34      pcl::FieldComparison<pcl::PointXYZ> ("z", pcl::ComparisonOps::LT, 0.8)));
35
36  // build the filter
37  pcl::ConditionalRemoval<pcl::PointXYZ> condrem;
38  condrem.setCondition (range_cond);
39  condrem.setInputCloud (cloud);
40  condrem.setKeepOrganized (true);
41
42  // apply filter
43  condrem.filter (*cloud_filtered);
44
45  // display pointcloud after filtering
46  std::cerr << "Cloud after filtering: " << std::endl;
47  for (std::size_t i = 0; i < cloud_filtered->size (); ++i)
48    std::cerr << "    " << (*cloud_filtered)[i].x << " "
49                        << (*cloud_filtered)[i].y << " "
50                        << (*cloud_filtered)[i].z << std::endl;
51  return (0);
52}

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 it’s content to screen.

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);

  // Fill in the cloud data
  cloud->width  = 5;
  cloud->height = 1;
  cloud->points.resize (cloud->width * cloud->height);

  for (std::size_t i = 0; i < cloud->size (); ++i)
  {
    (*cloud)[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
    (*cloud)[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
    (*cloud)[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
  }

  std::cerr << "Cloud before filtering: " << std::endl;
  for (std::size_t i = 0; i < cloud->size (); ++i)
    std::cerr << "    " << (*cloud)[i].x << " "
                        << (*cloud)[i].y << " "
                        << (*cloud)[i].z << std::endl;

Then, we create the condition which a given point must satisfy so that it remains in our PointCloud. To do this we must add two comparisons to the condition, greater than 0.0, and less than 0.8. This condition is then used to build the filter.

  // build the condition
  pcl::ConditionAnd<pcl::PointXYZ>::Ptr range_cond (new
                  pcl::ConditionAnd<pcl::PointXYZ> ());
  range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
      pcl::FieldComparison<pcl::PointXYZ> ("z", pcl::ComparisonOps::GT, 0.0)));
  range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new
      pcl::FieldComparison<pcl::PointXYZ> ("z", pcl::ComparisonOps::LT, 0.8)));

  // build the filter
  pcl::ConditionalRemoval<pcl::PointXYZ> condrem;
  condrem.setCondition (range_cond);
  condrem.setInputCloud (cloud);
  condrem.setKeepOrganized (true);

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
  condrem.filter (*cloud_filtered);

  // display pointcloud after filtering
  std::cerr << "Cloud after filtering: " << std::endl;
  for (std::size_t i = 0; i < cloud_filtered->size (); ++i)
    std::cerr << "    " << (*cloud_filtered)[i].x << " "
                        << (*cloud_filtered)[i].y << " "
                        << (*cloud_filtered)[i].z << std::endl;

Compiling and running the program

Add the following lines to your CMakeLists.txt file:

 1cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 2
 3project(conditional_removal)
 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 (conditional_removal conditional_removal.cpp)
12target_link_libraries (conditional_removal ${PCL_LIBRARIES})

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

$ ./conditioinal_removal

You will see something similar to:

Cloud before filtering:
              0.352222 -0.151883 -0.106395
              -0.397406 -0.473106 0.292602
              -0.731898 0.667105 0.441304
              -0.734766 0.854581 -0.0361733
              -0.4607 -0.277468 -0.916762
Cloud after filtering:
              -0.397406 -0.473106 0.292602
              -0.731898 0.667105 0.441304