Progressive Morphological Filter论文:http://users.cis.fiu.edu/~chens/PDF/TGRS.pdf
本算法本身用于处理高空获取的激光雷达数据,把地面与非地面的物体分割,来获取地貌3d地图的,目前已经集成在PCL中。
具体的算法细节分析以后会详细学习,这里先展示算法效果。此算法参数多,不看论文细节,无法调参数。如果参数不调好,算法耗时很长。对使用者极度不友好。调试过程大部分参考了这篇博客。在此表示感谢。
1.使用Progressive Morphological Filter处理建图结果
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/progressive_morphological_filter.h>
#include <pcl/filters/morphological_filter.h>
#include <pcl/pcl_base.h>
#include <pcl/visualization/cloud_viewer.h>using namespace std;
#define max_window_size 0.05 //
#define slope 0.7f
#define max_distance 0.9f
#define initial_distance 0.5f
#define cell_size 0.01f
#define base 2.0f
#define exponential truepcl::PointIndicesPtr ground (new pcl::PointIndices); //???????
pcl::PointCloud<pcl::PointXYZ>::Ptr my_pmf (pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in)
{
// Compute the series of window sizes and height thresholds
std::vector<float> height_thresholds;
std::vector<float> window_sizes;
std::vector<int> ground_indices;
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);
cout << "window_size " << window_size << endl;
// 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;
cout << "height_threshold " << height_threshold << endl;// 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
for (int i=0;i<cloud_in->points.size();i++)
{
ground_indices.push_back(i);
}// Progressively filter ground returns using morphological open window_sizes.size 为迭代次数
for (size_t i = 0; i < window_sizes.size (); ++i)
{
cout<< "\nIteration " << i << " height threshold = " << height_thresholds[i]
<< " window size = " <<window_sizes[i] << endl;// Limit filtering to those points currently considered ground returns
typename pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud<pcl::PointXYZ> (*cloud_in, ground_indices, *cloud); //将cloud_in的索引为groung_indices的点云复制到cloud// Create new cloud to hold the filtered results. Apply the morphological
// opening operation at the current window size.
typename pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
pcl::applyMorphologicalOperator<pcl::PointXYZ> (cloud, window_sizes[i], pcl::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;
//cout << "ground.size() = " << ground.size() << endl;
for (size_t p_idx = 0; p_idx < ground_indices.size (); ++p_idx)
{
float diff = cloud->points[p_idx].z - cloud_f->points[p_idx].z;
//cout << "diff " << diff << endl;
if (diff < height_thresholds[i])
pt_indices.push_back (ground_indices[p_idx]);
}// Ground is now limited to pt_indices
ground_indices.swap (pt_indices);
cout << "ground now has " << ground_indices.size () << " points" << endl;
}
typename pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);
// Extract cloud_in with ground indices
pcl::copyPointCloud<pcl::PointXYZ> (*cloud_in, ground_indices, *cloud_out);
ground->indices=ground_indices; //索引赋值
return cloud_out;}
void viewerOneOff(pcl::visualization::PCLVisualizer& viewer)
{
viewer.setBackgroundColor(0,0,1);
}int main (int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);pcl::PCDReader reader; //读取pcd文件
reader.read<pcl::PointXYZ> ("/home/zqt/map/1.pcd", *cloud);
//reader.read<pcl::PointXYZ> ("/home/zqt/map/changsha10cm.pcd", *cloud);
//reader.read<pcl::PointXYZ> ("/home/zqt/map/zjuLab.pcd", *cloud);std::cout << "Cloud before filtering: " << std::endl;
std::cout << *cloud << std::endl;cloud_filtered = my_pmf(cloud);// !!!Run progressive morphological filter
std::cout << "\nGround cloud after filtering: " << std::endl;
std::cout << *cloud_filtered << std::endl;
pcl::PCDWriter writer;
writer.write<pcl::PointXYZ> ("ground.pcd", *cloud_filtered, false);// Extract non-ground returns
pcl::ExtractIndices<pcl::PointXYZ> extract; //extracts a set of indices from a point cloud
extract.setInputCloud (cloud);
extract.setIndices (ground);
//extract.filter (*cloud_filtered);
extract.setNegative (true);
extract.filter (*cloud_filtered);std::cout << "Object cloud after filtering: " << std::endl;
std::cout << *cloud_filtered << std::endl;
writer.write<pcl::PointXYZ> ("object.pcd", *cloud_filtered, false);
// 点云可视化
//pcl::visualization::CloudViewer viewer("Filtered");
//viewer.showCloud(cloud_filtered);
//viewer.runOnVisualizationThreadOnce(viewerOneOff);
//while(!viewer.wasStopped());return (0);
}
处理得到的结果如图所示:
左边为滤除地面后的地图,右边为分割出来的地面。
参数调整后(关于参数调整的细节,后续做具体研究)的效果:
左边为滤除地面后的地图,右边为分割出来的地面。可以从结果看出,可以滤除较大部分的地面点。但是在实际运行中发现,此算法运行速度很慢,不能达到实时的要求,但是可以作为建图结束的处理步骤,实时性的要求没有这么高。
2.利用Progressive Morphological Filter在雷达原始数据分割地面(失败)
由于以上的结果还不错,所以我就想在原始雷达的数据上做地面分割,看其效果是否比RANCAS要好。只要将上篇博客的ROS程序核心算法换成Progressive Morphological Filter即可。
#include <iostream>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <visualization_msgs/Marker.h>#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/progressive_morphological_filter.h>
#include <pcl/filters/morphological_filter.h>ros::Publisher pcl_pub;
/* //基于RANSAC
void laserCLoudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
{
// 将点云格式为sensor_msgs/PointCloud2 格式转为 pcl/PointCloud
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudIn(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg (*laserCloudMsg,*cloudIn);
pcl::SACSegmentation<pcl::PointXYZ> seg; // 创建一个分割方法
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
// pcl::ModelCoefficients coefficients; //申明模型的参数
// pcl::PointIndices inliers; //申明存储模型的内点的索引
seg.setOptimizeCoefficients (true); // 这一句可以选择最优化参数的因子
seg.setModelType (pcl::SACMODEL_PLANE); //平面模型
seg.setMethodType (pcl::SAC_RANSAC); //分割平面模型所使用的分割方法
seg.setDistanceThreshold (0.2); //设置最小的阀值距离
seg.setInputCloud (cloudIn); //设置输入的点云
seg.segment (*inliers,*coefficients);
//ROS_INFO("request: A=%ld, B=%ld C=%ld", (int)req.A, (int)req.B, (int)req.C);// 把提取出来的外点 -> ros发布出去
pcl::ExtractIndices<pcl::PointXYZ> extract; //ExtractIndices滤波器,基于某一分割算法提取点云中的一个子集
extract.setInputCloud (cloudIn);
extract.setIndices (inliers); //设置分割后的内点为需要提取的点集
extract.setNegative (false); //设置提取内点而非外点 或者相反
extract.filter (*cloud_filtered);
//再rviz上显示所以要转换回PointCloud2
sensor_msgs::PointCloud2 cloud_filteredMsg;
pcl::toROSMsg(*cloud_filtered,cloud_filteredMsg);
cloud_filteredMsg.header.stamp=laserCloudMsg->header.stamp;
cloud_filteredMsg.header.frame_id="/velodyne";
pcl_pub.publish (cloud_filteredMsg);
}
*///基于Progressive Morphological Filter
#define max_window_size 0.05 //
#define slope 0.7f
#define max_distance 0.9f
#define initial_distance 0.5f
#define cell_size 0.01f
#define base 2.0f
#define exponential truepcl::PointIndicesPtr ground (new pcl::PointIndices); //地面点索引
pcl::PointCloud<pcl::PointXYZ>::Ptr my_pmf (pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in)
{
// Compute the series of window sizes and height thresholds
std::vector<float> height_thresholds;
std::vector<float> window_sizes;
std::vector<int> ground_indices;
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);
cout << "window_size " << window_size << endl;
// 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;
cout << "height_threshold " << height_threshold << endl;// 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
for (int i=0;i<cloud_in->points.size();i++)
{
ground_indices.push_back(i);
}// Progressively filter ground returns using morphological open window_sizes.size 为迭代次数
for (size_t i = 0; i < window_sizes.size (); ++i)
{
cout<< "\nIteration " << i << " height threshold = " << height_thresholds[i]
<< " window size = " <<window_sizes[i] << endl;// Limit filtering to those points currently considered ground returns
typename pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud<pcl::PointXYZ> (*cloud_in, ground_indices, *cloud); //将cloud_in的索引为groung_indices的点云复制到cloud// Create new cloud to hold the filtered results. Apply the morphological
// opening operation at the current window size.
typename pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
pcl::applyMorphologicalOperator<pcl::PointXYZ> (cloud, window_sizes[i], pcl::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;
//cout << "ground.size() = " << ground.size() << endl;
for (size_t p_idx = 0; p_idx < ground_indices.size (); ++p_idx)
{
float diff = cloud->points[p_idx].z - cloud_f->points[p_idx].z;
//cout << "diff " << diff << endl;
if (diff < height_thresholds[i])
pt_indices.push_back (ground_indices[p_idx]);
}// Ground is now limited to pt_indices
ground_indices.swap (pt_indices);
cout << "ground now has " << ground_indices.size () << " points" << endl;
}
typename pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);
// Extract cloud_in with ground indices
pcl::copyPointCloud<pcl::PointXYZ> (*cloud_in, ground_indices, *cloud_out);
ground->indices=ground_indices; //索引赋值
return cloud_out;}
void laserCLoudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
{
// 将点云格式为sensor_msgs/PointCloud2 格式转为 pcl/PointCloud
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudIn(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg (*laserCloudMsg,*cloudIn);
cloud_filtered = my_pmf(cloudIn);// !!!Run progressive morphological filter
// 把提取出来的外点 -> ros发布出去
//pcl::ExtractIndices<pcl::PointXYZ> extract; //ExtractIndices滤波器,基于某一分割算法提取点云中的一个子集
//extract.setInputCloud (cloudIn);
//extract.setIndices (ground); //设置分割后的内点为需要提取的点集
//extract.setNegative (false); //设置提取内点而非外点 或者相反
//extract.filter (*cloud_filtered);
//再rviz上显示所以要转换回PointCloud2
sensor_msgs::PointCloud2 cloud_filteredMsg;
pcl::toROSMsg(*cloud_filtered,cloud_filteredMsg);
cloud_filteredMsg.header.stamp=laserCloudMsg->header.stamp;
cloud_filteredMsg.header.frame_id="/velodyne";
pcl_pub.publish (cloud_filteredMsg);
}int main (int argc, char **argv)
{
ros::init (argc, argv, "ros_vlp16_scan_ground_segmentation");
ros::NodeHandle nh;
ros::Subscriber subLaserCloud=nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_points",2,laserCLoudHandler);
pcl_pub = nh.advertise<sensor_msgs::PointCloud2> ("/velodyne_points_segmentation", 2);
ros::spin();
return 0;
}
对RANSAC表现较好的室外场景分割结果如下:
左边是滤除地面后的雷达数据,右边是分割出来的地面(完全错误的)。这个结果基本是完全错误的,当然我是直接用检测建图结果的参数来分割雷达的数据,这样的结果是否是因为参数设置不当造成的,还有待进一步研究。