【点云处理】K-means聚类算法在点云数据精简上的应用

62 阅读 0 评论 41 点赞

我是靠谱客的博主迷路鞋子，最近开发中收集的这篇文章主要介绍【点云处理】K-means聚类算法在点云数据精简上的应用，觉得挺不错的，现在分享给大家，希望可以做个参考。

概述

我们通过深度相机获取的物体点云数据十分庞大，对后续的点云数据预处理需要大量的计算时间，而传统的体素网格法算法简单，但是对于物体的细节部分不能很好的保留。

我把整个程序分成了两个部分，精简的效果还是不错的，程序太长就不上图了。

一：设置体素大小，提取点云体素中心

#include<iostream>
#include<pcl/visualization/pcl_visualizer.h>
#include<pcl/visualization/cloud_viewer.h>
#include<pcl/octree/octree.h>
#include<pcl/io/pcd_io.h>
#include<vector>
using namespace std;
typedef Eigen::aligned_allocator<pcl::PointXYZ> AlignedPointT;
typedef pcl::PointXYZ PointT;
int main()
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile("C:\Users\15390\Desktop\horse实验结果\horse.pcd", *cloud);
float resolution = 0.002; //体素的大小
cout << "before点云" << cloud->points.size() << endl;
pcl::octree::OctreePointCloud<pcl::PointXYZ> octree(resolution);
octree.setInputCloud(cloud);
octree.addPointsFromInputCloud();
vector<PointT, AlignedPointT> voxel_centers;
octree.getOccupiedVoxelCenters(voxel_centers);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_core(new pcl::PointCloud<pcl::PointXYZ>);
cloud_core->width = voxel_centers.size();
cloud_core->height = 1;
cloud_core->points.resize(cloud_core->height*cloud_core->width);
for (size_t i = 0; i < voxel_centers.size() - 1; i++)
{
cloud_core->points[i].x = voxel_centers[i].x;
cloud_core->points[i].y = voxel_centers[i].y;
cloud_core->points[i].z = voxel_centers[i].z;
}
pcl::PCDWriter writer;
writer.write("C:\Users\15390\Desktop\小论文—显示点云\体素中心\xxx.pcd", *cloud_core);
cout << voxel_centers.size() << endl;
system("pause");
return 0;
}

二：将体素中心作为初始聚类中心，然后根据条件进行点云精简

#include<iostream>
#include<cstring>
#include<string>
#include<vector>
#include<pcl/visualization/cloud_viewer.h>
#include<pcl/point_types.h>
#include<pcl/io/pcd_io.h>
#include <pcl/common/common_headers.h>
#include<pcl/features/normal_3d.h>
#include<pcl/features/principal_curvatures.h>
using namespace std;
using namespace pcl;
//结构体
typedef struct st_point
{
pcl::PointXYZ pnt;
int ID;
st_point(){}
st_point(pcl::PointXYZ &p, int id)
{
pnt = p;
ID = id;
}
}st_point;
class KMeans
{
public:
int m_k;//定义了一个k
类
vector<st_point> mv_pntcloud;//定义一个容器 存放欲聚类得点云
vector<vector<st_point>> m_grp_pntcloud;//容器：存放每一类的点
vector<pcl::PointXYZ> mv_center; //容器：存放类中心
KMeans() //构造函数
{
m_k = 0;
}
inline void SetK(int k) //设置k值
{
m_k = k;
m_grp_pntcloud.resize(m_k);
}
bool SetInputCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr pPntCloud); //设置输入点云
bool InitKCenter();//初始化最初的k个聚类中心
double DistBetweenPoint(pcl::PointXYZ &p1, pcl::PointXYZ &p2); //空间内两点之间的距离
bool UpdateEveryClassCenter(vector<vector<st_point>> &grp_pntcloud, vector<pcl::PointXYZ> &center); //更新k个类的中心
bool ExistCenterShift(vector<pcl::PointXYZ> &prev_center, vector<pcl::PointXYZ> &cur_center);//是否存在中心点移动
bool Cluster();//开始聚类
bool SaveFile(const char *prex_name);//保存各自的类到各自的pcd文件中
bool SaveFile(const char *dir_name, const char *prex_name);//将聚类的点分别存到各自的pcd文件中
};
//设置输入点云：将输入的点云数据传入 容器 mv_pntcloud 中
bool KMeans::SetInputCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr pPntCloud)
{
size_t pntCount = (size_t)pPntCloud->points.size(); //pntCount 点云的大小
for (size_t i = 0; i < pntCount; i++)
{
st_point point;
point.pnt.x = pPntCloud->points[i].x;
point.pnt.y = pPntCloud->points[i].y;
point.pnt.z = pPntCloud->points[i].z;
point.ID = 0;
mv_pntcloud.push_back(point);
}
return true;
}
//初始化最初的k个聚类中心 也就是设置聚类中心
以输入的数组中的点 作为聚类中心
bool KMeans::InitKCenter()
{
vector<pcl::PointXYZ> pnt_arr;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_2(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCDReader reader;
reader.read("C:\Users\15390\Desktop\PAPER\PAPER\体素中心(92).pcd", *cloud_2);
mv_center.resize(m_k); //k个类 也就是有k个聚类中心
for (size_t i = 0; i < m_k; i++)
{
mv_center[i].x = cloud_2->points[i].x;
mv_center[i].y = cloud_2->points[i].y;
mv_center[i].z = cloud_2->points[i].z;
}
return true;
}
//空间内两点之间的距离
double KMeans::DistBetweenPoint(pcl::PointXYZ &p1, pcl::PointXYZ &p2)
{
double X = 0, Y = 0, Z = 0;
X = p1.x - p2.x;
Y = p1.y - p2.y;
Z = p1.z - p2.z;
return sqrt(X*X + Y*Y + Z*Z);
}
//更新k个类的中心
bool KMeans::UpdateEveryClassCenter(vector<vector<st_point>> &grp_pntcloud, vector<pcl::PointXYZ> &center)
{
if (center.size() != m_k)
{
PCL_ERROR("分类的类别个数不能为0n");
return false;
}
for (size_t i = 0; i < m_k; ++i)
{
float x = 0, y = 0, z = 0;
size_t pnt_num_in_grp = grp_pntcloud[i].size();
for (size_t j = 0; j < pnt_num_in_grp; ++j)
{
//这里的x y z 表示每个小类里面的每个 小点
x = x + grp_pntcloud[i][j].pnt.x;
y = y + grp_pntcloud[i][j].pnt.y;
z = z + grp_pntcloud[i][j].pnt.z;
}
x = x / pnt_num_in_grp;
y = y / pnt_num_in_grp;
z = z / pnt_num_in_grp;
center[i].x = x;
center[i].y = y;
center[i].z = z;
}
return true;
}
//是否存在中心点移动
bool KMeans::ExistCenterShift(vector<pcl::PointXYZ> &prev_center, vector<pcl::PointXYZ> &cur_center)
{
for (size_t i = 0; i < m_k; i++)
{
double dis = DistBetweenPoint(prev_center[i], cur_center[i]);
if (dis > 0.001)
{
return true;
}
}
return false;
}
//开始聚类
bool KMeans::Cluster()
{
pcl::PointCloud<pcl::PointXYZ>::Ptr pPntCloud;
vector<pcl::PointXYZ> v_center(mv_center.size());
do
{
//size_t pntCount = (size_t)pPntCloud->points.size();
for (size_t i = 0; i < mv_pntcloud.size(); ++i)
{
double min_dist = DBL_MAX;
int pnt_grp = 0;
for (size_t j = 0; j < m_k; ++j)
{
double dis = DistBetweenPoint(mv_pntcloud[i].pnt, mv_center[j]);
if (min_dist - dis > 0.000001)
{
min_dist = dis;
pnt_grp = j;
}
}
m_grp_pntcloud[pnt_grp].push_back(st_point(mv_pntcloud[i].pnt, pnt_grp));
}
for (size_t i = 0; i < mv_center.size(); ++i)
{
v_center[i] = mv_center[i];
}
if (!UpdateEveryClassCenter(m_grp_pntcloud, mv_center))
{
return false;
}
if (!ExistCenterShift(v_center, mv_center))
{
break;
}
for (size_t i = 0; i < m_k; ++i)
{
m_grp_pntcloud[i].clear();
}
} while (true);
return true;
}
//将聚类的点分别存到各自的pcd文件中
bool KMeans::SaveFile(const char *dir_name, const char *prex_name)
{
for (size_t i = 0; i < m_k; ++i)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr p_pnt_cloud(new pcl::PointCloud<pcl::PointXYZ>);
for (size_t j = 0; j < m_grp_pntcloud[i].size(); ++j)
{
pcl::PointXYZ pt;
pt.x = m_grp_pntcloud[i][j].pnt.x;
pt.y = m_grp_pntcloud[i][j].pnt.y;
pt.z = m_grp_pntcloud[i][j].pnt.z;
p_pnt_cloud->points.push_back(pt);
}
p_pnt_cloud->width = (int)m_grp_pntcloud[i].size();
p_pnt_cloud->height = 1;
char newFileName[256] = { 0 };
char indexStr[16] = { 0 };
strcat_s(newFileName, dir_name);
strcat_s(newFileName, "/");
strcat_s(newFileName, prex_name);
strcat_s(newFileName, "-");
sprintf(newFileName, "%d", i + 1);
strcat_s(newFileName, indexStr);
strcat_s(newFileName, ".pcd");
pcl::io::savePCDFileASCII(newFileName, *p_pnt_cloud);
}
return true;
}
void main()
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_1(new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile<pcl::PointXYZ>("C:\Users\15390\Desktop\PCL\bunny多.pcd", *cloud_1);
cloud_1->width = (int)cloud_1->points.size();
KMeans kmeans;
kmeans.SetInputCloud(cloud_1);
kmeans.SetK(91);
kmeans.InitKCenter();
kmeans.Cluster();
kmeans.SaveFile(".", "k3");
cout << "程序结束" << endl;
system("pause");
}