PCL支持点云的形态学滤波，四种操作：侵蚀、膨胀、开（先侵蚀后膨胀）、闭（先膨胀后侵蚀）
在#include <pcl/filters/morphological_filter.h>中定义了枚举类型
1 enum MorphologicalOperators
2 {
3 MORPH_OPEN,
4 MORPH_CLOSE,
5 MORPH_DILATE,
6 MORPH_ERODE
7 };
具体实现：
1 template <typename PointT> void
2 pcl::applyMorphologicalOperator (const typename pcl::PointCloud<PointT>::ConstPtr &cloud_in,
3 float resolution, const int morphological_operator,
4 pcl::PointCloud<PointT> &cloud_out)
5 {
6 if (cloud_in->empty ())
7 return;
8
9 pcl::copyPointCloud<PointT, PointT> (*cloud_in, cloud_out);
10
11 pcl::octree::OctreePointCloudSearch<PointT> tree (resolution);
12
13 tree.setInputCloud (cloud_in);
14 tree.addPointsFromInputCloud ();
15
16 float half_res = resolution / 2.0f;
17
18 switch (morphological_operator)
19 {
20 case MORPH_DILATE:
21 case MORPH_ERODE:
22 {
23 for (size_t p_idx = 0; p_idx < cloud_in->points.size (); ++p_idx)
24 {
25 Eigen::Vector3f bbox_min, bbox_max;
26 std::vector<int> pt_indices;
27 float minx = cloud_in->points[p_idx].x - half_res;
28 float miny = cloud_in->points[p_idx].y - half_res;
29 float minz = -std::numeric_limits<float>::max ();
30 float maxx = cloud_in->points[p_idx].x + half_res;
31 float maxy = cloud_in->points[p_idx].y + half_res;
32 float maxz = std::numeric_limits<float>::max ();
33 bbox_min = Eigen::Vector3f (minx, miny, minz);
34 bbox_max = Eigen::Vector3f (maxx, maxy, maxz);
35 tree.boxSearch (bbox_min, bbox_max, pt_indices);
36
37 if (pt_indices.size () > 0)
38 {
39 Eigen::Vector4f min_pt, max_pt;
40 pcl::getMinMax3D<PointT> (*cloud_in, pt_indices, min_pt, max_pt);
41
42 switch (morphological_operator)
43 {
44 case MORPH_DILATE:
45 {
46 cloud_out.points[p_idx].z = max_pt.z ();
47 break;
48 }
49 case MORPH_ERODE:
50 {
51 cloud_out.points[p_idx].z = min_pt.z ();
52 break;
53 }
54 }
55 }
56 }
57 break;
58 }
59 case MORPH_OPEN:
60 case MORPH_CLOSE:
61 {
62 pcl::PointCloud<PointT> cloud_temp;
63
64 pcl::copyPointCloud<PointT, PointT> (*cloud_in, cloud_temp);
65
66 for (size_t p_idx = 0; p_idx < cloud_temp.points.size (); ++p_idx)
67 {
68 Eigen::Vector3f bbox_min, bbox_max;
69 std::vector<int> pt_indices;
70 float minx = cloud_temp.points[p_idx].x - half_res;
71 float miny = cloud_temp.points[p_idx].y - half_res;
72 float minz = -std::numeric_limits<float>::max ();
73 float maxx = cloud_temp.points[p_idx].x + half_res;
74 float maxy = cloud_temp.points[p_idx].y + half_res;
75 float maxz = std::numeric_limits<float>::max ();
76 bbox_min = Eigen::Vector3f (minx, miny, minz);
77 bbox_max = Eigen::Vector3f (maxx, maxy, maxz);
78 tree.boxSearch (bbox_min, bbox_max, pt_indices);
79
80 if (pt_indices.size () > 0)
81 {
82 Eigen::Vector4f min_pt, max_pt;
83 pcl::getMinMax3D<PointT> (cloud_temp, pt_indices, min_pt, max_pt);
84
85 switch (morphological_operator)
86 {
87 case MORPH_OPEN:
88 {
89 cloud_out.points[p_idx].z = min_pt.z ();
90 break;
91 }
92 case MORPH_CLOSE:
93 {
94 cloud_out.points[p_idx].z = max_pt.z ();
95 break;
96 }
97 }
98 }
99 }
100
101 cloud_temp.swap (cloud_out);
102
103 for (size_t p_idx = 0; p_idx < cloud_temp.points.size (); ++p_idx)
104 {
105 Eigen::Vector3f bbox_min, bbox_max;
106 std::vector<int> pt_indices;
107 float minx = cloud_temp.points[p_idx].x - half_res;
108 float miny = cloud_temp.points[p_idx].y - half_res;
109 float minz = -std::numeric_limits<float>::max ();
110 float maxx = cloud_temp.points[p_idx].x + half_res;
111 float maxy = cloud_temp.points[p_idx].y + half_res;
112 float maxz = std::numeric_limits<float>::max ();
113 bbox_min = Eigen::Vector3f (minx, miny, minz);
114 bbox_max = Eigen::Vector3f (maxx, maxy, maxz);
115 tree.boxSearch (bbox_min, bbox_max, pt_indices);
116
117 if (pt_indices.size () > 0)
118 {
119 Eigen::Vector4f min_pt, max_pt;
120 pcl::getMinMax3D<PointT> (cloud_temp, pt_indices, min_pt, max_pt);
121
122 switch (morphological_operator)
123 {
124 case MORPH_OPEN:
125 default:
126 {
127 cloud_out.points[p_idx].z = max_pt.z ();
128 break;
129 }
130 case MORPH_CLOSE:
131 {
132 cloud_out.points[p_idx].z = min_pt.z ();
133 break;
134 }
135 }
136 }
137 }
138 break;
139 }
140 default:
141 {
142 PCL_ERROR ("Morphological operator is not supported!\n");
143 break;
144 }
145 }
146
147 return;
148 }
而渐进形态学滤波则是逐渐增大窗口，循环进行开操作
template <typename PointT> void
pcl::ProgressiveMorphologicalFilter<PointT>::extract (std::vector<int>& ground)
{
bool segmentation_is_possible = initCompute ();
if (!segmentation_is_possible)
{
deinitCompute ();
return;
}
// Compute the series of window sizes and height thresholds
std::vector<float> height_thresholds;
std::vector<float> window_sizes;
int iteration = 0;
float window_size = 0.0f;
float height_threshold = 0.0f;
while (window_size < max_window_size_)
{
// Determine the initial window size.
if (exponential_)
window_size = cell_size_ * (2.0f * std::pow (base_, iteration) + 1.0f);
else
window_size = cell_size_ * (2.0f * (iteration+1) * base_ + 1.0f);
// Calculate the height threshold to be used in the next iteration.
if (iteration == 0)
height_threshold = initial_distance_;
else
height_threshold = slope_ * (window_size - window_sizes[iteration-1]) * cell_size_ + initial_distance_;
// Enforce max distance on height threshold
if (height_threshold > max_distance_)
height_threshold = max_distance_;
window_sizes.push_back (window_size);
height_thresholds.push_back (height_threshold);
iteration++;
}
// Ground indices are initially limited to those points in the input cloud we
// wish to process
ground = *indices_;
// Progressively filter ground returns using morphological open
for (size_t i = 0; i < window_sizes.size (); ++i)
{
PCL_DEBUG (" Iteration %d (height threshold = %f, window size = %f)...",
i, height_thresholds[i], window_sizes[i]);
// Limit filtering to those points currently considered ground returns
typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
pcl::copyPointCloud<PointT> (*input_, ground, *cloud);
// Create new cloud to hold the filtered results. Apply the morphological
// opening operation at the current window size.
typename pcl::PointCloud<PointT>::Ptr cloud_f (new pcl::PointCloud<PointT>);
pcl::applyMorphologicalOperator<PointT> (cloud, window_sizes[i], MORPH_OPEN, *cloud_f);
// Find indices of the points whose difference between the source and
// filtered point clouds is less than the current height threshold.
std::vector<int> pt_indices;
for (size_t p_idx = 0; p_idx < ground.size (); ++p_idx)
{
float diff = cloud->points[p_idx].z - cloud_f->points[p_idx].z;
if (diff < height_thresholds[i])
pt_indices.push_back (ground[p_idx]);
}
// Ground is now limited to pt_indices
ground.swap (pt_indices);
PCL_DEBUG ("ground now has %d points\n", ground.size ());
}
deinitCompute ();
}
时间： 2024-10-13 01:46:27