基于 PCL 实现三维点云欧式聚类

PCL: Point Cloud Library，集成大量点云处理算法的 C++ 库
Euclidean Cluster - 欧式聚类

基础知识

什么是欧式聚类

将点云分成若干个子点云（类），使得对于每个类中的任意一点，都存在类中的另一点与其欧式距离小于一个给定的阈值。

【来源】Abstractly, euclidean clustering groups points into clusters such that for any two points in a cluster, there exists a chain of points also within that cluster between both points such that the projected distance between subsequent points in the chain is less than some threshold.

欧式聚类和 K-Means 聚类的区别

欧式聚类旨在判断点与点之间的距离是否超出阈值，其效果如下图所示。此时上下两部分之间的距离正好超过阈值，因此被判为两类，如果在上下两部分之间增加一些点进行衔接，那么欧式聚类会将其聚为一类。

K-Means 旨在寻找中心点，找到中心点后，所有点云中的点自动分类到距离自己最近的中心点，K-Means 聚成两类（k=2）的效果如下图所示。

【图片来源】

PCL 代码实现

实现三维点云欧式聚类不用 PCL 也可以，Github 上有一些速度更快的欧式聚类算法（因为使用近似算法导致速度变快），例如【这个】，用法和 PCL 类似。

C++ 代码

引入头文件：

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/search/kdtree.h>
#include <pcl/segmentation/extract_clusters.h>

待聚类的点云数据指针：

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

指针数据可以直接读取 .pcd 文件获得：

pcl::PCDReader reader;
reader.read("table_scene_lms400.pcd", *cloud);

也可以通过接收 ROS 消息并转换类型得到：

#include <sensor_msgs/PointCloud2.h>

template<typename T>
pcl::PointCloud<T> cloudmsg2cloud(sensor_msgs::PointCloud2 cloudmsg) {
    pcl::PointCloud<T> cloudresult;
    pcl::fromROSMsg(cloudmsg,cloudresult);
    return cloudresult;
}

void callbackNode(const sensor_msgs::PointCloud2::ConstPtr &msg) {
	*cloud = cloudmsg2cloud<pcl::PointXYZ>(*msg);
}

定义 KdTree 并将点云数据指针输入其中：

pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud(cloud);

定义用于存放聚类结果的向量容器如下，类似于一个二维数组，cluster_indices[0] 表示第一个点云类中所有点的索引，通过索引可以在点云数据指针 cloud 中找到这个点（例如 (*cloud)[idx]，其中 idx 是索引）。

std::vector<pcl::PointIndices> cluster_indices;

使用 PCL 欧式聚类算法的主体：

pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance(0.02); // 单位为 m
ec.setMinClusterSize(1);
ec.setMaxClusterSize(25000);
ec.setSearchMethod(tree);
ec.setInputCloud(cloud);
ec.extract(cluster_indices);

使用聚类结果的示例：

int j = 0;
for (const auto& cluster : cluster_indices) {  // 对于向量容器中的每一个点云类 cluster
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
    for (const auto& idx : cluster.indices) {  // 对于点云类中的每一个点的索引 idx
        cloud_cluster->push_back((*cloud)[idx]);  // 把同一个类的点合成到一个新的点云数据指针 cloud_cluster 中
    }
    cloud_cluster->width = cloud_cluster->size ();
    cloud_cluster->height = 1;
    cloud_cluster->is_dense = true;

    std::cout << "PointCloud representing the Cluster: " << cloud_cluster->size () << " data points." << std::endl;
    std::stringstream ss;
    ss << std::setw(4) << std::setfill('0') << j;
    pcl::PCDWriter writer;
    writer.write<pcl::PointXYZ> ("cloud_cluster_" + ss.str () + ".pcd", *cloud_cluster, false);  // 输出到 .pcd 文件
    j++;
  }

参数解析

最重要的参数就是欧式聚类的阈值，即 ec.setClusterTolerance(0.02) 中的 0.02。减小这个值会导致原本聚成一类的点云被聚为多个类，增大这个值会导致原本聚为多个类的点云被聚为一个类。

ec.setMinClusterSize(1) 和 ec.setMaxClusterSize(25000) 分别规定了最少聚类数量和最大聚类数量，一般不会特意调整。