3D点云地图地面去除（1）：基于随机一致性采样（RANSAC），未做RANSAC后续处理

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

       点云地图中包括了大量的地面点，不仅让整个地图显得很杂乱，而且对后续障碍物点云的分类、识别和跟踪带来麻烦，所以需要首先去除。首先我有两个思路：

• 在雷达的原始数据上把地面滤除，这样在后续的建图就不会有地面信息。
• 在建图结束后检测地面，然后将地面滤除。

        接下来就讨论两个思路的实现。地面的滤除可以通过点云分割来实现，点云分割的目的提取点云中的不同物体，从而实现分而治之，突出重点，单独处理的目的。而在现实点云数据中，往往对场景中的物体有一定先验知识。比如：桌面墙面多半是大平面，桌上的罐子应该是圆柱体，长方体的盒子可能是牛奶盒......对于复杂场景中的物体，其几何外形可以归结于简单的几何形状。这为分割带来了巨大的便利，因为简单几何形状是可以用方程来描述的，或者说，可以用有限的参数来描述复杂的物体。而方程则代表的物体的拓扑抽象。点云分割有以下两种方法：

• 聚类分割算法
• 基于随机一致性采样

       我首先采用RANSAC算法分割地面

1.基于随机一致性采样对雷达采集的原始数据进行地面分割

雷达数据由16线velodyne采集，，程序基于ros系统，代码如下：

#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>

ros::Publisher pcl_pub;

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 (true); //设置提取内点而非外点 或者相反

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="/camera";

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",10,laserCLoudHandler);

pcl_pub = nh.advertise<sensor_msgs::PointCloud2> ("/velodyne_points_segmentation", 10);

 ros::spin();

return 0;

}

室外运行效果：

白色的是我们滤除的地面，可以看出本算法可以滤除部分地面，利用过滤后的数据进行建图的效果如下：

左边为雷达数据没有滤除地面的建图效果，右边为滤出地面后的建图效果。可以看出来，虽然在雷达原始数据上做了地面滤除处理，但是最终的建图结果让仍然会包含大量的地面点。经过分析是由于地面滤除的不够彻底，由于建图是一个积累的过程，即使残留了很少的地面特征点，最终建图的结果仍然会包含大量的地面点，所以，除非能够保证在原始的雷达数据处滤除所有的地面特征点，否则在最后的建图仍然会包含大量的地面点。

室内运行效果：

在室内分割地面的效果较差，由于我们采用的平面参数作为地面的判别依据，所以会大量的把墙面当做地面。地面滤除前后建图效果也差别不大：

左边为未滤除地面的建图效果。

2.基于随机一致性采样对建图结果进行地面分割

#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.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>
 
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("/home/zqt/map/zhinengzhizao_blam.pcd",*cloud);
    std::cerr << "Point cloud data: " << cloud->points.size () << " points" << std::endl;
    
    pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
    // Create the segmentation object
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    // Optional
    seg.setOptimizeCoefficients (true);
    // Mandatory
    seg.setModelType (pcl::SACMODEL_PLANE);
    seg.setMethodType (pcl::SAC_RANSAC);
    // 距离阈值 单位m
    seg.setDistanceThreshold (0.3);
    seg.setInputCloud (cloud);
    seg.segment (*inliers, *coefficients);
    if (inliers->indices.size () == 0)
    {
        PCL_ERROR ("Could not estimate a planar model for the given dataset.");
        return (-1);
    }
    // 提取地面
    pcl::ExtractIndices<pcl::PointXYZ> extract;
    extract.setInputCloud (cloud);
    extract.setIndices (inliers);
    extract.filter (*cloud_filtered);
    std::cerr << "Ground cloud after filtering: " << std::endl;
    std::cerr << *cloud_filtered << std::endl;
 
    pcl::PCDWriter writer;
    writer.write<pcl::PointXYZ> ("ground.pcd", *cloud_filtered, false);
    // 提取除地面外的物体
    extract.setNegative (true);
    extract.filter (*cloud_filtered);
    std::cerr << "Object cloud after filtering: " << std::endl;
    std::cerr << *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);
}
 

分割结果：

从图片可以看出，地面分割的效果较差。其中很不解的是，在右侧的被分割出来的地面中竟然出现了比地面还高的物体，但是其周围的地面并没有被分割出来？？其中的原因要后续去了解算法的原理进一步解释。