Filtering a PointCloud using ModelOutlierRemoval

This tutorial demonstrates how to extract parametric models for example for planes or spheres out of a PointCloud by using SAC_Models with known coefficients. If you don’t know the models coefficients take a look at the How to use Random Sample Consensus model tutorial.

The code

First, create a file, let’s call it model_outlier_removal.cpp, in your favorite editor, and place the following inside it:

 1#include <iostream>
 2#include <pcl/point_types.h>
 3#include <pcl/filters/model_outlier_removal.h>
 4
 5int
 6main ()
 7{
 8  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
 9  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_sphere_filtered (new pcl::PointCloud<pcl::PointXYZ>);
10
11  // 1. Generate cloud data
12  std::size_t noise_size = 5;
13  std::size_t sphere_data_size = 10;
14  cloud->width = noise_size + sphere_data_size;
15  cloud->height = 1;
16  cloud->points.resize (cloud->width * cloud->height);
17  // 1.1 Add noise
18  for (std::size_t i = 0; i < noise_size; ++i)
19  {
20    (*cloud)[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
21    (*cloud)[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
22    (*cloud)[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
23  }
24  // 1.2 Add sphere:
25  double rand_x1 = 1;
26  double rand_x2 = 1;
27  for (std::size_t i = noise_size; i < (noise_size + sphere_data_size); ++i)
28  {
29    // See: http://mathworld.wolfram.com/SpherePointPicking.html
30    while (pow (rand_x1, 2) + pow (rand_x2, 2) >= 1)
31    {
32      rand_x1 = (rand () % 100) / (50.0f) - 1;
33      rand_x2 = (rand () % 100) / (50.0f) - 1;
34    }
35    double pre_calc = sqrt (1 - pow (rand_x1, 2) - pow (rand_x2, 2));
36    (*cloud)[i].x = 2 * rand_x1 * pre_calc;
37    (*cloud)[i].y = 2 * rand_x2 * pre_calc;
38    (*cloud)[i].z = 1 - 2 * (pow (rand_x1, 2) + pow (rand_x2, 2));
39    rand_x1 = 1;
40    rand_x2 = 1;
41  }
42
43  std::cerr << "Cloud before filtering: " << std::endl;
44  for (const auto& point: *cloud)
45    std::cout << "    " << point.x << " " << point.y << " " << point.z << std::endl;
46
47  // 2. filter sphere:
48  // 2.1 generate model:
49  // modelparameter for this sphere:
50  // position.x: 0, position.y: 0, position.z:0, radius: 1
51  pcl::ModelCoefficients sphere_coeff;
52  sphere_coeff.values.resize (4);
53  sphere_coeff.values[0] = 0;
54  sphere_coeff.values[1] = 0;
55  sphere_coeff.values[2] = 0;
56  sphere_coeff.values[3] = 1;
57
58  pcl::ModelOutlierRemoval<pcl::PointXYZ> sphere_filter;
59  sphere_filter.setModelCoefficients (sphere_coeff);
60  sphere_filter.setThreshold (0.05);
61  sphere_filter.setModelType (pcl::SACMODEL_SPHERE);
62  sphere_filter.setInputCloud (cloud);
63  sphere_filter.filter (*cloud_sphere_filtered);
64
65  std::cerr << "Sphere after filtering: " << std::endl;
66  for (const auto& point: *cloud_sphere_filtered)
67    std::cout << "    " << point.x << " " << point.y << " " << point.z << std::endl;
68
69  return (0);
70}

The explanation

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

In the following lines, we define the PointClouds structures, fill in noise, random points on a plane as well as random points on a sphere and display its content to screen.

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

  // 1. Generate cloud data
  std::size_t noise_size = 5;
  std::size_t sphere_data_size = 10;
  cloud->width = noise_size + sphere_data_size;
  cloud->height = 1;
  cloud->points.resize (cloud->width * cloud->height);
  // 1.1 Add noise
  for (std::size_t i = 0; i < noise_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);
  }
  // 1.2 Add sphere:
  double rand_x1 = 1;
  double rand_x2 = 1;
  for (std::size_t i = noise_size; i < (noise_size + sphere_data_size); ++i)
  {
    // See: http://mathworld.wolfram.com/SpherePointPicking.html
    while (pow (rand_x1, 2) + pow (rand_x2, 2) >= 1)
    {
      rand_x1 = (rand () % 100) / (50.0f) - 1;
      rand_x2 = (rand () % 100) / (50.0f) - 1;
    }
    double pre_calc = sqrt (1 - pow (rand_x1, 2) - pow (rand_x2, 2));
    (*cloud)[i].x = 2 * rand_x1 * pre_calc;
    (*cloud)[i].y = 2 * rand_x2 * pre_calc;
    (*cloud)[i].z = 1 - 2 * (pow (rand_x1, 2) + pow (rand_x2, 2));
    rand_x1 = 1;
    rand_x2 = 1;
  }

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

Finally we extract the sphere using ModelOutlierRemoval.

  // position.x: 0, position.y: 0, position.z:0, radius: 1
  pcl::ModelCoefficients sphere_coeff;
  sphere_coeff.values.resize (4);
  sphere_coeff.values[0] = 0;
  sphere_coeff.values[1] = 0;
  sphere_coeff.values[2] = 0;
  sphere_coeff.values[3] = 1;

  pcl::ModelOutlierRemoval<pcl::PointXYZ> sphere_filter;
  sphere_filter.setModelCoefficients (sphere_coeff);
  sphere_filter.setThreshold (0.05);
  sphere_filter.setModelType (pcl::SACMODEL_SPHERE);

Compiling and running the program

Add the following lines to your CMakeLists.txt file:

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

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

$ ./model_outlier_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
  -0.82 -0.341666 0.4592
  -0.728589 0.667873 0.152
  -0.3134 -0.873043 -0.3736
  0.62553 0.590779 0.5096
  -0.54048 0.823588 -0.172
  -0.707627 0.424576 0.5648
  -0.83153 0.523556 0.1856
  -0.513903 -0.719464 0.4672
  0.291534 0.692393 0.66
  0.258758 0.654505 -0.7104
Sphere after filtering:
  -0.82 -0.341666 0.4592
  -0.728589 0.667873 0.152
  -0.3134 -0.873043 -0.3736
  0.62553 0.590779 0.5096
  -0.54048 0.823588 -0.172
  -0.707627 0.424576 0.5648
  -0.83153 0.523556 0.1856
  -0.513903 -0.719464 0.4672
  0.291534 0.692393 0.66
  0.258758 0.654505 -0.7104