【激光雷达点云障碍物检测】processPointClouds.h、processPointClouds.cpp 重点部分

processPointClouds.h

// PCL lib Functions for processing point clouds 

#ifndef PROCESSPOINTCLOUDS_H_
#define PROCESSPOINTCLOUDS_H_
#include <pcl/io/pcd_io.h>
#include <pcl/common/common.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/crop_box.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/common/transforms.h>
#include <unordered_set>
#include <iostream>
#include <string>
#include <vector>
#include <ctime>
#include <chrono>
#include "box.h"
#include "ransac3d.cpp"
#include "cluster3d.cpp"

//命名空间的使用：避免了名字相同函数和变量的冲突
namespace lidar_obstacle_detection {

	// shorthand for point cloud pointer
	template<typename PointT>
	using PtCdtr = typename pcl::PointCloud<PointT>::Ptr;

	template<typename PointT>
	class ProcessPointClouds {

	public:
		//constructor
		ProcessPointClouds();
		//deconstructor
		~ProcessPointClouds();

		void numPoints(PtCdtr<PointT> cloud);

		//1、点云滤波		ok
		PtCdtr<PointT> FilterCloud(PtCdtr<PointT> cloud, float filterRes, Eigen::Vector4f minPoint, Eigen::Vector4f maxPoint);
		
		//——————————————————————————对点云库中的模型分割，将点云保存为两个文件  主程序中依然好像没有用到		ok
		std::pair<PtCdtr<PointT>, PtCdtr<PointT>> SeparateClouds(pcl::PointIndices::Ptr inliers, PtCdtr<PointT> cloud);

		//2、地面与障碍物的分割 两块点云分别保存在in_plane（平面外的点）, out_plane（平面内的点）中	ok
		std::pair< PtCdtr<PointT>, PtCdtr<PointT> > RansacSegmentPlane(PtCdtr<PointT> cloud, int maxIterations, float distanceTol);

		// ——————————————————————————此处使用了pcl点云库中的模型分割 程序中貌似没有用到			ok
		std::pair<PtCdtr<PointT>, PtCdtr<PointT>> SegmentPlane(PtCdtr<PointT> cloud, int maxIterations, float distanceTol);

		//3、重写欧式聚类算法聚类障碍物点云		ok
		std::vector<PtCdtr<PointT>> EuclideanClustering(PtCdtr<PointT> cloud, float clusterTolerance, int minSize, int maxSize);

		//——————————————————————————此处使用了点云库中的欧氏距离分割模型  程序中没有用到			ok
		std::vector<PtCdtr<PointT>> Clustering(PtCdtr<PointT> cloud, float clusterTolerance, int minSize, int maxSize);

		Box BoundingBox(PtCdtr<PointT> cluster);

		void savePcd(PtCdtr<PointT> cloud, std::string file);

		PtCdtr<PointT> loadPcd(std::string file);

		std::vector<boost::filesystem::path> streamPcd(std::string dataPath);

	};
}
#endif /* PROCESSPOINTCLOUDS_H_ */

processPointClouds.cpp 

// PCL lib Functions for processing point clouds 

#include "processPointClouds.h"

using namespace lidar_obstacle_detection;

//constructor: 构造函数
template<typename PointT>
ProcessPointClouds<PointT>::ProcessPointClouds() {}

//de-constructor: 析构函数
template<typename PointT>
ProcessPointClouds<PointT>::~ProcessPointClouds() {}

template<typename PointT>
void ProcessPointClouds<PointT>::numPoints(PtCdtr<PointT> cloud) { std::cout << cloud->points.size() << std::endl; }

//2、随机采样一致性分割   地面与障碍物的分割 两块点云分别保存在in_plane（平面外的点）, out_plane（平面内的点）中	ok
template<typename PointT>
std::pair<PtCdtr<PointT>, PtCdtr<PointT>>  //pair可以将两种不同类型的值合为一个值
ProcessPointClouds<PointT>::RansacSegmentPlane(PtCdtr<PointT> cloud, int maxIterations, float distanceTol) 
{
	// Count time
	auto startTime = std::chrono::steady_clock::now();
	srand(time(NULL));

	int num_points = cloud->points.size();
	Ransac<PointT> RansacSeg(maxIterations, distanceTol, num_points);  //使用Ransac<PointT>类创建了一个对象RansacSeg

	// Get inliers from RANSAC implementation
	std::unordered_set<int> inliersResult = RansacSeg.Ransac3d(cloud); //使用RansacSeg对象实现函数

	auto endTime = std::chrono::steady_clock::now();
	auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);
	std::cout << "plane ransac-segment took " << elapsedTime.count() << " milliseconds" << std::endl;

	//此处表示不清  这里out_plane表示平面中的点  in_plane表示平面外中的点
	PtCdtr<PointT> out_plane(new pcl::PointCloud<PointT>());
	PtCdtr<PointT> in_plane(new pcl::PointCloud<PointT>());
	for (int i = 0; i < num_points; i++) 
	{
		PointT pt = cloud->points[i];
		if (inliersResult.count(i)) 
		{
			out_plane->points.push_back(pt);
		}
		else 
		{
			in_plane->points.push_back(pt);
		}
	}
	return std::pair<PtCdtr<PointT>, PtCdtr<PointT>>(in_plane, out_plane);
}

//1、点云滤波		ok
template<typename PointT>
PtCdtr<PointT> ProcessPointClouds<PointT>::FilterCloud(PtCdtr<PointT> cloud, float filterRes, Eigen::Vector4f minPoint,Eigen::Vector4f maxPoint) 
{
	// Time segmentation process
	auto startTime = std::chrono::steady_clock::now();

	//使用体素滤波下采样
	pcl::VoxelGrid<PointT> vg;   //滤波对象
	PtCdtr<PointT> cloudFiltered(new pcl::PointCloud<PointT>); //创建保存体素滤波后的点对象
	vg.setInputCloud(cloud);
	vg.setLeafSize(filterRes, filterRes, filterRes);
	vg.filter(*cloudFiltered);

	//过滤掉在用户给定立方体内的点云数据
	//理解：将自身车辆作为坐标轴的中心点，然后在身边自身为中心 ，圈出范围，成为每一次运动时候的感兴趣区域，也就是只关心区域内点的聚类等后续操作
	PtCdtr<PointT> cloudRegion(new pcl::PointCloud<PointT>);
	pcl::CropBox<PointT> region(true);
	region.setMin(minPoint);
	region.setMax(maxPoint);
	region.setInputCloud(cloudFiltered);
	region.filter(*cloudRegion);

	//提取车身周围范围内的所有的点，并将提取到的所有点保存在indices中
	std::vector<int> indices;
	pcl::CropBox<PointT> roof(true);
	roof.setMin(Eigen::Vector4f(-1.5, -1.7, -1, 1));
	roof.setMax(Eigen::Vector4f(2.6, 1.7, -0.4, 1));
	roof.setInputCloud(cloudRegion);
	roof.filter(indices);

	pcl::PointIndices::Ptr inliers{ new pcl::PointIndices }; //创建一个内点对象，将提取到车身周围点，放到内点对象中
	for (int point : indices) 
	{
		inliers->indices.push_back(point);
	}
	pcl::ExtractIndices<PointT> extract;
	extract.setInputCloud(cloudRegion);
	extract.setIndices(inliers);
	extract.setNegative(true);  //false 提取内点也就是提取车身周围的几个点，， true提取出了车身周围的点
	extract.filter(*cloudRegion);

	auto endTime = std::chrono::steady_clock::now();
	auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);
	std::cout << "filtering took " << elapsedTime.count() << " milliseconds" << std::endl;

	return cloudRegion;
}

//——————————————————————————对点云库中的模型分割，将点云保存为两个文件  主程序中依然好像没有用到  ok
template<typename PointT>
std::pair<PtCdtr<PointT>, PtCdtr<PointT>>
ProcessPointClouds<PointT>::SeparateClouds(pcl::PointIndices::Ptr inliers, PtCdtr<PointT> cloud) 
{
	//创建了两个点云块，一个存放障碍物，一个存放地面 
	PtCdtr<PointT> obstCloud(new pcl::PointCloud<PointT>());
	PtCdtr<PointT> planeCloud(new pcl::PointCloud<PointT>());
	for (int index : inliers->indices) 
	{
		planeCloud->points.push_back(cloud->points[index]);
	}
	// create extraction object
	pcl::ExtractIndices<PointT> extract;
	extract.setInputCloud(cloud);
	extract.setIndices(inliers);
	extract.setNegative(true);
	extract.filter(*obstCloud);
	std::pair<PtCdtr<PointT>, PtCdtr<PointT>> segResult(obstCloud,
		planeCloud);
	//    std::pair<PtCdtr<PointT>, PtCdtr<PointT>> segResult(cloud, cloud);
	return segResult;
}

//——————————————————————————此处使用了pcl点云库中的模型分割 程序中貌似没有用到   ok
template<typename PointT>
std::pair<PtCdtr<PointT>, PtCdtr<PointT>>
ProcessPointClouds<PointT>::SegmentPlane(PtCdtr<PointT> cloud, int maxIterations, float distanceThreshold) {
	// Time segmentation process
	auto startTime = std::chrono::steady_clock::now();
	//	pcl::PointIndices::Ptr inliers; // Build on the stack
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices); // Build on the heap
														   // TODO:: Fill in this function to find inliers for the cloud.
	pcl::ModelCoefficients::Ptr coefficient(new pcl::ModelCoefficients);
	pcl::SACSegmentation<PointT> seg;
	seg.setOptimizeCoefficients(true);
	seg.setModelType(pcl::SACMODEL_PLANE);
	seg.setMethodType(pcl::SAC_RANSAC);
	seg.setMaxIterations(maxIterations);
	seg.setDistanceThreshold(distanceThreshold);

	// Segment the largest planar component from the remaining cloud
	seg.setInputCloud(cloud);
	seg.segment(*inliers, *coefficient);

	if (inliers->indices.size() == 0) {
		std::cerr << "Could not estimate a planar model for the given dataset" << std::endl;
	}

	auto endTime = std::chrono::steady_clock::now();
	auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);
	std::cout << "plane segmentation took " << elapsedTime.count() << " milliseconds" << std::endl;
	std::pair<PtCdtr<PointT>, PtCdtr<PointT>> segResult = SeparateClouds(inliers, cloud);
	return segResult;
}

//——————————————————————————此处使用了点云库中的欧氏距离分割模型  程序中没有用到   ok
template<typename PointT>
std::vector<PtCdtr<PointT>>
ProcessPointClouds<PointT>::Clustering(PtCdtr<PointT> cloud, float clusterTolerance, int minSize, int maxSize) {

	// Time clustering process
	auto startTime = std::chrono::steady_clock::now();

	std::vector<PtCdtr<PointT>> clusters;  //保存分割后的所有类 每一类为一个点云
	// 欧式聚类对检测到的障碍物进行分组

	typename pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>); //对cloud点云创建kdtree
	tree->setInputCloud(cloud);
	std::vector<pcl::PointIndices> clusterIndices; // 创建索引类型对象
	pcl::EuclideanClusterExtraction<PointT> ec; // 欧式聚类对象
	ec.setClusterTolerance(clusterTolerance);  //设置近邻搜索半径
	ec.setMinClusterSize(minSize); //设置一个类需要的最小的点数
	ec.setMaxClusterSize(maxSize); //设置一个类需要的最大的点数
	ec.setSearchMethod(tree); //设置搜索方法
	ec.setInputCloud(cloud); // feed point cloud
	ec.extract(clusterIndices); // 得到所有类别的索引 clusterIndices  

	// 将得到的所有类的索引分别在点云中找到，即每一个索引形成了一个类
	for (pcl::PointIndices getIndices : clusterIndices) 
	{
		PtCdtr<PointT> cloudCluster(new pcl::PointCloud<PointT>);
		// For each point indice in each cluster
		for (int index : getIndices.indices) 
		{
			cloudCluster->points.push_back(cloud->points[index]);
		}
		cloudCluster->width = cloudCluster->points.size();
		cloudCluster->height = 1;
		cloudCluster->is_dense = true;
		clusters.push_back(cloudCluster);
	}

	auto endTime = std::chrono::steady_clock::now();
	auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);
	std::cout << "clustering took " << elapsedTime.count() << " milliseconds and found " << clusters.size()
		<< " clusters" << std::endl;
	return clusters;
}

//3、欧式聚类函数：		对分割掉地面点云的障碍物点云进行欧式聚类 传入 segmentCloud.first, "obstCloud"  0.5 10 140 
template<typename PointT>
std::vector<PtCdtr<PointT>>
ProcessPointClouds<PointT>::EuclideanClustering(PtCdtr<PointT> cloud, float clusterTolerance, int minSize,int maxSize) 
{
	// Time clustering process
	auto startTime = std::chrono::steady_clock::now();

	// 创建聚类对象  实例化ClusterPts类 创建对象clusterPoints
	ClusterPts<PointT> clusterPoints(cloud->points.size(), clusterTolerance, minSize, maxSize);

	//对象调用欧式聚类函数  clusters 保存了类数的所有点云
	std::vector<PtCdtr<PointT>> clusters = clusterPoints.EuclidCluster(cloud);
	auto endTime = std::chrono::steady_clock::now();
	auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);
	std::cout << "KDTree clustering took " << elapsedTime.count() << " milliseconds and found " << clusters.size()<< " clusters" << std::endl;
	return clusters;
}


//4、框出每个类
template<typename PointT>
Box ProcessPointClouds<PointT>::BoundingBox(PtCdtr<PointT> cluster) 
{

	// Find bounding box for one of the clusters
	PointT minPoint, maxPoint;
	pcl::getMinMax3D(*cluster, minPoint, maxPoint); //想得到它x,y,z三个轴上的最大值和最小值

	Box box;
	box.x_min = minPoint.x;
	box.y_min = minPoint.y;
	box.z_min = minPoint.z;
	box.x_max = maxPoint.x;
	box.y_max = maxPoint.y;
	box.z_max = maxPoint.z;
	return box;
}

template<typename PointT>
void ProcessPointClouds<PointT>::savePcd(PtCdtr<PointT> cloud, std::string file) {
	pcl::io::savePCDFileASCII(file, *cloud);
	std::cerr << "Saved " << cloud->points.size() << " data points to " + file << std::endl;
}

template<typename PointT>//加载点云数据
PtCdtr<PointT> ProcessPointClouds<PointT>::loadPcd(std::string file) {

	PtCdtr<PointT> cloud(new pcl::PointCloud<PointT>);
	if (pcl::io::loadPCDFile<PointT>(file, *cloud) == -1) 
	{ //* load the file
		PCL_ERROR("Couldn't read file \n");
	}
	std::cerr << "Loaded " << cloud->points.size() << " data points from " + file << std::endl;
	return cloud;
}

template<typename PointT>
std::vector<boost::filesystem::path> ProcessPointClouds<PointT>::streamPcd(std::string dataPath) 
{
	std::vector<boost::filesystem::path> paths(boost::filesystem::directory_iterator{dataPath},boost::filesystem::directory_iterator{});
	// sort files in accending order so playback is chronological
	sort(paths.begin(), paths.end());
	return paths;
}