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 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 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 | #include <iostream>
#include <pcl/point_types.h>
#include <pcl/filters/model_outlier_removal.h>
int
main ()
{
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
int noise_size = 5;
int 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;
// 2. filter sphere:
// 2.1 generate model:
// modelparameter for this sphere:
// 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);
sphere_filter.setInputCloud (cloud);
sphere_filter.filter (*cloud_sphere_filtered);
std::cerr << "Sphere after filtering: " << std::endl;
for (const auto& point: *cloud_sphere_filtered)
std::cout << " " << point.x << " " << point.y << " " << point.z << std::endl;
return (0);
}
|
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
int noise_size = 5;
int 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:
1 2 3 4 5 6 7 8 9 10 11 12 | cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
project(model_outlier_removal)
find_package(PCL 1.7 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
add_executable (model_outlier_removal model_outlier_removal.cpp)
target_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