39 #ifndef PCL_SEGMENTATION_APPROXIMATE_PROGRESSIVE_MORPHOLOGICAL_FILTER_HPP_
40 #define PCL_SEGMENTATION_APPROXIMATE_PROGRESSIVE_MORPHOLOGICAL_FILTER_HPP_
43 #include <pcl/common/io.h>
44 #include <pcl/filters/morphological_filter.h>
45 #include <pcl/segmentation/approximate_progressive_morphological_filter.h>
46 #include <pcl/point_cloud.h>
50 template <
typename Po
intT>
52 max_window_size_ (33),
54 max_distance_ (10.0f),
55 initial_distance_ (0.15f),
64 template <
typename Po
intT>
68 template <
typename Po
intT>
void
71 bool segmentation_is_possible = initCompute ();
72 if (!segmentation_is_possible)
79 std::vector<float> height_thresholds;
80 std::vector<float> window_sizes;
81 std::vector<int> half_sizes;
83 float window_size = 0.0f;
85 while (window_size < max_window_size_)
88 int half_size = (exponential_) ? (
static_cast<int> (std::pow (
static_cast<float> (base_), iteration))) : ((iteration+1) * base_);
90 window_size = 2 * half_size + 1;
93 float height_threshold = (iteration == 0) ? (initial_distance_) : (slope_ * (window_size - window_sizes[iteration-1]) * cell_size_ + initial_distance_);
96 if (height_threshold > max_distance_)
97 height_threshold = max_distance_;
99 half_sizes.push_back (half_size);
100 window_sizes.push_back (window_size);
101 height_thresholds.push_back (height_threshold);
107 Eigen::Vector4f global_max, global_min;
108 pcl::getMinMax3D<PointT> (*input_, global_min, global_max);
110 float xextent = global_max.x () - global_min.x ();
111 float yextent = global_max.y () - global_min.y ();
113 int rows =
static_cast<int> (std::floor (yextent / cell_size_) + 1);
114 int cols =
static_cast<int> (std::floor (xextent / cell_size_) + 1);
116 Eigen::MatrixXf A (rows, cols);
117 A.setConstant (std::numeric_limits<float>::quiet_NaN ());
119 Eigen::MatrixXf Z (rows, cols);
120 Z.setConstant (std::numeric_limits<float>::quiet_NaN ());
122 Eigen::MatrixXf Zf (rows, cols);
123 Zf.setConstant (std::numeric_limits<float>::quiet_NaN ());
125 #pragma omp parallel for \
127 shared(A, global_min) \
128 num_threads(threads_)
129 for (
int i = 0; i < static_cast<int>(input_->size ()); ++i)
133 int row = std::floor((p.y - global_min.y ()) / cell_size_);
134 int col = std::floor((p.x - global_min.x ()) / cell_size_);
136 if (p.z < A (row, col) || std::isnan (A (row, col)))
147 for (std::size_t i = 0; i < window_sizes.size (); ++i)
149 PCL_DEBUG (
" Iteration %d (height threshold = %f, window size = %f, half size = %d)...",
150 i, height_thresholds[i], window_sizes[i], half_sizes[i]);
154 pcl::copyPointCloud<PointT> (*input_, ground, *cloud);
157 #pragma omp parallel for \
159 shared(A, cols, half_sizes, i, rows, Z) \
160 num_threads(threads_)
161 for (
int row = 0; row < rows; ++row)
164 rs = ((row - half_sizes[i]) < 0) ? 0 : row - half_sizes[i];
165 re = ((row + half_sizes[i]) > (rows-1)) ? (rows-1) : row + half_sizes[i];
167 for (
int col = 0; col < cols; ++col)
170 cs = ((col - half_sizes[i]) < 0) ? 0 : col - half_sizes[i];
171 ce = ((col + half_sizes[i]) > (cols-1)) ? (cols-1) : col + half_sizes[i];
173 float min_coeff = std::numeric_limits<float>::max ();
175 for (
int j = rs; j < (re + 1); ++j)
177 for (
int k = cs; k < (ce + 1); ++k)
179 if (A (j, k) != std::numeric_limits<float>::quiet_NaN ())
181 if (A (j, k) < min_coeff)
182 min_coeff = A (j, k);
187 if (min_coeff != std::numeric_limits<float>::max ())
188 Z(row, col) = min_coeff;
192 #pragma omp parallel for \
194 shared(cols, half_sizes, i, rows, Z, Zf) \
195 num_threads(threads_)
196 for (
int row = 0; row < rows; ++row)
199 rs = ((row - half_sizes[i]) < 0) ? 0 : row - half_sizes[i];
200 re = ((row + half_sizes[i]) > (rows-1)) ? (rows-1) : row + half_sizes[i];
202 for (
int col = 0; col < cols; ++col)
205 cs = ((col - half_sizes[i]) < 0) ? 0 : col - half_sizes[i];
206 ce = ((col + half_sizes[i]) > (cols-1)) ? (cols-1) : col + half_sizes[i];
208 float max_coeff = -std::numeric_limits<float>::max ();
210 for (
int j = rs; j < (re + 1); ++j)
212 for (
int k = cs; k < (ce + 1); ++k)
214 if (Z (j, k) != std::numeric_limits<float>::quiet_NaN ())
216 if (Z (j, k) > max_coeff)
217 max_coeff = Z (j, k);
222 if (max_coeff != -std::numeric_limits<float>::max ())
223 Zf (row, col) = max_coeff;
230 for (std::size_t p_idx = 0; p_idx < ground.size (); ++p_idx)
232 PointT p = (*cloud)[p_idx];
233 int erow =
static_cast<int> (std::floor ((p.y - global_min.y ()) / cell_size_));
234 int ecol =
static_cast<int> (std::floor ((p.x - global_min.x ()) / cell_size_));
236 float diff = p.z - Zf (erow, ecol);
237 if (diff < height_thresholds[i])
238 pt_indices.push_back (ground[p_idx]);
244 ground.swap (pt_indices);
246 PCL_DEBUG (
"ground now has %d points\n", ground.size ());
253 #define PCL_INSTANTIATE_ApproximateProgressiveMorphologicalFilter(T) template class pcl::ApproximateProgressiveMorphologicalFilter<T>;
virtual void extract(Indices &ground)
This method launches the segmentation algorithm and returns indices of points determined to be ground...
ApproximateProgressiveMorphologicalFilter()
Constructor that sets default values for member variables.
~ApproximateProgressiveMorphologicalFilter() override
PointCloud represents the base class in PCL for storing collections of 3D points.
shared_ptr< PointCloud< PointT > > Ptr
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.
A point structure representing Euclidean xyz coordinates, and the RGB color.