点云聚类算法存档
欧式聚类
在PCL库的基本过程如下：
首先通过pcl::VoxelGrid (filters)先对点云数据进行下采样滤波；
然后通过pcl::SACSegmentation
然后通过 pcl::ExtractIndices
最后通过pcl::EuclideanClusterExtraction
#include "cluster.h"
#include <pcl/search/kdtree.h>
#include <cmath>
#include <algorithm>
pcl::PointCloud<pcl::PointXYZ>::Ptr Cluster::voxelDownsample(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, float leaf_size) {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
pcl::VoxelGrid<pcl::PointXYZ> sor;
sor.setInputCloud(cloud);
sor.setLeafSize(leaf_size, leaf_size, leaf_size);
sor.filter(*cloud_filtered);
return cloud_filtered;
}
std::vector<pcl::PointIndices> Cluster::euclideanCluster(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
float cluster_tolerance,
int min_cluster_size,
int max_cluster_size) {
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud(cloud);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance(cluster_tolerance);
ec.setMinClusterSize(min_cluster_size);
ec.setMaxClusterSize(max_cluster_size);
ec.setSearchMethod(tree);
ec.setInputCloud(cloud);
ec.extract(cluster_indices);
return cluster_indices;
}距离图聚类
std::vector<pcl::PointIndices> Cluster::depthCluster(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
double angle_threshold,
int search_step) {
// 初始化数据结构
std::array<double, DEPTH_HEIGHT*DEPTH_WIDTH*4> range_image = {};
std::vector<std::vector<std::pair<int,double>>> pointlist(DEPTH_HEIGHT*DEPTH_WIDTH);
std::vector<int> cluster_points(cloud->size(), -1);
// 步骤1：将3D点云投影到2D深度图像
projectToImage(cloud, range_image, pointlist);
// 步骤2：执行深度聚类
int current_label = 0;
auto labels = depthClusterCore(range_image, current_label, angle_threshold, search_step);
// 步骤3：将2D聚类结果映射回3D点云
for(int i = 0; i < pointlist.size(); i++) {
if(!pointlist[i].empty()) {
// 为最近的点分配聚类标签
cluster_points[pointlist[i][pointlist[i].size()-1].first] = labels[i];
int dist = pointlist[i][pointlist[i].size()-1].second;
// 为深度相近的其他点分配相同标签
for(int j = pointlist[i].size()-2; j > -1; j--) {
if(pointlist[i][j].second - dist < 0) {
cluster_points[pointlist[i][j].first] = labels[i];
}
dist = pointlist[i][j].second;
}
}
}
// 步骤4：将结果转换为PCL格式
std::vector<pcl::PointIndices> cluster_indices;
std::map<int, std::vector<int>> label_to_indices;
for(int i = 0; i < cluster_points.size(); i++) {
if(cluster_points[i] > 0) {
label_to_indices[cluster_points[i]].push_back(i);
}
}
for(auto& pair : label_to_indices) {
if(pair.second.size() > 10) { // 过滤小聚类
pcl::PointIndices indices;
indices.indices = pair.second;
cluster_indices.push_back(indices);
}
}
return cluster_indices;
}FEC
虽然其readme形同虚设，但是常规配置即可
https://github.com/YizhenLAO/FEC
核心的头文件如下：
struct PointIndex_NumberTag
{
float nPointIndex;
float nNumberTag;
};
bool NumberTag(const PointIndex_NumberTag& p0, const PointIndex_NumberTag& p1)
{
return p0.nNumberTag < p1.nNumberTag;
}
std::vector<pcl::PointIndices> FEC(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, int min_component_size, double tolorance, int max_n) {
unsigned long i, j;
if (cloud->size() < min_component_size)
{
PCL_ERROR("Could not find any cluster");
}
pcl::KdTreeFLANN<pcl::PointXYZ>cloud_kdtreeflann;
cloud_kdtreeflann.setInputCloud(cloud);
int cloud_size = cloud->size();
std::vector<int> marked_indices;
marked_indices.resize(cloud_size);
memset(marked_indices.data(), 0, sizeof(int) * cloud_size);
std::vector<int> pointIdx;
std::vector<float> pointquaredDistance;
int tag_num = 1, temp_tag_num = -1;
for (i = 0; i < cloud_size; i++)
{
// Clustering process
if (marked_indices[i] == 0) // reset to initial value if this point has not been manipulated
{
pointIdx.clear();
pointquaredDistance.clear();
cloud_kdtreeflann.radiusSearch(cloud->points[i], tolorance, pointIdx, pointquaredDistance, max_n);
/**
* All neighbors closest to a specified point with a query within a given radius
* para.tolorance is the radius of the sphere that surrounds all neighbors
* pointIdx is the resulting index of neighboring points
* pointquaredDistance is the final square distance to adjacent points
* pointIdx.size() is the maximum number of neighbors returned by limit
*/
int min_tag_num = tag_num;
for (j = 0; j < pointIdx.size(); j++)
{
/**
* find the minimum label value contained in the field points, and tag it to this cluster label.
*/
if ((marked_indices[pointIdx[j]] > 0) && (marked_indices[pointIdx[j]] < min_tag_num))
{
min_tag_num = marked_indices[pointIdx[j]];
}
}
for (j = 0; j < pointIdx.size(); j++)
{
temp_tag_num = marked_indices[pointIdx[j]];
/*
* Each domain point, as well as all points in the same cluster, is uniformly assigned this label
*/
if (temp_tag_num > min_tag_num)
{
for (int k = 0; k < cloud_size; k++)
{
if (marked_indices[k] == temp_tag_num)
{
marked_indices[k] = min_tag_num;
}
}
}
marked_indices[pointIdx[j]] = min_tag_num;
}
tag_num++;
}
}
std::vector<PointIndex_NumberTag> indices_tags;
std::vector<pcl::PointIndices> cluster_indices;
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
indices_tags.resize(cloud_size);
PointIndex_NumberTag temp_index_tag;
for (i = 0; i < cloud_size; i++)
{
/**
* Put each point index and the corresponding tag value into the indices_tags
*/
temp_index_tag.nPointIndex = i;
temp_index_tag.nNumberTag = marked_indices[i];
indices_tags[i] = temp_index_tag;
}
sort(indices_tags.begin(), indices_tags.end(), NumberTag);
unsigned long begin_index = 0;
for (i = 0; i < indices_tags.size(); i++)
{
// Relabel each cluster
if (indices_tags[i].nNumberTag != indices_tags[begin_index].nNumberTag)
{
if ((i - begin_index) >= min_component_size)
{
unsigned long m = 0;
inliers->indices.resize(i - begin_index);
for (j = begin_index; j < i; j++)
inliers->indices[m++] = indices_tags[j].nPointIndex;
cluster_indices.push_back(*inliers);
}
begin_index = i;
}
}
if ((i - begin_index) >= min_component_size)
{
for (j = begin_index; j < i; j++)
{
unsigned long m = 0;
inliers->indices.resize(i - begin_index);
for (j = begin_index; j < i; j++)
{
inliers->indices[m++] = indices_tags[j].nPointIndex;
}
cluster_indices.push_back(*inliers);
}
}
return cluster_indices;
}算法解析
同样使用欧几里得（L2）距离度量来测量无组织点云的接近度。但是和EC不同的是并不反复使用簇级方案。对每个点只使用了一次KD树的查询。查询次数为O（N）。
EC：每次找到一个点，查找所有临近点，构成一个点簇。对点簇内继续递归查找直到簇扩展完成。
FEC：对点进行遍历，对没有标记的点进行查询，自动合并到临近簇。
输入：未聚类的未排序的点云P，判断为同类的阈值 $ d_th $ ，领域点的最大数量 $ Th_max $。
输出：P.lab 带有聚类标签的点云。
初始化：P.lab设置为0；SegLab设置为1；建立P的KD树。
for p.lab in P: # 该逐点遍历方案和EC相反
if p.lab = 0：
Pnn = FindNeighbor(p, $ d_th $);
if 非0的Pnn.lab 存在：
minSegLab = min(非0的Pnn.lab，SegLab);
else:
minSegLab = SegLab;
end
for pj in Pnn:
if pj.lab > Pnn:
for pk.lab in P:
if pk.lab = minSegLab: pk.lab = minSegLab;
end
end
end
SegLab++;
end
end
return point labels