Point Cloud Library (PCL)  1.10.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 (std::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 (std::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 
point_types.h
common.h
pcl::PointCloud::points
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:397
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: projection_matrix.h:52
pcl::ProgressiveMorphologicalFilter::ProgressiveMorphologicalFilter
ProgressiveMorphologicalFilter()
Constructor that sets default values for member variables.
Definition: progressive_morphological_filter.hpp:52
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:415
pcl::MORPH_OPEN
Definition: morphological_filter.h:54
pcl::ProgressiveMorphologicalFilter::extract
virtual void extract(std::vector< int > &ground)
This method launches the segmentation algorithm and returns indices of points determined to be ground...
Definition: progressive_morphological_filter.hpp:71
pcl::ProgressiveMorphologicalFilter::~ProgressiveMorphologicalFilter
~ProgressiveMorphologicalFilter()
Definition: progressive_morphological_filter.hpp:65