PCL点云按指定方向进行聚类（指定类的宽度）

需指定方向和类的宽度。测试代码如下：

#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/common/centroid.h>  // 新增质心计算头文件
#include <pcl/common/common.h> 

 //类型定义简化
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

/**
 * @brief 沿Z轴方向聚类点云，限制每类在Z轴的长度不超过thresh
 * @param cloud 输入点云
 * @param thresh Z轴方向的最大允许长度
 * @return 聚类结果的索引集合
 */
std::vector<pcl::PointIndices> axisAlignedClustering(
	Eigen::Vector3f direction,
    const PointCloudT::Ptr& cloud, float thresh)
{
    std::vector<pcl::PointIndices> clusters;
    if (cloud->empty()) return clusters;

	auto p0 = cloud->points[0];
	direction.normalize();
auto project_dist = [&direction, &p0](PointT& p)
	{
		return direction.dot(Eigen::Vector3f(p.x - p0.x, p.y - p0.y, p.z - p0.z));
	};

    std::vector<int> indices(cloud->size());
    for (int i = 0; i < indices.size(); ++i) indices[i] = i;
    std::sort(indices.begin(), indices.end(), [&](int a, int b) {
        return project_dist(cloud->points[a]) < project_dist(cloud->points[b]);
        });

     
     //2. 滑动窗口分割
    pcl::PointIndices current_cluster;
	float start_z = project_dist(cloud->points[indices[0]]);//cloud->points[indices[0]].z;
    current_cluster.indices.push_back(indices[0]);

    for (size_t i = 1; i < indices.size(); ++i) {
        float current_z = project_dist(cloud->points[indices[i]]);
        if (current_z - start_z <= thresh) {
            current_cluster.indices.push_back(indices[i]);
        }
        else {
            clusters.push_back(current_cluster);
            current_cluster.indices.clear();
            current_cluster.indices.push_back(indices[i]);
            start_z = current_z;
        }
    }

    if (!current_cluster.indices.empty()) {
        clusters.push_back(current_cluster);
    }

    return clusters;
}

/**
 * @brief 可视化聚类结果
 * @param cloud 原始点云
 * @param clusters 聚类索引
 */
void visualizeClusters(
    const PointCloudT::Ptr& cloud,
    const std::vector<pcl::PointIndices>& clusters, float cloud_size, Eigen::Vector4f centroid)
{
    pcl::visualization::PCLVisualizer viewer("Cluster Visualization");
    viewer.setBackgroundColor(0, 0, 0);	

     //为每个聚类生成随机颜色
    std::vector<pcl::visualization::PointCloudColorHandlerCustom<PointT>> color_handlers;
    for (size_t i = 0; i < clusters.size(); ++i) {
        uint8_t r = rand() % 256;
        uint8_t g = rand() % 256;
        uint8_t b = rand() % 256;
        color_handlers.emplace_back(
            pcl::visualization::PointCloudColorHandlerCustom<PointT>(
                cloud, r, g, b));
    }

    
     //添加每个聚类的点云到可视化
    for (size_t i = 0; i < clusters.size(); ++i) {
        PointCloudT::Ptr cluster_cloud(new PointCloudT);
        pcl::ExtractIndices<PointT> extract;
        pcl::PointIndices::Ptr indices(new pcl::PointIndices(clusters[i]));
        extract.setInputCloud(cloud);
        extract.setIndices(indices);
        extract.filter(*cluster_cloud);

        std::string cluster_id = "cluster_" + std::to_string(i);
		viewer.addPointCloud<PointT>(
			cluster_cloud, color_handlers[i], cluster_id);
		viewer.setPointCloudRenderingProperties(
			pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, cluster_id);
    }

	// 关键参数设置
	const float camera_distance = 3.0 * cloud_size;  // 观察距离为点云尺寸的3倍
	const Eigen::Vector3f camera_pos = {
		centroid[0],
		centroid[1],
		centroid[2] + camera_distance
	};
viewer.initCameraParameters();
viewer.setCameraPosition(
	camera_pos[0], camera_pos[1], camera_pos[2],  // 相机位置
	centroid[0], centroid[1], centroid[2],         // 焦点位置
	0, -1, 0                                       // 上方向向量(Y轴负方向)
);

    viewer.addCoordinateSystem(1.0);
    while (!viewer.wasStopped()) {
        viewer.spinOnce(100);
    }
}

int main(int argc, char** argv) {

 //1. 创建点云对象
pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);

 //2. 打开并读取TXT文件
std::ifstream file("E:\\Data\\PathPlanning\\data.txt");
if (!file.is_open()) {
	std::cerr << "Error opening file: " << argv[1] << std::endl;
	return -1;
}

std::string line;
while (std::getline(file, line)) {
	if (line.empty()) continue;

	std::istringstream iss(line);
	PointT point;

	 //读取坐标 (x,y,z) 和法向量
    float nx, ny, nz;
	if (!(iss >> point.x >> point.y >> point.z >> nx >> ny >> nz)) {
		std::cerr << "Error parsing line: " << line << std::endl;
		continue;
	}
	cloud->push_back(point);
}
file.close();

 //3. 设置点云属性
cloud->width = cloud->size();
cloud->height = 1;
cloud->is_dense = false;

// 计算点云质心
Eigen::Vector4f centroid;
if (pcl::compute3DCentroid(*cloud, centroid)){
	std::cout << "点云质心坐标: ("
		<< centroid[0] << ", "
		<< centroid[1] << ", "
		<< centroid[2] << ")" << std::endl;
}
else {
	std::cerr << "无法计算空点云的质心" << std::endl;
	return -1;
}

// 计算点云尺寸范围
PointT min_pt, max_pt;
pcl::getMinMax3D(*cloud, min_pt, max_pt);
float cloud_size = std::max({
	max_pt.x - min_pt.x,
	max_pt.y - min_pt.y,
	max_pt.z - min_pt.z
	});


 //2. 沿Z轴聚类（设置Thresh = 1.0）
float thresh = 100.0f;
std::vector<pcl::PointIndices> clusters = 
	axisAlignedClustering(Eigen::Vector3f(1, 1, 0), cloud, thresh);

 //3. 输出聚类信息
std::cout << "Found " << clusters.size() << " clusters." << std::endl;
for (size_t i = 0; i < clusters.size(); ++i) {
	std::cout << "Cluster " << i << ": " << clusters[i].indices.size()
		<< " points" << std::endl;
}

 //4. 可视化
visualizeClusters(cloud, clusters, cloud_size, centroid);	

return 0;
}