Point Cloud Library (PCL)
1.7.0
Main Page
Modules
Namespaces
Classes
doc
tutorials
content
sources
iros2011
include
solution
segmentation.h
1
#ifndef SEGMENTATION_H
2
#define SEGMENTATION_H
3
4
#include "typedefs.h"
5
6
#include <pcl/ModelCoefficients.h>
7
#include <pcl/sample_consensus/method_types.h>
8
#include <pcl/sample_consensus/model_types.h>
9
#include <pcl/segmentation/sac_segmentation.h>
10
#include <pcl/filters/extract_indices.h>
11
#include <pcl/segmentation/extract_clusters.h>
12
13
14
/* Use SACSegmentation to find the dominant plane in the scene
15
* Inputs:
16
* input
17
* The input point cloud
18
* max_iterations
19
* The maximum number of RANSAC iterations to run
20
* distance_threshold
21
* The inlier/outlier threshold. Points within this distance
22
* from the hypothesized plane are scored as inliers.
23
* Return: A pointer to the ModelCoefficients (i.e., the 4 coefficients of the plane,
24
* represented in c0*x + c1*y + c2*z + c3 = 0 form)
25
*/
26
pcl::ModelCoefficients::Ptr
27
fitPlane (
const
PointCloudPtr & input,
float
distance_threshold,
float
max_iterations)
28
{
29
// Intialize the SACSegmentation object
30
pcl::SACSegmentation<PointT>
seg;
31
seg.
setOptimizeCoefficients
(
true
);
32
seg.
setModelType
(
pcl::SACMODEL_PLANE
);
33
seg.
setMethodType
(
pcl::SAC_RANSAC
);
34
seg.
setDistanceThreshold
(distance_threshold);
35
seg.
setMaxIterations
(max_iterations);
36
37
seg.
setInputCloud
(input);
38
pcl::ModelCoefficients::Ptr
coefficients (
new
pcl::ModelCoefficients
());
39
pcl::PointIndices::Ptr
inliers (
new
pcl::PointIndices
());
40
seg.
segment
(*inliers, *coefficients);
41
42
return
(coefficients);
43
}
44
45
/* Use SACSegmentation and an ExtractIndices filter to find the dominant plane and subtract it
46
* Inputs:
47
* input
48
* The input point cloud
49
* max_iterations
50
* The maximum number of RANSAC iterations to run
51
* distance_threshold
52
* The inlier/outlier threshold. Points within this distance
53
* from the hypothesized plane are scored as inliers.
54
* Return: A pointer to a new point cloud which contains only the non-plane points
55
*/
56
PointCloudPtr
57
findAndSubtractPlane (
const
PointCloudPtr & input,
float
distance_threshold,
float
max_iterations)
58
{
59
// Find the dominant plane
60
pcl::SACSegmentation<PointT>
seg;
61
seg.
setOptimizeCoefficients
(
false
);
62
seg.
setModelType
(
pcl::SACMODEL_PLANE
);
63
seg.
setMethodType
(
pcl::SAC_RANSAC
);
64
seg.
setDistanceThreshold
(distance_threshold);
65
seg.
setMaxIterations
(max_iterations);
66
seg.
setInputCloud
(input);
67
pcl::ModelCoefficients::Ptr
coefficients (
new
pcl::ModelCoefficients
());
68
pcl::PointIndices::Ptr
inliers (
new
pcl::PointIndices
());
69
seg.
segment
(*inliers, *coefficients);
70
71
// Extract the inliers
72
pcl::ExtractIndices<PointT>
extract;
73
extract.
setInputCloud
(input);
74
extract.
setIndices
(inliers);
75
extract.
setNegative
(
true
);
76
PointCloudPtr output (
new
PointCloud
);
77
extract.
filter
(*output);
78
79
return
(output);
80
}
81
82
/* Use EuclidieanClusterExtraction to group a cloud into contiguous clusters
83
* Inputs:
84
* input
85
* The input point cloud
86
* cluster_tolerance
87
* The maximum distance between neighboring points in a cluster
88
* min/max_cluster_size
89
* The minimum and maximum allowable cluster sizes
90
* Return (by reference): a vector of PointIndices containing the points indices in each cluster
91
*/
92
void
93
clusterObjects (
const
PointCloudPtr & input,
94
float
cluster_tolerance,
int
min_cluster_size,
int
max_cluster_size,
95
std::vector<pcl::PointIndices> & cluster_indices_out)
96
{
97
pcl::EuclideanClusterExtraction<PointT>
ec;
98
ec.
setClusterTolerance
(cluster_tolerance);
99
ec.
setMinClusterSize
(min_cluster_size);
100
ec.
setMaxClusterSize
(max_cluster_size);
101
102
ec.
setInputCloud
(input);
103
ec.
extract
(cluster_indices_out);
104
}
105
106
#endif