3D點雲地圖地面去除（2）：Progressive Morphological Filter
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表現較好的室外場景分割結果如下：
左邊是濾除地面後的雷達資料，右邊是分割出來的地面（完全錯誤的）。這個結果基本是完全錯誤的，當然我是直接用檢測建圖結果的引數來分割雷達的資料，這樣的結果是否是因為引數設定不當造成的，還有待進一步研究。