3D点云地图地面去除（2）：Progressive Morphological Filter

版权声明：本文为博主原创文章，未经博主允许不得转载。 https://blog.csdn.net/sru_alo/article/details/84678910

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 true

pcl::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 true

pcl::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表现较好的室外场景分割结果如下：

左边是滤除地面后的雷达数据，右边是分割出来的地面（完全错误的）。这个结果基本是完全错误的，当然我是直接用检测建图结果的参数来分割雷达的数据，这样的结果是否是因为参数设置不当造成的，还有待进一步研究。