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;
87 float window_size = 0.0f;
88 float height_threshold = 0.0f;
90 while (window_size < max_window_size_)
94 half_size =
static_cast<int> (std::pow (static_cast<float> (base_), iteration));
96 half_size = (iteration+1) * base_;
98 window_size = 2 * half_size + 1;
102 height_threshold = initial_distance_;
104 height_threshold = slope_ * (window_size - window_sizes[iteration-1]) * cell_size_ + initial_distance_;
107 if (height_threshold > max_distance_)
108 height_threshold = max_distance_;
110 half_sizes.push_back (half_size);
111 window_sizes.push_back (window_size);
112 height_thresholds.push_back (height_threshold);
118 Eigen::Vector4f global_max, global_min;
119 pcl::getMinMax3D<PointT> (*input_, global_min, global_max);
121 float xextent = global_max.x () - global_min.x ();
122 float yextent = global_max.y () - global_min.y ();
124 int rows =
static_cast<int> (std::floor (yextent / cell_size_) + 1);
125 int cols =
static_cast<int> (std::floor (xextent / cell_size_) + 1);
127 Eigen::MatrixXf A (rows, cols);
128 A.setConstant (std::numeric_limits<float>::quiet_NaN ());
130 Eigen::MatrixXf Z (rows, cols);
131 Z.setConstant (std::numeric_limits<float>::quiet_NaN ());
133 Eigen::MatrixXf Zf (rows, cols);
134 Zf.setConstant (std::numeric_limits<float>::quiet_NaN ());
137 #pragma omp parallel for num_threads(threads_) 139 for (
int i = 0; i < (int)input_->points.size (); ++i)
142 PointT p = input_->points[i];
143 int row = std::floor((p.y - global_min.y ()) / cell_size_);
144 int col = std::floor((p.x - global_min.x ()) / cell_size_);
146 if (p.z < A (row, col) || pcl_isnan (A (row, col)))
157 for (
size_t i = 0; i < window_sizes.size (); ++i)
159 PCL_DEBUG (
" Iteration %d (height threshold = %f, window size = %f, half size = %d)...",
160 i, height_thresholds[i], window_sizes[i], half_sizes[i]);
164 pcl::copyPointCloud<PointT> (*input_, ground, *cloud);
168 #pragma omp parallel for num_threads(threads_) 170 for (
int row = 0; row < rows; ++row)
173 rs = ((row - half_sizes[i]) < 0) ? 0 : row - half_sizes[i];
174 re = ((row + half_sizes[i]) > (rows-1)) ? (rows-1) : row + half_sizes[i];
176 for (
int col = 0; col < cols; ++col)
179 cs = ((col - half_sizes[i]) < 0) ? 0 : col - half_sizes[i];
180 ce = ((col + half_sizes[i]) > (cols-1)) ? (cols-1) : col + half_sizes[i];
182 float min_coeff = std::numeric_limits<float>::max ();
184 for (
int j = rs; j < (re + 1); ++j)
186 for (
int k = cs; k < (ce + 1); ++k)
188 if (A (j, k) != std::numeric_limits<float>::quiet_NaN ())
190 if (A (j, k) < min_coeff)
191 min_coeff = A (j, k);
196 if (min_coeff != std::numeric_limits<float>::max ())
197 Z(row, col) = min_coeff;
202 #pragma omp parallel for num_threads(threads_) 204 for (
int row = 0; row < rows; ++row)
207 rs = ((row - half_sizes[i]) < 0) ? 0 : row - half_sizes[i];
208 re = ((row + half_sizes[i]) > (rows-1)) ? (rows-1) : row + half_sizes[i];
210 for (
int col = 0; col < cols; ++col)
213 cs = ((col - half_sizes[i]) < 0) ? 0 : col - half_sizes[i];
214 ce = ((col + half_sizes[i]) > (cols-1)) ? (cols-1) : col + half_sizes[i];
216 float max_coeff = -std::numeric_limits<float>::max ();
218 for (
int j = rs; j < (re + 1); ++j)
220 for (
int k = cs; k < (ce + 1); ++k)
222 if (Z (j, k) != std::numeric_limits<float>::quiet_NaN ())
224 if (Z (j, k) > max_coeff)
225 max_coeff = Z (j, k);
230 if (max_coeff != -std::numeric_limits<float>::max ())
231 Zf (row, col) = max_coeff;
237 std::vector<int> pt_indices;
238 for (
size_t p_idx = 0; p_idx < ground.size (); ++p_idx)
241 int erow =
static_cast<int> (std::floor ((p.y - global_min.y ()) / cell_size_));
242 int ecol =
static_cast<int> (std::floor ((p.x - global_min.x ()) / cell_size_));
244 float diff = p.z - Zf (erow, ecol);
245 if (diff < height_thresholds[i])
246 pt_indices.push_back (ground[p_idx]);
252 ground.swap (pt_indices);
254 PCL_DEBUG (
"ground now has %d points\n", ground.size ());
261 #define PCL_INSTANTIATE_ApproximateProgressiveMorphologicalFilter(T) template class pcl::ApproximateProgressiveMorphologicalFilter<T>; 263 #endif // PCL_SEGMENTATION_APPROXIMATE_PROGRESSIVE_MORPHOLOGICAL_FILTER_HPP_ std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
boost::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.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
virtual ~ApproximateProgressiveMorphologicalFilter()
ApproximateProgressiveMorphologicalFilter()
Constructor that sets default values for member variables.
virtual void extract(std::vector< int > &ground)
This method launches the segmentation algorithm and returns indices of points determined to be ground...
A point structure representing Euclidean xyz coordinates, and the RGB color.