Point Cloud Library (PCL)  1.9.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 
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) || std::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:423
float max_distance_
Maximum height above the parameterized ground surface to be considered a ground return.
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition: pcl_base.h:154
float slope_
Slope value to be used in computing the height threshold.
bool initCompute()
This method should get called before starting the actual computation.
Definition: pcl_base.hpp:138
Define standard C methods and C++ classes that are common to all methods.
int max_window_size_
Maximum window size to be used in filtering ground returns.
Defines all the PCL implemented PointT point type structures.
float initial_distance_
Initial height above the parameterized ground surface to be considered a ground return.
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:441
bool deinitCompute()
This method should get called after finishing the actual computation.
Definition: pcl_base.hpp:171
PointCloud represents the base class in PCL for storing collections of 3D points. ...
ApproximateProgressiveMorphologicalFilter()
Constructor that sets default values for member variables.
float base_
Base to be used in computing progressive window sizes.
virtual void extract(std::vector< int > &ground)
This method launches the segmentation algorithm and returns indices of points determined to be ground...
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:151
A point structure representing Euclidean xyz coordinates, and the RGB color.