PCL支持点云的形态学滤波，四种操作：侵蚀、膨胀、开（先侵蚀后膨胀）、闭（先膨胀后侵蚀）
关于渐进的策略，在初始cell_size_ 的基础上逐渐变大。满足如下公式：
$$window\_size =cell\_size *(2*base^{k}+1)$$
$$window\_size =cell\_size *(2*base*(k+1)+1)$$
1 // Compute the series of window sizes and height thresholds 2 std::vector<float> height_thresholds; 3 std::vector<float> window_sizes; 4 int iteration = 0; 5 float window_size = 0.0f; 6 float height_threshold = 0.0f; 7 8 while (window_size < max_window_size_) 9 { 10 // Determine the initial window size. 11 if (exponential_) 12 window_size = cell_size_ * (2.0f * std::pow (base_, iteration) + 1.0f); 13 else 14 window_size = cell_size_ * (2.0f * (iteration+1) * base_ + 1.0f); 15 16 // Calculate the height threshold to be used in the next iteration. 17 if (iteration == 0) 18 height_threshold = initial_distance_; 19 else 20 height_threshold = slope_ * (window_size - window_sizes[iteration-1]) * cell_size_ + initial_distance_; 21 22 // Enforce max distance on height threshold 23 if (height_threshold > max_distance_) 24 height_threshold = max_distance_; 25 26 window_sizes.push_back (window_size); 27 height_thresholds.push_back (height_threshold); 28 29 iteration++; 30 }
在#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! "); 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 ", ground.size ()); } deinitCompute (); }