39 #ifndef PCL_SEGMENTATION_APPROXIMATE_PROGRESSIVE_MORPHOLOGICAL_FILTER_HPP_ 40 #define PCL_SEGMENTATION_APPROXIMATE_PROGRESSIVE_MORPHOLOGICAL_FILTER_HPP_ 42 #include <pcl/common/common.h> 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> 48 #include <pcl/point_types.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 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;
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;
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 ());
144 int col = std::floor(p.x - global_min.x ());
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.
float max_distance_
Maximum height above the parameterized ground surface to be considered a ground return.
IndicesPtr indices_
A pointer to the vector of point indices to use.
float slope_
Slope value to be used in computing the height threshold.
bool initCompute()
This method should get called before starting the actual computation.
boost::shared_ptr< PointCloud< PointT > > Ptr
float cell_size_
Cell size.
int max_window_size_
Maximum window size to be used in filtering ground returns.
float initial_distance_
Initial height above the parameterized ground surface to be considered a ground return.
bool deinitCompute()
This method should get called after finishing the actual computation.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
virtual ~ApproximateProgressiveMorphologicalFilter()
ApproximateProgressiveMorphologicalFilter()
Constructor that sets default values for member variables.
float base_
Base to be used in computing progressive window sizes.
bool exponential_
Exponentially grow window sizes?
virtual void extract(std::vector< int > &ground)
This method launches the segmentation algorithm and returns indices of points determined to be ground...
PointCloudConstPtr input_
The input point cloud dataset.
A point structure representing Euclidean xyz coordinates, and the RGB color.