Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
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/filters/extract_indices.h>
46#include <pcl/segmentation/approximate_progressive_morphological_filter.h>
47#include <pcl/point_cloud.h>
48#include <pcl/point_types.h>
49
50//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
51template <typename PointT>
53 max_window_size_ (33),
54 slope_ (0.7f),
55 max_distance_ (10.0f),
56 initial_distance_ (0.15f),
57 cell_size_ (1.0f),
58 base_ (2.0f),
59 exponential_ (true),
60 threads_ (0)
61{
62}
63
64//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
65template <typename PointT>
69
70//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
71template <typename PointT> void
73{
74 bool segmentation_is_possible = initCompute ();
75 if (!segmentation_is_possible)
76 {
77 deinitCompute ();
78 return;
79 }
80
81 // Compute the series of window sizes and height thresholds
82 std::vector<float> height_thresholds;
83 std::vector<float> window_sizes;
84 std::vector<int> half_sizes;
85 int iteration = 0;
86 float window_size = 0.0f;
87
88 while (window_size < max_window_size_)
89 {
90 // Determine the initial window size.
91 int half_size = (exponential_) ? (static_cast<int> (std::pow (static_cast<float> (base_), iteration))) : ((iteration+1) * base_);
92
93 window_size = 2 * half_size + 1;
94
95 // Calculate the height threshold to be used in the next iteration.
96 float height_threshold = (iteration == 0) ? (initial_distance_) : (slope_ * (window_size - window_sizes[iteration-1]) * cell_size_ + initial_distance_);
97
98 // Enforce max distance on height threshold
99 if (height_threshold > max_distance_)
100 height_threshold = max_distance_;
101
102 half_sizes.push_back (half_size);
103 window_sizes.push_back (window_size);
104 height_thresholds.push_back (height_threshold);
105
106 iteration++;
107 }
108
109 // setup grid based on scale and extents
110 Eigen::Vector4f global_max, global_min;
111 pcl::getMinMax3D<PointT> (*input_, global_min, global_max);
112
113 float xextent = global_max.x () - global_min.x ();
114 float yextent = global_max.y () - global_min.y ();
115
116 int rows = static_cast<int> (std::floor (yextent / cell_size_) + 1);
117 int cols = static_cast<int> (std::floor (xextent / cell_size_) + 1);
118
119 Eigen::MatrixXf A (rows, cols);
120 A.setConstant (std::numeric_limits<float>::quiet_NaN ());
121
122 Eigen::MatrixXf Z (rows, cols);
123 Z.setConstant (std::numeric_limits<float>::quiet_NaN ());
124
125 Eigen::MatrixXf Zf (rows, cols);
126 Zf.setConstant (std::numeric_limits<float>::quiet_NaN ());
127
128#pragma omp parallel for \
129 default(none) \
130 shared(A, global_min) \
131 num_threads(threads_)
132 for (int i = 0; i < (int)input_->size (); ++i)
133 {
134 // ...then test for lower points within the cell
135 PointT p = (*input_)[i];
136 int row = std::floor((p.y - global_min.y ()) / cell_size_);
137 int col = std::floor((p.x - global_min.x ()) / cell_size_);
138
139 if (p.z < A (row, col) || std::isnan (A (row, col)))
140 {
141 A (row, col) = p.z;
142 }
143 }
144
145 // Ground indices are initially limited to those points in the input cloud we
146 // wish to process
147 ground = *indices_;
148
149 // Progressively filter ground returns using morphological open
150 for (std::size_t i = 0; i < window_sizes.size (); ++i)
151 {
152 PCL_DEBUG (" Iteration %d (height threshold = %f, window size = %f, half size = %d)...",
153 i, height_thresholds[i], window_sizes[i], half_sizes[i]);
154
155 // Limit filtering to those points currently considered ground returns
157 pcl::copyPointCloud<PointT> (*input_, ground, *cloud);
158
159 // Apply the morphological opening operation at the current window size.
160#pragma omp parallel for \
161 default(none) \
162 shared(A, cols, half_sizes, i, rows, Z) \
163 num_threads(threads_)
164 for (int row = 0; row < rows; ++row)
165 {
166 int rs, re;
167 rs = ((row - half_sizes[i]) < 0) ? 0 : row - half_sizes[i];
168 re = ((row + half_sizes[i]) > (rows-1)) ? (rows-1) : row + half_sizes[i];
169
170 for (int col = 0; col < cols; ++col)
171 {
172 int cs, ce;
173 cs = ((col - half_sizes[i]) < 0) ? 0 : col - half_sizes[i];
174 ce = ((col + half_sizes[i]) > (cols-1)) ? (cols-1) : col + half_sizes[i];
175
176 float min_coeff = std::numeric_limits<float>::max ();
177
178 for (int j = rs; j < (re + 1); ++j)
179 {
180 for (int k = cs; k < (ce + 1); ++k)
181 {
182 if (A (j, k) != std::numeric_limits<float>::quiet_NaN ())
183 {
184 if (A (j, k) < min_coeff)
185 min_coeff = A (j, k);
186 }
187 }
188 }
189
190 if (min_coeff != std::numeric_limits<float>::max ())
191 Z(row, col) = min_coeff;
192 }
193 }
194
195#pragma omp parallel for \
196 default(none) \
197 shared(cols, half_sizes, i, rows, Z, Zf) \
198 num_threads(threads_)
199 for (int row = 0; row < rows; ++row)
200 {
201 int rs, re;
202 rs = ((row - half_sizes[i]) < 0) ? 0 : row - half_sizes[i];
203 re = ((row + half_sizes[i]) > (rows-1)) ? (rows-1) : row + half_sizes[i];
204
205 for (int col = 0; col < cols; ++col)
206 {
207 int cs, ce;
208 cs = ((col - half_sizes[i]) < 0) ? 0 : col - half_sizes[i];
209 ce = ((col + half_sizes[i]) > (cols-1)) ? (cols-1) : col + half_sizes[i];
210
211 float max_coeff = -std::numeric_limits<float>::max ();
212
213 for (int j = rs; j < (re + 1); ++j)
214 {
215 for (int k = cs; k < (ce + 1); ++k)
216 {
217 if (Z (j, k) != std::numeric_limits<float>::quiet_NaN ())
218 {
219 if (Z (j, k) > max_coeff)
220 max_coeff = Z (j, k);
221 }
222 }
223 }
224
225 if (max_coeff != -std::numeric_limits<float>::max ())
226 Zf (row, col) = max_coeff;
227 }
228 }
229
230 // Find indices of the points whose difference between the source and
231 // filtered point clouds is less than the current height threshold.
232 Indices pt_indices;
233 for (std::size_t p_idx = 0; p_idx < ground.size (); ++p_idx)
234 {
235 PointT p = (*cloud)[p_idx];
236 int erow = static_cast<int> (std::floor ((p.y - global_min.y ()) / cell_size_));
237 int ecol = static_cast<int> (std::floor ((p.x - global_min.x ()) / cell_size_));
238
239 float diff = p.z - Zf (erow, ecol);
240 if (diff < height_thresholds[i])
241 pt_indices.push_back (ground[p_idx]);
242 }
243
244 A.swap (Zf);
245
246 // Ground is now limited to pt_indices
247 ground.swap (pt_indices);
248
249 PCL_DEBUG ("ground now has %d points\n", ground.size ());
250 }
251
252 deinitCompute ();
253}
254
255
256#define PCL_INSTANTIATE_ApproximateProgressiveMorphologicalFilter(T) template class pcl::ApproximateProgressiveMorphologicalFilter<T>;
257
258#endif // PCL_SEGMENTATION_APPROXIMATE_PROGRESSIVE_MORPHOLOGICAL_FILTER_HPP_
259
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.
shared_ptr< PointCloud< PointT > > Ptr
Define standard C methods and C++ classes that are common to all methods.
Defines all the PCL implemented PointT point type structures.
void getMinMax3D(const pcl::PointCloud< PointT > &cloud, PointT &min_pt, PointT &max_pt)
Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud.
Definition common.hpp:295
void copyPointCloud(const pcl::PointCloud< PointInT > &cloud_in, pcl::PointCloud< PointOutT > &cloud_out)
Copy all the fields from a given point cloud into a new point cloud.
Definition io.hpp:144
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
A point structure representing Euclidean xyz coordinates, and the RGB color.