【激光雷达点云障碍物检测】cluster3d.cpp、cluster3d.h

自己理解的注释。

cluster3d.h

#ifndef PLAYBACK_CLUSTER3D_H
#define PLAYBACK_CLUSTER3D_H
#include <pcl/common/common.h>
#include <chrono>
#include <string>
#include "kdtree3d.h"

namespace lidar_obstacle_detection {
	// shorthand for point cloud pointer
	template<typename PointT>
	using PtCdtr = typename pcl::PointCloud<PointT>::Ptr;
	template<typename PointT>
	class ClusterPts 
	{
	private:
		int num_points;
		float distanceTol;
		int minClusterSize;
		int maxClusterSize;
		std::vector<bool> processed;
		std::vector<PtCdtr<PointT>> clusters;
	public:
		//构造函数
		ClusterPts(int nPts, float cTol, int minSize, int maxSize) : num_points(nPts), distanceTol(cTol),minClusterSize(minSize), maxClusterSize(maxSize) 
		{
			processed.assign(num_points, false);
		}
		~ClusterPts();
		void clusterHelper(int ind, PtCdtr<PointT> cloud, std::vector<int> &cluster, KdTree *tree);
		std::vector<PtCdtr<PointT>> EuclidCluster(PtCdtr<PointT> cloud);
	};
}
#endif //PLAYBACK_CLUSTER3D_H

 cluster3d.cpp

#include "cluster3d.h"
using namespace lidar_obstacle_detection;
template<typename PointT>
ClusterPts<PointT>::~ClusterPts() {}
//根据最近邻搜索的阈值，找到了类数，每一类包含了属于该类的点			ok
template<typename PointT>
void ClusterPts<PointT>::clusterHelper(int ind, PtCdtr<PointT> cloud, std::vector<int> &cluster, KdTree *tree) 
{
	//std::vector<bool> processed;
	processed[ind] = true;
	cluster.push_back(ind);
	//ec.setClusterTolerance(clusterTolerance);  设置近邻搜索半径
	std::vector<int> nearest_point = tree->search(cloud->points[ind], distanceTol); 

	for (int nearest_id : nearest_point) 
	{
		if (!processed[nearest_id]) 
		{
			clusterHelper(nearest_id, cloud, cluster, tree);
		}
	}
}

//重写欧式聚类算法		ok
template<typename PointT>
std::vector<PtCdtr<PointT>> ClusterPts<PointT>::EuclidCluster(PtCdtr<PointT> cloud) 
{
	KdTree *tree = new KdTree;  //创建重写kdtree的对象
	//对cloud创建kdtree
	for (int ind = 0; ind < num_points; ind++) 
	{
		tree->insert(cloud->points[ind], ind);
	}
	//std::vector<bool> processed;
	for (int ind = 0; ind < num_points; ind++) 
	{
		if (processed[ind]) 
		{
			ind++;
			continue;
		}
		std::vector<int> cluster_ind; //每一类包含点的索引
		PtCdtr<PointT> cloudCluster(new pcl::PointCloud<PointT>);
		clusterHelper(ind, cloud, cluster_ind, tree);

		int cluster_size = cluster_ind.size();  //总类数
		if (cluster_size >= minClusterSize && cluster_size <= maxClusterSize) 
		{
			for (int i = 0; i < cluster_size; i++) 
			{
				cloudCluster->points.push_back(cloud->points[cluster_ind[i]]);
			}
			cloudCluster->width = cloudCluster->points.size();
			cloudCluster->height = 1;
			clusters.push_back(cloudCluster);
		}
	}
	return clusters;
}