14、 在lego_loam提取出的PCD点云文件中提取地面

一、背景:

在10.lego-loam的编译、安装、运行以及结果的保存的基础上进行地面点和非地面点的提取.
使用的PCD数据是10.lego建图保存的,使用的方法是PCL的平面分割,用RANSAC分割法,更多注释见如下代码.

二、代码:

#include <iostream>
#include <pcl/io/pcd_io.h>
//#include <pcl/io/ply_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/point_types.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <math.h>
#include <chrono>
#include<iostream>

void RemoveCloudFloor(const pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_ptr,
                      pcl::PointCloud<pcl::PointXYZI>::Ptr nofloor_cloudptr,
                      pcl::PointCloud<pcl::PointXYZI>::Ptr onlyfloor_cloudptr,
                      float disThreshold, float EpsAngle);


int main(int argc, char** argv)
{
    //note run time
    auto startTime    = std::chrono::steady_clock::now();
	

	// Fill in the cloud data
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
	pcl::PCDReader reader;
	reader.read("1573523885.388732910.pcd", *cloud);
	std::cerr << "Cloud Points number: " << cloud->points.size() << std::endl;

    //defeine nofloor_cloudptr && onlyfloor_cloudptr
	pcl::PointCloud<pcl::PointXYZI>::Ptr nofloor_cloudptr(new pcl::PointCloud<pcl::PointXYZI>);
	pcl::PointCloud<pcl::PointXYZI>::Ptr onlyfloor_cloudptr(new pcl::PointCloud<pcl::PointXYZI>);
 
    //RemoveCloudFloor
    RemoveCloudFloor(cloud, nofloor_cloudptr, onlyfloor_cloudptr, 1.0, 100.0f * (M_PI / 180.0f));
	

    //write to two pcd file
        pcl::PCDWriter writer;
	    writer.write("nofloor_cloud.pcd", *nofloor_cloudptr, false);
	    writer.write("onlyfloor_cloud.pcd", *onlyfloor_cloudptr, false);   
    
    //cout running time
    auto endTime      = std::chrono::steady_clock::now();
    auto ellapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);
    std::cout << "Ellapse-Time: " << ellapsedTime.count() << " milliseconds." << std::endl;  

	return 0;
}

void RemoveCloudFloor(const pcl::PointCloud<pcl::PointXYZI>::Ptr cloud,
                      pcl::PointCloud<pcl::PointXYZI>::Ptr nofloor_cloudptr,
                      pcl::PointCloud<pcl::PointXYZI>::Ptr onlyfloor_cloudptr,
                      float disThreshold, float EpsAngle)
{
	// For Accelerate This Segmentation, You Can Do Cloud-Preprocess.
	// For Example: Filter Cloud With PassThrough or Conditional Filter.

	pcl::SACSegmentation<pcl::PointXYZI> seg;
    pcl::PointIndices::Ptr              inliers(new pcl::PointIndices);
    pcl::ModelCoefficients::Ptr         coeffs(new pcl::ModelCoefficients);
	// 设置是否对估计的模型参数进行优化,true为进行优化
    seg.setOptimizeCoefficients(true);

    // 设置几何模型类型，这里设置为有条件限制的平面模型
    seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);

    // 设置使用随机采样一致性算法类型，这里使用RANSAC
    seg.setMethodType(pcl::SAC_RANSAC);

    // 设置迭代回归拟合次数，这里需要根据实际情况来进行设置，越大越耗时
    seg.setMaxIterations(2000);

    // 对高度坐标值进行回归拟合，X Y Z分别代码括号里面的值，为1.0即为对那个坐标系进行操作
    seg.setAxis(Eigen::Vector3f(0.0, 0.0, 1.0));
    seg.setEpsAngle(EpsAngle);

	// 设置距离阈值，如果点到模型的距离不超过这个阈值，则认为该点为局内点；否则为局外点
    seg.setDistanceThreshold(disThreshold);
    seg.setOptimizeCoefficients(true);

    seg.setInputCloud(cloud);
	// 分割或许内联点索引
    seg.segment(*inliers, *coeffs);

    if (0 == inliers->indices.size())
    {
        std::cout << "Could Not Estimate A Planar Model For The Given Dataset In Range Space."<<std::endl;
    }

    pcl::ExtractIndices<pcl::PointXYZI> extract;
    extract.setInputCloud(cloud);
    extract.setIndices(inliers);

    //提取非地面点
    extract.setNegative(true);
    extract.filter(*nofloor_cloudptr);
	// 提取地面点
    extract.setNegative(false);
    extract.filter(*onlyfloor_cloudptr);
}

三、运行结果

显示了PCD中的点云和运行花费的时间.
可见,将地面点和非地面点分别放在了不同的PCD文件中.
代码使用方法:
环境:ubuntu16.04+pcl1.9
1、新建groud文件夹,放groundfilter.cpp(上述代码)和CMakeLists.txt在里面,
2、新建build文件夹,在此文件夹下打开终端输入

cmake ..
make 

进行编译.
3、运行:在build下:

./groud

可以得到结果.

将三个文件同时显示:

附录:同时显示的文章:15、同时显示多个点云文件