1 #pragma once
2
3 #include <pcl/filters/passthrough.h>
4 #include <pcl/filters/voxel_grid.h>
5 #include <pcl/filters/radius_outlier_removal.h>
6
7 #include "typedefs.h"
8
9 /* Use a PassThrough filter to remove points with depth values that are too large or too small */
10 PointCloudPtr
thresholdDepth(const PointCloudPtr & input,float min_depth,float max_depth)11 thresholdDepth (const PointCloudPtr & input, float min_depth, float max_depth)
12 {
13 pcl::PassThrough<PointT> pass_through;
14 pass_through.setInputCloud (input);
15 pass_through.setFilterFieldName ("z");
16 pass_through.setFilterLimits (min_depth, max_depth);
17 PointCloudPtr thresholded (new PointCloud);
18 pass_through.filter (*thresholded);
19
20 return (thresholded);
21 }
22
23 /* Use a VoxelGrid filter to reduce the number of points */
24 PointCloudPtr
downsample(const PointCloudPtr & input,float leaf_size)25 downsample (const PointCloudPtr & input, float leaf_size)
26 {
27 pcl::VoxelGrid<PointT> voxel_grid;
28 voxel_grid.setInputCloud (input);
29 voxel_grid.setLeafSize (leaf_size, leaf_size, leaf_size);
30 PointCloudPtr downsampled (new PointCloud);
31 voxel_grid.filter (*downsampled);
32
33 return (downsampled);
34 }
35
36 /* Use a RadiusOutlierRemoval filter to remove all points with too few local neighbors */
37 PointCloudPtr
removeOutliers(const PointCloudPtr & input,float radius,int min_neighbors)38 removeOutliers (const PointCloudPtr & input, float radius, int min_neighbors)
39 {
40 pcl::RadiusOutlierRemoval<pcl::PointXYZRGB> radius_outlier_removal;
41 radius_outlier_removal.setInputCloud (input);
42 radius_outlier_removal.setRadiusSearch (radius);
43 radius_outlier_removal.setMinNeighborsInRadius (min_neighbors);
44 PointCloudPtr inliers (new PointCloud);
45 radius_outlier_removal.filter (*inliers);
46
47 return (inliers);
48 }
49
50 /* Apply a series of filters (threshold depth, downsample, and remove outliers) */
51 PointCloudPtr
applyFilters(const PointCloudPtr & input,float min_depth,float max_depth,float leaf_size,float radius,float min_neighbors)52 applyFilters (const PointCloudPtr & input, float min_depth, float max_depth, float leaf_size, float radius,
53 float min_neighbors)
54 {
55 PointCloudPtr filtered;
56 filtered = thresholdDepth (input, min_depth, max_depth);
57 filtered = downsample (filtered, leaf_size);
58 filtered = removeOutliers (filtered, radius, min_neighbors);
59
60 return (filtered);
61 }
62