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;
}