Point Cloud Library (PCL) 1.13.0
approximate_progressive_morphological_filter.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2009-2012, Willow Garage, Inc.
6 * Copyright (c) 2012-, Open Perception, Inc.
7 * Copyright (c) 2014, RadiantBlue Technologies, Inc.
8 *
9 * All rights reserved.
10 *
11 * Redistribution and use in source and binary forms, with or without
12 * modification, are permitted provided that the following conditions
13 * are met:
14 *
15 * * Redistributions of source code must retain the above copyright
16 * notice, this list of conditions and the following disclaimer.
17 * * Redistributions in binary form must reproduce the above
18 * copyright notice, this list of conditions and the following
19 * disclaimer in the documentation and/or other materials provided
20 * with the distribution.
21 * * Neither the name of the copyright holder(s) nor the names of its
22 * contributors may be used to endorse or promote products derived
23 * from this software without specific prior written permission.
24 *
25 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
26 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
27 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
28 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
29 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
30 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
31 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
32 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
33 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
34 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
35 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
36 * POSSIBILITY OF SUCH DAMAGE.
37 */
38
39#ifndef PCL_SEGMENTATION_APPROXIMATE_PROGRESSIVE_MORPHOLOGICAL_FILTER_HPP_
40#define PCL_SEGMENTATION_APPROXIMATE_PROGRESSIVE_MORPHOLOGICAL_FILTER_HPP_
41
42#include <pcl/common/common.h>
43#include <pcl/common/io.h>
44#include <pcl/filters/morphological_filter.h>
45#include <pcl/segmentation/approximate_progressive_morphological_filter.h>
46#include <pcl/point_cloud.h>
47#include <pcl/point_types.h>
48
49//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
50template <typename PointT>
52 max_window_size_ (33),
53 slope_ (0.7f),
54 max_distance_ (10.0f),
55 initial_distance_ (0.15f),
56 cell_size_ (1.0f),
57 base_ (2.0f),
58 exponential_ (true),
59 threads_ (0)
60{
61}
62
63//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
64template <typename PointT>
66
67//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
68template <typename PointT> void
70{
71 bool segmentation_is_possible = initCompute ();
72 if (!segmentation_is_possible)
73 {
74 deinitCompute ();
75 return;
76 }
77
78 // Compute the series of window sizes and height thresholds
79 std::vector<float> height_thresholds;
80 std::vector<float> window_sizes;
81 std::vector<int> half_sizes;
82 int iteration = 0;
83 float window_size = 0.0f;
84
85 while (window_size < max_window_size_)
86 {
87 // Determine the initial window size.
88 int half_size = (exponential_) ? (static_cast<int> (std::pow (static_cast<float> (base_), iteration))) : ((iteration+1) * base_);
89
90 window_size = 2 * half_size + 1;
91
92 // Calculate the height threshold to be used in the next iteration.
93 float height_threshold = (iteration == 0) ? (initial_distance_) : (slope_ * (window_size - window_sizes[iteration-1]) * cell_size_ + initial_distance_);
94
95 // Enforce max distance on height threshold
96 if (height_threshold > max_distance_)
97 height_threshold = max_distance_;
98
99 half_sizes.push_back (half_size);
100 window_sizes.push_back (window_size);
101 height_thresholds.push_back (height_threshold);
102
103 iteration++;
104 }
105
106 // setup grid based on scale and extents
107 Eigen::Vector4f global_max, global_min;
108 pcl::getMinMax3D<PointT> (*input_, global_min, global_max);
109
110 float xextent = global_max.x () - global_min.x ();
111 float yextent = global_max.y () - global_min.y ();
112
113 int rows = static_cast<int> (std::floor (yextent / cell_size_) + 1);
114 int cols = static_cast<int> (std::floor (xextent / cell_size_) + 1);
115
116 Eigen::MatrixXf A (rows, cols);
117 A.setConstant (std::numeric_limits<float>::quiet_NaN ());
118
119 Eigen::MatrixXf Z (rows, cols);
120 Z.setConstant (std::numeric_limits<float>::quiet_NaN ());
121
122 Eigen::MatrixXf Zf (rows, cols);
123 Zf.setConstant (std::numeric_limits<float>::quiet_NaN ());
124
125#pragma omp parallel for \
126 default(none) \
127 shared(A, global_min) \
128 num_threads(threads_)
129 for (int i = 0; i < (int)input_->size (); ++i)
130 {
131 // ...then test for lower points within the cell
132 PointT p = (*input_)[i];
133 int row = std::floor((p.y - global_min.y ()) / cell_size_);
134 int col = std::floor((p.x - global_min.x ()) / cell_size_);
135
136 if (p.z < A (row, col) || std::isnan (A (row, col)))
137 {
138 A (row, col) = p.z;
139 }
140 }
141
142 // Ground indices are initially limited to those points in the input cloud we
143 // wish to process
144 ground = *indices_;
145
146 // Progressively filter ground returns using morphological open
147 for (std::size_t i = 0; i < window_sizes.size (); ++i)
148 {
149 PCL_DEBUG (" Iteration %d (height threshold = %f, window size = %f, half size = %d)...",
150 i, height_thresholds[i], window_sizes[i], half_sizes[i]);
151
152 // Limit filtering to those points currently considered ground returns
154 pcl::copyPointCloud<PointT> (*input_, ground, *cloud);
155
156 // Apply the morphological opening operation at the current window size.
157#pragma omp parallel for \
158 default(none) \
159 shared(A, cols, half_sizes, i, rows, Z) \
160 num_threads(threads_)
161 for (int row = 0; row < rows; ++row)
162 {
163 int rs, re;
164 rs = ((row - half_sizes[i]) < 0) ? 0 : row - half_sizes[i];
165 re = ((row + half_sizes[i]) > (rows-1)) ? (rows-1) : row + half_sizes[i];
166
167 for (int col = 0; col < cols; ++col)
168 {
169 int cs, ce;
170 cs = ((col - half_sizes[i]) < 0) ? 0 : col - half_sizes[i];
171 ce = ((col + half_sizes[i]) > (cols-1)) ? (cols-1) : col + half_sizes[i];
172
173 float min_coeff = std::numeric_limits<float>::max ();
174
175 for (int j = rs; j < (re + 1); ++j)
176 {
177 for (int k = cs; k < (ce + 1); ++k)
178 {
179 if (A (j, k) != std::numeric_limits<float>::quiet_NaN ())
180 {
181 if (A (j, k) < min_coeff)
182 min_coeff = A (j, k);
183 }
184 }
185 }
186
187 if (min_coeff != std::numeric_limits<float>::max ())
188 Z(row, col) = min_coeff;
189 }
190 }
191
192#pragma omp parallel for \
193 default(none) \
194 shared(cols, half_sizes, i, rows, Z, Zf) \
195 num_threads(threads_)
196 for (int row = 0; row < rows; ++row)
197 {
198 int rs, re;
199 rs = ((row - half_sizes[i]) < 0) ? 0 : row - half_sizes[i];
200 re = ((row + half_sizes[i]) > (rows-1)) ? (rows-1) : row + half_sizes[i];
201
202 for (int col = 0; col < cols; ++col)
203 {
204 int cs, ce;
205 cs = ((col - half_sizes[i]) < 0) ? 0 : col - half_sizes[i];
206 ce = ((col + half_sizes[i]) > (cols-1)) ? (cols-1) : col + half_sizes[i];
207
208 float max_coeff = -std::numeric_limits<float>::max ();
209
210 for (int j = rs; j < (re + 1); ++j)
211 {
212 for (int k = cs; k < (ce + 1); ++k)
213 {
214 if (Z (j, k) != std::numeric_limits<float>::quiet_NaN ())
215 {
216 if (Z (j, k) > max_coeff)
217 max_coeff = Z (j, k);
218 }
219 }
220 }
221
222 if (max_coeff != -std::numeric_limits<float>::max ())
223 Zf (row, col) = max_coeff;
224 }
225 }
226
227 // Find indices of the points whose difference between the source and
228 // filtered point clouds is less than the current height threshold.
229 Indices pt_indices;
230 for (std::size_t p_idx = 0; p_idx < ground.size (); ++p_idx)
231 {
232 PointT p = (*cloud)[p_idx];
233 int erow = static_cast<int> (std::floor ((p.y - global_min.y ()) / cell_size_));
234 int ecol = static_cast<int> (std::floor ((p.x - global_min.x ()) / cell_size_));
235
236 float diff = p.z - Zf (erow, ecol);
237 if (diff < height_thresholds[i])
238 pt_indices.push_back (ground[p_idx]);
239 }
240
241 A.swap (Zf);
242
243 // Ground is now limited to pt_indices
244 ground.swap (pt_indices);
245
246 PCL_DEBUG ("ground now has %d points\n", ground.size ());
247 }
248
249 deinitCompute ();
250}
251
252
253#define PCL_INSTANTIATE_ApproximateProgressiveMorphologicalFilter(T) template class pcl::ApproximateProgressiveMorphologicalFilter<T>;
254
255#endif // PCL_SEGMENTATION_APPROXIMATE_PROGRESSIVE_MORPHOLOGICAL_FILTER_HPP_
256
virtual void extract(Indices &ground)
This method launches the segmentation algorithm and returns indices of points determined to be ground...
ApproximateProgressiveMorphologicalFilter()
Constructor that sets default values for member variables.
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
Define standard C methods and C++ classes that are common to all methods.
Defines all the PCL implemented PointT point type structures.
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
A point structure representing Euclidean xyz coordinates, and the RGB color.