RANSAC方法滤除地面

项目git链接:
https://gitee.com/long_xiao_wyh/ground_filter_pcl_RANSAC

环境:
Ubuntu16.04
pcl

运行:

mkdir build
cd build
cmake ..
make
cd ..
./pcl_test

通过命令行查看pcd文件需要用到pcl_viewer,pcl_viewer是在pcl-tools中,先安装pcl-tools:

sudo apt-get install pcl-tools

通过pcl_viewer查看原pcd文件和滤除地面后的pcd文件:

pcl_viewer pointcloud_files/000000.pcd
pcl_viewer pointcloud_files/000000_filtered.pcd

RANSAC(随机采样一致性算法)

1：随机选择确定模型参数所需的最小点数。
2：求解模型的参数。
3：确定所有点集中有多少点符合预定的公差[公式]。
4：如果在集合中的点数总数中分数的百分比超过预定义的阈值[公式]，则使用所有已识别的点数重新估计模型参数并终止。
5：否则，重复步骤1到4（最多N次）

伪代码:

Given:
    data - a set of observed data points(这里指点云数据)
    model - a model that can be fitted to data points(这里指平面模型)
    n - the minimum number of data values required to fit the model
    (n - 满足模型的最少数据个数)
    k - the maximum number of iterations allowed in the algorithm(最多迭代次数)
    t - a threshold value for determining when a data point fits a model
    (t - 点是否适用于模型的阈值，这里指的是点到平面的距离)
    d - the number of close data values required to assert that a model fits well to data
(   (d - 判定模型是否适用于数据集的数据数目)
Return:
    bestfit - model parameters which best fit the data (or nil if no good model is found)

iterations = 0
bestfit = nil
besterr = something really large
while iterations < k {
    maybeinliers = n randomly selected values from data
    maybemodel = model parameters fitted to maybeinliers
    alsoinliers = empty set
    for every point in data not in maybeinliers {
        if point fits maybemodel with an error smaller than t
             add point to alsoinliers
    }
    if the number of elements in alsoinliers is > d {
        % this implies that we may have found a good model
        % now test how good it is
        bettermodel = model parameters fitted to all points in maybeinliers and alsoinliers
        thiserr = a measure of how well model fits these points
        if thiserr < besterr {
            bestfit = bettermodel
            besterr = thiserr
        }
    }
    increment iterations
}
return bestfit

pcl_test.cpp代码:

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/segmentation/sac_segmentation.h>

using namespace std;

void detectObjectsOnCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_filtered)
{
    if (cloud->size() > 0)
    {
    	//创建分割时所需要的模型系数对象，coefficients及存储内点的点索引集合对象inliers
        pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
        pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
        // 创建分割对象
        pcl::SACSegmentation<pcl::PointXYZ> seg;
        // 可选择配置，设置模型系数需要优化
        seg.setOptimizeCoefficients(true);
        // 必要的配置，设置分割的模型类型，所用的随机参数估计方法，距离阀值，输入点云
        seg.setModelType(pcl::SACMODEL_PLANE);//设置模型类型
        seg.setMethodType(pcl::SAC_RANSAC);//设置随机采样一致性方法类型
        // you can modify the parameter below
  		seg.setMaxIterations(10000);//表示点到估计模型的距离最大值，
        seg.setDistanceThreshold(0.15);//设定距离阀值，距离阀值决定了点被认为是局内点是必须满足的条件
        seg.setInputCloud(cloud);
        //引发分割实现，存储分割结果到点几何inliers及存储平面模型的系数coefficients
        seg.segment(*inliers, *coefficients);
        if (inliers->indices.size() == 0)
        {
            cout<<"error! Could not found any inliers!"<<endl;
        }
        // extract ground
        // 从点云中抽取分割的处在平面上的点集
        pcl::ExtractIndices<pcl::PointXYZ> extractor;//点提取对象
        extractor.setInputCloud(cloud);
        extractor.setIndices(inliers);
        extractor.setNegative(true);
        extractor.filter(*cloud_filtered);
        // vise-versa, remove the ground not just extract the ground
        // just setNegative to be true
        cout << "filter done."<<endl;
    }
    else
    {
        cout<<"no data!"<<endl;
    }
}

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>);

    // Fill in the cloud data
    pcl::PCDReader reader;//PCD文件读取对象
    // Replace the path below with the path where you saved your file
    reader.read<pcl::PointXYZ> ("pointcloud_files/000000.pcd", *cloud);
    detectObjectsOnCloud(cloud, cloud_filtered);
    pcl::PCDWriter writer;
    writer.write<pcl::PointXYZ> ("pointcloud_files/000000_filtered.pcd", *cloud_filtered, false);

    return (0);
}

由于地面不是倾角为零的平面，可以看到还是有些地面点的，所以设置距离阈值参数的时候需要试试，这里设为0.2比较好(测试下来效果不错)，源码设为0.15就不改了.

seg.setDistanceThreshold(0.2);

下面这个是修改成0.2之后的效果,明显比0.15效果好很多,能够滤除的吧比较彻底

参考:
添加链接描述
添加链接描述
添加链接描述