Point Cloud Library (PCL)  1.14.1
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 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
51 template <typename PointT>
53 
54 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
55 template <typename PointT>
57 
58 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
59 template <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_EXPORTS 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...
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
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Definition: distances.h:55
ApproximateProgressiveMorphologicalFilter()
Constructor that sets default values for member variables.
A point structure representing Euclidean xyz coordinates, and the RGB color.