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/filters/extract_indices.h>
46 #include <pcl/segmentation/approximate_progressive_morphological_filter.h>
47 #include <pcl/point_cloud.h>
51 template <
typename Po
intT>
53 max_window_size_ (33),
55 max_distance_ (10.0f),
56 initial_distance_ (0.15f),
65 template <
typename Po
intT>
69 template <
typename Po
intT>
void
72 bool segmentation_is_possible = initCompute ();
73 if (!segmentation_is_possible)
80 std::vector<float> height_thresholds;
81 std::vector<float> window_sizes;
82 std::vector<int> half_sizes;
84 float window_size = 0.0f;
86 while (window_size < max_window_size_)
89 int half_size = (exponential_) ? (
static_cast<int> (std::pow (
static_cast<float> (base_), iteration))) : ((iteration+1) * base_);
91 window_size = 2 * half_size + 1;
94 float height_threshold = (iteration == 0) ? (initial_distance_) : (slope_ * (window_size - window_sizes[iteration-1]) * cell_size_ + initial_distance_);
97 if (height_threshold > max_distance_)
98 height_threshold = max_distance_;
100 half_sizes.push_back (half_size);
101 window_sizes.push_back (window_size);
102 height_thresholds.push_back (height_threshold);
108 Eigen::Vector4f global_max, global_min;
109 pcl::getMinMax3D<PointT> (*input_, global_min, global_max);
111 float xextent = global_max.x () - global_min.x ();
112 float yextent = global_max.y () - global_min.y ();
114 int rows =
static_cast<int> (std::floor (yextent / cell_size_) + 1);
115 int cols =
static_cast<int> (std::floor (xextent / cell_size_) + 1);
117 Eigen::MatrixXf A (rows, cols);
118 A.setConstant (std::numeric_limits<float>::quiet_NaN ());
120 Eigen::MatrixXf Z (rows, cols);
121 Z.setConstant (std::numeric_limits<float>::quiet_NaN ());
123 Eigen::MatrixXf Zf (rows, cols);
124 Zf.setConstant (std::numeric_limits<float>::quiet_NaN ());
126 #pragma omp parallel for \
128 shared(A, global_min) \
129 num_threads(threads_)
130 for (
int i = 0; i < (int)input_->size (); ++i)
134 int row = std::floor((p.y - global_min.y ()) / cell_size_);
135 int col = std::floor((p.x - global_min.x ()) / cell_size_);
137 if (p.z < A (row, col) || std::isnan (A (row, col)))
148 for (std::size_t i = 0; i < window_sizes.size (); ++i)
150 PCL_DEBUG (
" Iteration %d (height threshold = %f, window size = %f, half size = %d)...",
151 i, height_thresholds[i], window_sizes[i], half_sizes[i]);
155 pcl::copyPointCloud<PointT> (*input_, ground, *cloud);
158 #pragma omp parallel for \
160 shared(A, cols, half_sizes, i, rows, Z) \
161 num_threads(threads_)
162 for (
int row = 0; row < rows; ++row)
165 rs = ((row - half_sizes[i]) < 0) ? 0 : row - half_sizes[i];
166 re = ((row + half_sizes[i]) > (rows-1)) ? (rows-1) : row + half_sizes[i];
168 for (
int col = 0; col < cols; ++col)
171 cs = ((col - half_sizes[i]) < 0) ? 0 : col - half_sizes[i];
172 ce = ((col + half_sizes[i]) > (cols-1)) ? (cols-1) : col + half_sizes[i];
174 float min_coeff = std::numeric_limits<float>::max ();
176 for (
int j = rs; j < (re + 1); ++j)
178 for (
int k = cs; k < (ce + 1); ++k)
180 if (A (j, k) != std::numeric_limits<float>::quiet_NaN ())
182 if (A (j, k) < min_coeff)
183 min_coeff = A (j, k);
188 if (min_coeff != std::numeric_limits<float>::max ())
189 Z(row, col) = min_coeff;
193 #pragma omp parallel for \
195 shared(cols, half_sizes, i, rows, Z, Zf) \
196 num_threads(threads_)
197 for (
int row = 0; row < rows; ++row)
200 rs = ((row - half_sizes[i]) < 0) ? 0 : row - half_sizes[i];
201 re = ((row + half_sizes[i]) > (rows-1)) ? (rows-1) : row + half_sizes[i];
203 for (
int col = 0; col < cols; ++col)
206 cs = ((col - half_sizes[i]) < 0) ? 0 : col - half_sizes[i];
207 ce = ((col + half_sizes[i]) > (cols-1)) ? (cols-1) : col + half_sizes[i];
209 float max_coeff = -std::numeric_limits<float>::max ();
211 for (
int j = rs; j < (re + 1); ++j)
213 for (
int k = cs; k < (ce + 1); ++k)
215 if (Z (j, k) != std::numeric_limits<float>::quiet_NaN ())
217 if (Z (j, k) > max_coeff)
218 max_coeff = Z (j, k);
223 if (max_coeff != -std::numeric_limits<float>::max ())
224 Zf (row, col) = max_coeff;
231 for (std::size_t p_idx = 0; p_idx < ground.size (); ++p_idx)
233 PointT p = (*cloud)[p_idx];
234 int erow =
static_cast<int> (std::floor ((p.y - global_min.y ()) / cell_size_));
235 int ecol =
static_cast<int> (std::floor ((p.x - global_min.x ()) / cell_size_));
237 float diff = p.z - Zf (erow, ecol);
238 if (diff < height_thresholds[i])
239 pt_indices.push_back (ground[p_idx]);
245 ground.swap (pt_indices);
247 PCL_DEBUG (
"ground now has %d points\n", ground.size ());
254 #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.