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