PCL支持点云的形态学滤波，四种操作：侵蚀、膨胀、开（先侵蚀后膨胀）、闭（先膨胀后侵蚀）
关于渐进的策略，在初始cell_size_ 的基础上逐渐变大。满足如下公式：
$$window\_size =cell\_size *(2*base^{k}+1)$$
$$window\_size =cell\_size *(2*base*(k+1)+1)$$
// Compute the series of window sizes and height thresholds
std::vector<float> height_thresholds;
std::vector<float> window_sizes;
int iteration = ;
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+) * base_ + 1.0f); // Calculate the height threshold to be used in the next iteration.
if (iteration == )
height_threshold = initial_distance_;
else
height_threshold = slope_ * (window_size - window_sizes[iteration-]) * 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++;
}
在#include <pcl/filters/morphological_filter.h>中定义了枚举类型
enum MorphologicalOperators
{
MORPH_OPEN,
MORPH_CLOSE,
MORPH_DILATE,
MORPH_ERODE
};
具体实现：
template <typename PointT> void
pcl::applyMorphologicalOperator (const typename pcl::PointCloud<PointT>::ConstPtr &cloud_in,
float resolution, const int morphological_operator,
pcl::PointCloud<PointT> &cloud_out)
{
if (cloud_in->empty ())
return; pcl::copyPointCloud<PointT, PointT> (*cloud_in, cloud_out); pcl::octree::OctreePointCloudSearch<PointT> tree (resolution);
tree.setInputCloud (cloud_in);
tree.addPointsFromInputCloud (); float half_res = resolution / 2.0f; switch (morphological_operator)
{
case MORPH_DILATE:
case MORPH_ERODE:
{
for (size_t p_idx = ; p_idx < cloud_in->points.size (); ++p_idx)
{
Eigen::Vector3f bbox_min, bbox_max;
std::vector<int> pt_indices;
float minx = cloud_in->points[p_idx].x - half_res;
float miny = cloud_in->points[p_idx].y - half_res;
float minz = -std::numeric_limits<float>::max ();
float maxx = cloud_in->points[p_idx].x + half_res;
float maxy = cloud_in->points[p_idx].y + half_res;
float maxz = std::numeric_limits<float>::max ();
bbox_min = Eigen::Vector3f (minx, miny, minz);
bbox_max = Eigen::Vector3f (maxx, maxy, maxz);
tree.boxSearch (bbox_min, bbox_max, pt_indices); if (pt_indices.size () > )
{
Eigen::Vector4f min_pt, max_pt;
pcl::getMinMax3D<PointT> (*cloud_in, pt_indices, min_pt, max_pt); switch (morphological_operator)
{
case MORPH_DILATE:
{
cloud_out.points[p_idx].z = max_pt.z ();
break;
}
case MORPH_ERODE:
{
cloud_out.points[p_idx].z = min_pt.z ();
break;
}
}
}
}
break;
}
case MORPH_OPEN:
case MORPH_CLOSE:
{
pcl::PointCloud<PointT> cloud_temp; pcl::copyPointCloud<PointT, PointT> (*cloud_in, cloud_temp); for (size_t p_idx = ; p_idx < cloud_temp.points.size (); ++p_idx)
{
Eigen::Vector3f bbox_min, bbox_max;
std::vector<int> pt_indices;
float minx = cloud_temp.points[p_idx].x - half_res;
float miny = cloud_temp.points[p_idx].y - half_res;
float minz = -std::numeric_limits<float>::max ();
float maxx = cloud_temp.points[p_idx].x + half_res;
float maxy = cloud_temp.points[p_idx].y + half_res;
float maxz = std::numeric_limits<float>::max ();
bbox_min = Eigen::Vector3f (minx, miny, minz);
bbox_max = Eigen::Vector3f (maxx, maxy, maxz);
tree.boxSearch (bbox_min, bbox_max, pt_indices);
if (pt_indices.size () > )
{
Eigen::Vector4f min_pt, max_pt;
pcl::getMinMax3D<PointT> (cloud_temp, pt_indices, min_pt, max_pt); switch (morphological_operator)
{
case MORPH_OPEN:
{
cloud_out.points[p_idx].z = min_pt.z ();
break;
}
case MORPH_CLOSE:
{
cloud_out.points[p_idx].z = max_pt.z ();
break;
}
}
}
} cloud_temp.swap (cloud_out); for (size_t p_idx = ; p_idx < cloud_temp.points.size (); ++p_idx)
{
Eigen::Vector3f bbox_min, bbox_max;
std::vector<int> pt_indices;
float minx = cloud_temp.points[p_idx].x - half_res;
float miny = cloud_temp.points[p_idx].y - half_res;
float minz = -std::numeric_limits<float>::max ();
float maxx = cloud_temp.points[p_idx].x + half_res;
float maxy = cloud_temp.points[p_idx].y + half_res;
float maxz = std::numeric_limits<float>::max ();
bbox_min = Eigen::Vector3f (minx, miny, minz);
bbox_max = Eigen::Vector3f (maxx, maxy, maxz);
tree.boxSearch (bbox_min, bbox_max, pt_indices); if (pt_indices.size () > )
{
Eigen::Vector4f min_pt, max_pt;
pcl::getMinMax3D<PointT> (cloud_temp, pt_indices, min_pt, max_pt); switch (morphological_operator)
{
case MORPH_OPEN:
default:
{
cloud_out.points[p_idx].z = max_pt.z ();
break;
}
case MORPH_CLOSE:
{
cloud_out.points[p_idx].z = min_pt.z ();
break;
}
}
}
}
break;
}
default:
{
PCL_ERROR ("Morphological operator is not supported!\n");
break;
}
} return;
}
而渐进形态学滤波则是逐渐增大窗口，循环进行开操作
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 = ;
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+) * base_ + 1.0f); // Calculate the height threshold to be used in the next iteration.
if (iteration == )
height_threshold = initial_distance_;
else
height_threshold = slope_ * (window_size - window_sizes[iteration-]) * 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 = ; 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 = ; 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 ();
}