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