Point Cloud Library (PCL)  1.7.2
filters.h
1 #ifndef FILTERS_H
2 #define FILTERS_H
3 
4 #include <pcl/filters/passthrough.h>
5 #include <pcl/filters/voxel_grid.h>
6 #include <pcl/filters/radius_outlier_removal.h>
7 
8 #include "typedefs.h"
9 
10 /* Use a PassThrough filter to remove points with depth values that are too large or too small */
11 PointCloudPtr
12 thresholdDepth (const PointCloudPtr & input, float min_depth, float max_depth)
13 {
14  pcl::PassThrough<PointT> pass_through;
15  pass_through.setInputCloud (input);
16  pass_through.setFilterFieldName ("z");
17  pass_through.setFilterLimits (min_depth, max_depth);
18  PointCloudPtr thresholded (new PointCloud);
19  pass_through.filter (*thresholded);
20 
21  return (thresholded);
22 }
23 
24 /* Use a VoxelGrid filter to reduce the number of points */
25 PointCloudPtr
26 downsample (const PointCloudPtr & input, float leaf_size)
27 {
28  pcl::VoxelGrid<PointT> voxel_grid;
29  voxel_grid.setInputCloud (input);
30  voxel_grid.setLeafSize (leaf_size, leaf_size, leaf_size);
31  PointCloudPtr downsampled (new PointCloud);
32  voxel_grid.filter (*downsampled);
33 
34  return (downsampled);
35 }
36 
37 /* Use a RadiusOutlierRemoval filter to remove all points with too few local neighbors */
38 PointCloudPtr
39 removeOutliers (const PointCloudPtr & input, float radius, int min_neighbors)
40 {
41  pcl::RadiusOutlierRemoval<pcl::PointXYZRGB> radius_outlier_removal;
42  radius_outlier_removal.setInputCloud (input);
43  radius_outlier_removal.setRadiusSearch (radius);
44  radius_outlier_removal.setMinNeighborsInRadius (min_neighbors);
45  PointCloudPtr inliers (new PointCloud);
46  radius_outlier_removal.filter (*inliers);
47 
48  return (inliers);
49 }
50 
51 /* Apply a series of filters (threshold depth, downsample, and remove outliers) */
52 PointCloudPtr
53 applyFilters (const PointCloudPtr & input, float min_depth, float max_depth, float leaf_size, float radius,
54  float min_neighbors)
55 {
56  PointCloudPtr filtered;
57  filtered = thresholdDepth (input, min_depth, max_depth);
58  filtered = downsample (filtered, leaf_size);
59  filtered = removeOutliers (filtered, radius, min_neighbors);
60 
61  return (filtered);
62 }
63 
64 #endif
void setFilterFieldName(const std::string &field_name)
Provide the name of the field to be used for filtering data.
Definition: passthrough.h:111
void filter(PointCloud &output)
Calls the filtering method and returns the filtered dataset in output.
Definition: filter.h:131
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data...
Definition: voxel_grid.h:178
RadiusOutlierRemoval filters points in a cloud based on the number of neighbors they have...
void setRadiusSearch(double radius)
Set the radius of the sphere that will determine which points are neighbors.
void setFilterLimits(const float &limit_min, const float &limit_max)
Set the numerical limits for the field for filtering data.
Definition: passthrough.h:131
PassThrough passes points in a cloud based on constraints for one particular field of the point type...
Definition: passthrough.h:80
void filter(PointCloud &output)
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: pcl_base.hpp:66
void setMinNeighborsInRadius(int min_pts)
Set the number of neighbors that need to be present in order to be classified as an inlier...
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
Definition: voxel_grid.h:223