Point Cloud Library (PCL)
1.7.0
|
00001 #ifndef SEGMENTATION_H 00002 #define SEGMENTATION_H 00003 00004 #include "typedefs.h" 00005 00006 #include <pcl/ModelCoefficients.h> 00007 #include <pcl/sample_consensus/method_types.h> 00008 #include <pcl/sample_consensus/model_types.h> 00009 #include <pcl/segmentation/sac_segmentation.h> 00010 #include <pcl/filters/extract_indices.h> 00011 #include <pcl/segmentation/extract_clusters.h> 00012 00013 00014 /* Use SACSegmentation to find the dominant plane in the scene 00015 * Inputs: 00016 * input 00017 * The input point cloud 00018 * max_iterations 00019 * The maximum number of RANSAC iterations to run 00020 * distance_threshold 00021 * The inlier/outlier threshold. Points within this distance 00022 * from the hypothesized plane are scored as inliers. 00023 * Return: A pointer to the ModelCoefficients (i.e., the 4 coefficients of the plane, 00024 * represented in c0*x + c1*y + c2*z + c3 = 0 form) 00025 */ 00026 pcl::ModelCoefficients::Ptr 00027 fitPlane (const PointCloudPtr & input, float distance_threshold, float max_iterations) 00028 { 00029 // Intialize the SACSegmentation object 00030 pcl::SACSegmentation<PointT> seg; 00031 seg.setOptimizeCoefficients (true); 00032 seg.setModelType (pcl::SACMODEL_PLANE); 00033 seg.setMethodType (pcl::SAC_RANSAC); 00034 seg.setDistanceThreshold (distance_threshold); 00035 seg.setMaxIterations (max_iterations); 00036 00037 seg.setInputCloud (input); 00038 pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ()); 00039 pcl::PointIndices::Ptr inliers (new pcl::PointIndices ()); 00040 seg.segment (*inliers, *coefficients); 00041 00042 return (coefficients); 00043 } 00044 00045 /* Use SACSegmentation and an ExtractIndices filter to find the dominant plane and subtract it 00046 * Inputs: 00047 * input 00048 * The input point cloud 00049 * max_iterations 00050 * The maximum number of RANSAC iterations to run 00051 * distance_threshold 00052 * The inlier/outlier threshold. Points within this distance 00053 * from the hypothesized plane are scored as inliers. 00054 * Return: A pointer to a new point cloud which contains only the non-plane points 00055 */ 00056 PointCloudPtr 00057 findAndSubtractPlane (const PointCloudPtr & input, float distance_threshold, float max_iterations) 00058 { 00059 // Find the dominant plane 00060 pcl::SACSegmentation<PointT> seg; 00061 seg.setOptimizeCoefficients (false); 00062 seg.setModelType (pcl::SACMODEL_PLANE); 00063 seg.setMethodType (pcl::SAC_RANSAC); 00064 seg.setDistanceThreshold (distance_threshold); 00065 seg.setMaxIterations (max_iterations); 00066 seg.setInputCloud (input); 00067 pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ()); 00068 pcl::PointIndices::Ptr inliers (new pcl::PointIndices ()); 00069 seg.segment (*inliers, *coefficients); 00070 00071 // Extract the inliers 00072 pcl::ExtractIndices<PointT> extract; 00073 extract.setInputCloud (input); 00074 extract.setIndices (inliers); 00075 extract.setNegative (true); 00076 PointCloudPtr output (new PointCloud); 00077 extract.filter (*output); 00078 00079 return (output); 00080 } 00081 00082 /* Use EuclidieanClusterExtraction to group a cloud into contiguous clusters 00083 * Inputs: 00084 * input 00085 * The input point cloud 00086 * cluster_tolerance 00087 * The maximum distance between neighboring points in a cluster 00088 * min/max_cluster_size 00089 * The minimum and maximum allowable cluster sizes 00090 * Return (by reference): a vector of PointIndices containing the points indices in each cluster 00091 */ 00092 void 00093 clusterObjects (const PointCloudPtr & input, 00094 float cluster_tolerance, int min_cluster_size, int max_cluster_size, 00095 std::vector<pcl::PointIndices> & cluster_indices_out) 00096 { 00097 pcl::EuclideanClusterExtraction<PointT> ec; 00098 ec.setClusterTolerance (cluster_tolerance); 00099 ec.setMinClusterSize (min_cluster_size); 00100 ec.setMaxClusterSize (max_cluster_size); 00101 00102 ec.setInputCloud (input); 00103 ec.extract (cluster_indices_out); 00104 } 00105 00106 #endif