使用CSF对kitti的点云数据过滤出地面点云，结合PCL使用，C++实现

2021-09-19 09:10:56

文章目录

前言
一、代码
二、相关配置
- 1.配置CSF工程
- 2.配置自己的工程
分割效果

前言

环境：win10+vs2019

使用CSF进行地面点云滤波，使用了PCL库读取显示点云，CSF算法使用github开源代码，自己编译生成CSF.lib。
https://github.com/jianboqi/CSF
测试所用pcd点云文件审核后发布

一、代码

#include <string>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>//PCL对各种格式的点的支持头文件
#include <pcl/io/pcd_io.h>//PCL的PCD格式文件的输入输出头文件
#include <pcl/visualization/cloud_viewer.h>//点云查看窗口头文件
#include <pcl/filters/filter.h>	//滤波相关头文件
#include <pcl/filters/passthrough.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include "CSF.h"

using namespace std;

void clothSimulationFilter(const vector< csf::Point >& pc,vector<int> &groundIndexes,vector<int> & offGroundIndexes)
{
	//step 1 read point cloud
	CSF csf;
	csf.setPointCloud(pc);// or csf.readPointsFromFile(pointClouds_filepath); 
	//pc can be vector< csf::Point > or PointCloud defined in point_cloud.h

	//step 2 parameter settings
	//Among these paramters:  
	//time_step  interations class_threshold can remain as defualt in most cases.
	csf.params.bSloopSmooth = false;
	csf.params.cloth_resolution = 0.5;
	csf.params.rigidness = 3;

	csf.params.time_step = 0.65;
	csf.params.class_threshold = 0.5;
	csf.params.interations = 500;

	//step 3 do filtering
	//result stores the index of ground points or non-ground points in the original point cloud
	
	csf.do_filtering(groundIndexes, offGroundIndexes);
	//csf.do_filtering(groundIndexes, offGroundIndexes,true); 
	//if the third parameter is set as true, then a file named "cloth_nodes.txt" will be created, 
	//it respresents the cloth nodes.By default, it is set as false

}


void addPointCloud(const vector<int>& index_vec, const pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered)
{
	auto& points = cloud_filtered->points;
	const auto& pointclouds = cloud->points;

	for_each(index_vec.begin(), index_vec.end(), [&](const auto& index) {
		pcl::PointXYZI pc;
		pc.x = pointclouds[index].x;
		pc.y = pointclouds[index].y;
		pc.z = pointclouds[index].z;
		pc.intensity = pointclouds[index].intensity;

		points.push_back(pc);
	});

	cloud_filtered->height = 1;
	cloud_filtered->width = cloud_filtered->points.size();
}


int main()
{
	string pcd_path = "E:\\kitti\\data_object_velodyne\\testing\\pcd\\000060.pcd";

	// Generate pointcloud data，新建指针cloud存放点云
	pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);	

	if (pcl::io::loadPCDFile<pcl::PointXYZI>(pcd_path, *cloud) == -1)//打开点云文件。
	{
		PCL_ERROR("Couldn't read that pcd file\n");
		return(-1);
	}

	vector<csf::Point> pc;
	const auto& pointclouds = cloud->points;
	pc.resize(cloud->size());
	transform(pointclouds.begin(), pointclouds.end(), pc.begin(), [&](const auto& p)->csf::Point {
		csf::Point pp;
		pp.x = p.x;
		pp.y = p.y;
		pp.z = p.z;
		return pp;
		});

	std::vector<int> groundIndexes, offGroundIndexes;
	clothSimulationFilter(pc, groundIndexes, offGroundIndexes);

	pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZI>);
	

	addPointCloud(groundIndexes, cloud,cloud_filtered);
	pcl::PCDWriter writer;
	writer.write<pcl::PointXYZI>("groundPointCloud.pcd", *cloud_filtered, false);

	cloud_filtered->points.clear();
	addPointCloud(offGroundIndexes, cloud,cloud_filtered);
	writer.write<pcl::PointXYZI>("nonGroundPointCloud.pcd", *cloud_filtered, false);

	//pcl::visualization::CloudViewer viewer("this is a point cloud viewer haha!!");
	//viewer.showCloud(cloud_filtered);
	//while (!viewer.wasStopped())
	//{
	//}

	return 0;
}

二、相关配置

1.配置CSF工程

在github上下载CSF的源码，在windows下使用cmake-gui进行配置（相关操作略，可自行百度），配置好后，会出现如图所示的build文件夹。

在build文件夹下打开CSF.sln

对里面的工程进行生成，生成成功后的CSF.lib保存在如下图所示的位置。
（生成CSF工程的过程中可能会报命令的错误，自行百度可解决。）