filters.h
Go to the documentation of this file.
00001 #ifndef FILTERS_H
00002 #define FILTERS_H
00003 
00004 #include <pcl/filters/passthrough.h>
00005 #include <pcl/filters/voxel_grid.h>
00006 #include <pcl/filters/radius_outlier_removal.h>
00007 
00008 #include "typedefs.h"
00009 
00010 /* Use a PassThrough filter to remove points with depth values that are too large or too small */
00011 PointCloudPtr
00012 thresholdDepth (const PointCloudPtr & input, float min_depth, float max_depth)
00013 {
00014   pcl::PassThrough<PointT> pass_through;
00015   pass_through.setInputCloud (input);
00016   pass_through.setFilterFieldName ("z");
00017   pass_through.setFilterLimits (min_depth, max_depth);
00018   PointCloudPtr thresholded (new PointCloud);
00019   pass_through.filter (*thresholded);
00020 
00021   return (thresholded);
00022 }
00023 
00024 /* Use a VoxelGrid filter to reduce the number of points */
00025 PointCloudPtr
00026 downsample (const PointCloudPtr & input, float leaf_size)
00027 {
00028   pcl::VoxelGrid<PointT> voxel_grid;
00029   voxel_grid.setInputCloud (input);
00030   voxel_grid.setLeafSize (leaf_size, leaf_size, leaf_size);
00031   PointCloudPtr downsampled (new PointCloud);
00032   voxel_grid.filter (*downsampled);
00033 
00034   return (downsampled);
00035 }
00036 
00037 /* Use a RadiusOutlierRemoval filter to remove all points with too few local neighbors */
00038 PointCloudPtr
00039 removeOutliers (const PointCloudPtr & input, float radius, int min_neighbors)
00040 {
00041   pcl::RadiusOutlierRemoval<pcl::PointXYZRGB> radius_outlier_removal;
00042   radius_outlier_removal.setInputCloud (input);
00043   radius_outlier_removal.setRadiusSearch (radius);
00044   radius_outlier_removal.setMinNeighborsInRadius (min_neighbors);
00045   PointCloudPtr inliers (new PointCloud);
00046   radius_outlier_removal.filter (*inliers);
00047 
00048   return (inliers);
00049 }
00050 
00051 /* Apply a series of filters (threshold depth, downsample, and remove outliers) */
00052 PointCloudPtr
00053 applyFilters (const PointCloudPtr & input, float min_depth, float max_depth, float leaf_size, float radius, 
00054               float min_neighbors)
00055 {
00056   PointCloudPtr filtered;
00057   filtered = thresholdDepth (input, min_depth, max_depth);
00058   filtered = downsample (filtered, leaf_size);
00059   filtered = removeOutliers (filtered, radius, min_neighbors);
00060 
00061   return (filtered);
00062 }
00063 
00064 #endif


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:24:08