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

自己理解的注释。

cluster3d.h

#ifndef PLAYBACK_CLUSTER3D_H
#define PLAYBACK_CLUSTER3D_H
#include 
#include 
#include 
#include "kdtree3d.h"

namespace lidar_obstacle_detection {
	// shorthand for point cloud pointer
	template
	using PtCdtr = typename pcl::PointCloud::Ptr;
	template
	class ClusterPts 
	{
	private:
		int num_points;
		float distanceTol;
		int minClusterSize;
		int maxClusterSize;
		std::vector processed;
		std::vector> 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 cloud, std::vector &cluster, KdTree *tree);
		std::vector> EuclidCluster(PtCdtr cloud);
	};
}
#endif //PLAYBACK_CLUSTER3D_H

 cluster3d.cpp

#include "cluster3d.h"
using namespace lidar_obstacle_detection;
template
ClusterPts::~ClusterPts() {}
//根据最近邻搜索的阈值，找到了类数，每一类包含了属于该类的点			ok
template
void ClusterPts::clusterHelper(int ind, PtCdtr cloud, std::vector &cluster, KdTree *tree) 
{
	//std::vector processed;
	processed[ind] = true;
	cluster.push_back(ind);
	//ec.setClusterTolerance(clusterTolerance);  设置近邻搜索半径
	std::vector 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
std::vector> ClusterPts::EuclidCluster(PtCdtr cloud) 
{
	KdTree *tree = new KdTree;  //创建重写kdtree的对象
	//对cloud创建kdtree
	for (int ind = 0; ind < num_points; ind++) 
	{
		tree->insert(cloud->points[ind], ind);
	}
	//std::vector processed;
	for (int ind = 0; ind < num_points; ind++) 
	{
		if (processed[ind]) 
		{
			ind++;
			continue;
		}
		std::vector cluster_ind; //每一类包含点的索引
		PtCdtr cloudCluster(new pcl::PointCloud);
		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;
}