Point Cloud Library (PCL)  1.12.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/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 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
51 template <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 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
65 template <typename PointT>
67 {
68 }
69 
70 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
71 template <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.
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.