Point Cloud Library (PCL)  1.8.0
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_PROGRESSIVE_MORPHOLOGICAL_FILTER_HPP_
40 #define PCL_SEGMENTATION_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/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 {
61 }
62 
63 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
64 template <typename PointT>
66 {
67 }
68 
69 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
70 template <typename PointT> void
72 {
73  bool segmentation_is_possible = initCompute ();
74  if (!segmentation_is_possible)
75  {
76  deinitCompute ();
77  return;
78  }
79 
80  // Compute the series of window sizes and height thresholds
81  std::vector<float> height_thresholds;
82  std::vector<float> window_sizes;
83  int iteration = 0;
84  float window_size = 0.0f;
85  float height_threshold = 0.0f;
86 
87  while (window_size < max_window_size_)
88  {
89  // Determine the initial window size.
90  if (exponential_)
91  window_size = cell_size_ * (2.0f * std::pow (base_, iteration) + 1.0f);
92  else
93  window_size = cell_size_ * (2.0f * (iteration+1) * base_ + 1.0f);
94 
95  // Calculate the height threshold to be used in the next iteration.
96  if (iteration == 0)
97  height_threshold = initial_distance_;
98  else
99  height_threshold = slope_ * (window_size - window_sizes[iteration-1]) * cell_size_ + initial_distance_;
100 
101  // Enforce max distance on height threshold
102  if (height_threshold > max_distance_)
103  height_threshold = max_distance_;
104 
105  window_sizes.push_back (window_size);
106  height_thresholds.push_back (height_threshold);
107 
108  iteration++;
109  }
110 
111  // Ground indices are initially limited to those points in the input cloud we
112  // wish to process
113  ground = *indices_;
114 
115  // Progressively filter ground returns using morphological open
116  for (size_t i = 0; i < window_sizes.size (); ++i)
117  {
118  PCL_DEBUG (" Iteration %d (height threshold = %f, window size = %f)...",
119  i, height_thresholds[i], window_sizes[i]);
120 
121  // Limit filtering to those points currently considered ground returns
123  pcl::copyPointCloud<PointT> (*input_, ground, *cloud);
124 
125  // Create new cloud to hold the filtered results. Apply the morphological
126  // opening operation at the current window size.
128  pcl::applyMorphologicalOperator<PointT> (cloud, window_sizes[i], MORPH_OPEN, *cloud_f);
129 
130  // Find indices of the points whose difference between the source and
131  // filtered point clouds is less than the current height threshold.
132  std::vector<int> pt_indices;
133  for (size_t p_idx = 0; p_idx < ground.size (); ++p_idx)
134  {
135  float diff = cloud->points[p_idx].z - cloud_f->points[p_idx].z;
136  if (diff < height_thresholds[i])
137  pt_indices.push_back (ground[p_idx]);
138  }
139 
140  // Ground is now limited to pt_indices
141  ground.swap (pt_indices);
142 
143  PCL_DEBUG ("ground now has %d points\n", ground.size ());
144  }
145 
146  deinitCompute ();
147 }
148 
149 #define PCL_INSTANTIATE_ProgressiveMorphologicalFilter(T) template class pcl::ProgressiveMorphologicalFilter<T>;
150 
151 #endif // PCL_SEGMENTATION_PROGRESSIVE_MORPHOLOGICAL_FILTER_HPP_
152 
float base_
Base to be used in computing progressive window sizes.
float slope_
Slope value to be used in computing the height threshold.
int max_window_size_
Maximum window size to be used in filtering ground returns.
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition: pcl_base.h:153
float initial_distance_
Initial height above the parameterized ground surface to be considered a ground return.
bool exponential_
Exponentially grow window sizes?
bool initCompute()
This method should get called before starting the actual computation.
Definition: pcl_base.hpp:139
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
Define standard C methods and C++ classes that are common to all methods.
ProgressiveMorphologicalFilter()
Constructor that sets default values for member variables.
Defines all the PCL implemented PointT point type structures.
bool deinitCompute()
This method should get called after finishing the actual computation.
Definition: pcl_base.hpp:174
PointCloud represents the base class in PCL for storing collections of 3D points. ...
float max_distance_
Maximum height above the parameterized ground surface to be considered a ground return.
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:150
virtual void extract(std::vector< int > &ground)
This method launches the segmentation algorithm and returns indices of points determined to be ground...