Point Cloud Library (PCL)  1.8.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/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  int half_size = 0.0f;
87  float window_size = 0.0f;
88  float height_threshold = 0.0f;
89 
90  while (window_size < max_window_size_)
91  {
92  // Determine the initial window size.
93  if (exponential_)
94  half_size = static_cast<int> (std::pow (static_cast<float> (base_), iteration));
95  else
96  half_size = (iteration+1) * base_;
97 
98  window_size = 2 * half_size + 1;
99 
100  // Calculate the height threshold to be used in the next iteration.
101  if (iteration == 0)
102  height_threshold = initial_distance_;
103  else
104  height_threshold = slope_ * (window_size - window_sizes[iteration-1]) * cell_size_ + initial_distance_;
105 
106  // Enforce max distance on height threshold
107  if (height_threshold > max_distance_)
108  height_threshold = max_distance_;
109 
110  half_sizes.push_back (half_size);
111  window_sizes.push_back (window_size);
112  height_thresholds.push_back (height_threshold);
113 
114  iteration++;
115  }
116 
117  // setup grid based on scale and extents
118  Eigen::Vector4f global_max, global_min;
119  pcl::getMinMax3D<PointT> (*input_, global_min, global_max);
120 
121  float xextent = global_max.x () - global_min.x ();
122  float yextent = global_max.y () - global_min.y ();
123 
124  int rows = static_cast<int> (std::floor (yextent / cell_size_) + 1);
125  int cols = static_cast<int> (std::floor (xextent / cell_size_) + 1);
126 
127  Eigen::MatrixXf A (rows, cols);
128  A.setConstant (std::numeric_limits<float>::quiet_NaN ());
129 
130  Eigen::MatrixXf Z (rows, cols);
131  Z.setConstant (std::numeric_limits<float>::quiet_NaN ());
132 
133  Eigen::MatrixXf Zf (rows, cols);
134  Zf.setConstant (std::numeric_limits<float>::quiet_NaN ());
135 
136 #ifdef _OPENMP
137 #pragma omp parallel for num_threads(threads_)
138 #endif
139  for (int i = 0; i < (int)input_->points.size (); ++i)
140  {
141  // ...then test for lower points within the cell
142  PointT p = input_->points[i];
143  int row = std::floor((p.y - global_min.y ()) / cell_size_);
144  int col = std::floor((p.x - global_min.x ()) / cell_size_);
145 
146  if (p.z < A (row, col) || pcl_isnan (A (row, col)))
147  {
148  A (row, col) = p.z;
149  }
150  }
151 
152  // Ground indices are initially limited to those points in the input cloud we
153  // wish to process
154  ground = *indices_;
155 
156  // Progressively filter ground returns using morphological open
157  for (size_t i = 0; i < window_sizes.size (); ++i)
158  {
159  PCL_DEBUG (" Iteration %d (height threshold = %f, window size = %f, half size = %d)...",
160  i, height_thresholds[i], window_sizes[i], half_sizes[i]);
161 
162  // Limit filtering to those points currently considered ground returns
164  pcl::copyPointCloud<PointT> (*input_, ground, *cloud);
165 
166  // Apply the morphological opening operation at the current window size.
167 #ifdef _OPENMP
168 #pragma omp parallel for num_threads(threads_)
169 #endif
170  for (int row = 0; row < rows; ++row)
171  {
172  int rs, re;
173  rs = ((row - half_sizes[i]) < 0) ? 0 : row - half_sizes[i];
174  re = ((row + half_sizes[i]) > (rows-1)) ? (rows-1) : row + half_sizes[i];
175 
176  for (int col = 0; col < cols; ++col)
177  {
178  int cs, ce;
179  cs = ((col - half_sizes[i]) < 0) ? 0 : col - half_sizes[i];
180  ce = ((col + half_sizes[i]) > (cols-1)) ? (cols-1) : col + half_sizes[i];
181 
182  float min_coeff = std::numeric_limits<float>::max ();
183 
184  for (int j = rs; j < (re + 1); ++j)
185  {
186  for (int k = cs; k < (ce + 1); ++k)
187  {
188  if (A (j, k) != std::numeric_limits<float>::quiet_NaN ())
189  {
190  if (A (j, k) < min_coeff)
191  min_coeff = A (j, k);
192  }
193  }
194  }
195 
196  if (min_coeff != std::numeric_limits<float>::max ())
197  Z(row, col) = min_coeff;
198  }
199  }
200 
201 #ifdef _OPENMP
202 #pragma omp parallel for num_threads(threads_)
203 #endif
204  for (int row = 0; row < rows; ++row)
205  {
206  int rs, re;
207  rs = ((row - half_sizes[i]) < 0) ? 0 : row - half_sizes[i];
208  re = ((row + half_sizes[i]) > (rows-1)) ? (rows-1) : row + half_sizes[i];
209 
210  for (int col = 0; col < cols; ++col)
211  {
212  int cs, ce;
213  cs = ((col - half_sizes[i]) < 0) ? 0 : col - half_sizes[i];
214  ce = ((col + half_sizes[i]) > (cols-1)) ? (cols-1) : col + half_sizes[i];
215 
216  float max_coeff = -std::numeric_limits<float>::max ();
217 
218  for (int j = rs; j < (re + 1); ++j)
219  {
220  for (int k = cs; k < (ce + 1); ++k)
221  {
222  if (Z (j, k) != std::numeric_limits<float>::quiet_NaN ())
223  {
224  if (Z (j, k) > max_coeff)
225  max_coeff = Z (j, k);
226  }
227  }
228  }
229 
230  if (max_coeff != -std::numeric_limits<float>::max ())
231  Zf (row, col) = max_coeff;
232  }
233  }
234 
235  // Find indices of the points whose difference between the source and
236  // filtered point clouds is less than the current height threshold.
237  std::vector<int> pt_indices;
238  for (size_t p_idx = 0; p_idx < ground.size (); ++p_idx)
239  {
240  PointT p = cloud->points[p_idx];
241  int erow = static_cast<int> (std::floor ((p.y - global_min.y ()) / cell_size_));
242  int ecol = static_cast<int> (std::floor ((p.x - global_min.x ()) / cell_size_));
243 
244  float diff = p.z - Zf (erow, ecol);
245  if (diff < height_thresholds[i])
246  pt_indices.push_back (ground[p_idx]);
247  }
248 
249  A.swap (Zf);
250 
251  // Ground is now limited to pt_indices
252  ground.swap (pt_indices);
253 
254  PCL_DEBUG ("ground now has %d points\n", ground.size ());
255  }
256 
257  deinitCompute ();
258 }
259 
260 
261 #define PCL_INSTANTIATE_ApproximateProgressiveMorphologicalFilter(T) template class pcl::ApproximateProgressiveMorphologicalFilter<T>;
262 
263 #endif // PCL_SEGMENTATION_APPROXIMATE_PROGRESSIVE_MORPHOLOGICAL_FILTER_HPP_
264 
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
Define standard C methods and C++ classes that are common to all methods.
Defines all the PCL implemented PointT point type structures.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
ApproximateProgressiveMorphologicalFilter()
Constructor that sets default values for member variables.
virtual void extract(std::vector< int > &ground)
This method launches the segmentation algorithm and returns indices of points determined to be ground...
A point structure representing Euclidean xyz coordinates, and the RGB color.