Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
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/segmentation/progressive_morphological_filter.h>
46#include <pcl/point_cloud.h>
47#include <pcl/point_types.h>
48
49//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
50template <typename PointT>
52 max_window_size_ (33),
53 slope_ (0.7f),
54 max_distance_ (10.0f),
55 initial_distance_ (0.15f),
56 cell_size_ (1.0f),
57 base_ (2.0f),
58 exponential_ (true)
59{
60}
61
62//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
63template <typename PointT>
67
68//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
69template <typename PointT> void
71{
72 bool segmentation_is_possible = initCompute ();
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 int iteration = 0;
83 float window_size = 0.0f;
84 float height_threshold = 0.0f;
85
86 while (window_size < max_window_size_)
87 {
88 // Determine the initial window size.
89 if (exponential_)
90 window_size = cell_size_ * (2.0f * std::pow (base_, iteration) + 1.0f);
91 else
92 window_size = cell_size_ * (2.0f * (iteration+1) * base_ + 1.0f);
93
94 // Calculate the height threshold to be used in the next iteration.
95 if (iteration == 0)
96 height_threshold = initial_distance_;
97 else
98 height_threshold = slope_ * (window_size - window_sizes[iteration-1]) * cell_size_ + initial_distance_;
99
100 // Enforce max distance on height threshold
101 if (height_threshold > max_distance_)
102 height_threshold = max_distance_;
103
104 window_sizes.push_back (window_size);
106
107 iteration++;
108 }
109
110 // Ground indices are initially limited to those points in the input cloud we
111 // wish to process
112 ground = *indices_;
113
114 // Progressively filter ground returns using morphological open
115 for (std::size_t i = 0; i < window_sizes.size (); ++i)
116 {
117 PCL_DEBUG (" Iteration %d (height threshold = %f, window size = %f)...",
119
120 // Limit filtering to those points currently considered ground returns
122 pcl::copyPointCloud<PointT> (*input_, ground, *cloud);
123
124 // Create new cloud to hold the filtered results. Apply the morphological
125 // opening operation at the current window size.
128
129 // Find indices of the points whose difference between the source and
130 // filtered point clouds is less than the current height threshold.
132 for (std::size_t p_idx = 0; p_idx < ground.size (); ++p_idx)
133 {
134 float diff = (*cloud)[p_idx].z - (*cloud_f)[p_idx].z;
135 if (diff < height_thresholds[i])
136 pt_indices.push_back (ground[p_idx]);
137 }
138
139 // Ground is now limited to pt_indices
140 ground.swap (pt_indices);
141
142 PCL_DEBUG ("ground now has %d points\n", ground.size ());
143 }
144
145 deinitCompute ();
146}
147
148#define PCL_INSTANTIATE_ProgressiveMorphologicalFilter(T) template class pcl::ProgressiveMorphologicalFilter<T>;
149
150#endif // PCL_SEGMENTATION_PROGRESSIVE_MORPHOLOGICAL_FILTER_HPP_
151
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
shared_ptr< PointCloud< PointT > > Ptr
virtual void extract(Indices &ground)
This method launches the segmentation algorithm and returns indices of points determined to be ground...
ProgressiveMorphologicalFilter()
Constructor that sets default values for member variables.
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