Point Cloud Library (PCL)  1.14.1-dev
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...
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.
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.