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