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>
71 template <
typename Po
intT>
void
74 bool segmentation_is_possible = initCompute ();
75 if (!segmentation_is_possible)
82 std::vector<float> height_thresholds;
83 std::vector<float> window_sizes;
84 std::vector<int> half_sizes;
86 float window_size = 0.0f;
88 while (window_size < max_window_size_)
91 int half_size = (exponential_) ? (
static_cast<int> (std::pow (
static_cast<float> (base_), iteration))) : ((iteration+1) * base_);
93 window_size = 2 * half_size + 1;
96 float height_threshold = (iteration == 0) ? (initial_distance_) : (slope_ * (window_size - window_sizes[iteration-1]) * cell_size_ + initial_distance_);
99 if (height_threshold > max_distance_)
100 height_threshold = max_distance_;
102 half_sizes.push_back (half_size);
103 window_sizes.push_back (window_size);
104 height_thresholds.push_back (height_threshold);
110 Eigen::Vector4f global_max, global_min;
111 pcl::getMinMax3D<PointT> (*input_, global_min, global_max);
113 float xextent = global_max.x () - global_min.x ();
114 float yextent = global_max.y () - global_min.y ();
116 int rows =
static_cast<int> (std::floor (yextent / cell_size_) + 1);
117 int cols =
static_cast<int> (std::floor (xextent / cell_size_) + 1);
119 Eigen::MatrixXf A (rows, cols);
120 A.setConstant (std::numeric_limits<float>::quiet_NaN ());
122 Eigen::MatrixXf Z (rows, cols);
123 Z.setConstant (std::numeric_limits<float>::quiet_NaN ());
125 Eigen::MatrixXf Zf (rows, cols);
126 Zf.setConstant (std::numeric_limits<float>::quiet_NaN ());
128 #pragma omp parallel for \
130 shared(A, global_min) \
131 num_threads(threads_)
132 for (
int i = 0; i < (int)input_->size (); ++i)
136 int row = std::floor((p.y - global_min.y ()) / cell_size_);
137 int col = std::floor((p.x - global_min.x ()) / cell_size_);
139 if (p.z < A (row, col) || std::isnan (A (row, col)))
150 for (std::size_t i = 0; i < window_sizes.size (); ++i)
152 PCL_DEBUG (
" Iteration %d (height threshold = %f, window size = %f, half size = %d)...",
153 i, height_thresholds[i], window_sizes[i], half_sizes[i]);
157 pcl::copyPointCloud<PointT> (*input_, ground, *cloud);
160 #pragma omp parallel for \
162 shared(A, cols, half_sizes, i, rows, Z) \
163 num_threads(threads_)
164 for (
int row = 0; row < rows; ++row)
167 rs = ((row - half_sizes[i]) < 0) ? 0 : row - half_sizes[i];
168 re = ((row + half_sizes[i]) > (rows-1)) ? (rows-1) : row + half_sizes[i];
170 for (
int col = 0; col < cols; ++col)
173 cs = ((col - half_sizes[i]) < 0) ? 0 : col - half_sizes[i];
174 ce = ((col + half_sizes[i]) > (cols-1)) ? (cols-1) : col + half_sizes[i];
176 float min_coeff = std::numeric_limits<float>::max ();
178 for (
int j = rs; j < (re + 1); ++j)
180 for (
int k = cs; k < (ce + 1); ++k)
182 if (A (j, k) != std::numeric_limits<float>::quiet_NaN ())
184 if (A (j, k) < min_coeff)
185 min_coeff = A (j, k);
190 if (min_coeff != std::numeric_limits<float>::max ())
191 Z(row, col) = min_coeff;
195 #pragma omp parallel for \
197 shared(cols, half_sizes, i, rows, Z, Zf) \
198 num_threads(threads_)
199 for (
int row = 0; row < rows; ++row)
202 rs = ((row - half_sizes[i]) < 0) ? 0 : row - half_sizes[i];
203 re = ((row + half_sizes[i]) > (rows-1)) ? (rows-1) : row + half_sizes[i];
205 for (
int col = 0; col < cols; ++col)
208 cs = ((col - half_sizes[i]) < 0) ? 0 : col - half_sizes[i];
209 ce = ((col + half_sizes[i]) > (cols-1)) ? (cols-1) : col + half_sizes[i];
211 float max_coeff = -std::numeric_limits<float>::max ();
213 for (
int j = rs; j < (re + 1); ++j)
215 for (
int k = cs; k < (ce + 1); ++k)
217 if (Z (j, k) != std::numeric_limits<float>::quiet_NaN ())
219 if (Z (j, k) > max_coeff)
220 max_coeff = Z (j, k);
225 if (max_coeff != -std::numeric_limits<float>::max ())
226 Zf (row, col) = max_coeff;
233 for (std::size_t p_idx = 0; p_idx < ground.size (); ++p_idx)
235 PointT p = (*cloud)[p_idx];
236 int erow =
static_cast<int> (std::floor ((p.y - global_min.y ()) / cell_size_));
237 int ecol =
static_cast<int> (std::floor ((p.x - global_min.x ()) / cell_size_));
239 float diff = p.z - Zf (erow, ecol);
240 if (diff < height_thresholds[i])
241 pt_indices.push_back (ground[p_idx]);
247 ground.swap (pt_indices);
249 PCL_DEBUG (
"ground now has %d points\n", ground.size ());
256 #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()
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.