DBSCAN聚类point cloud

之前找到的很多DBSCAN代码都是处理二维点的，而且点的数量比较小，这个是处理三维点的，由于点的数量比较大，所以加入了pcl中的octree、kdtree，用来做邻域搜索，提升代码速度。

代码如下：

#include "stdafx.h"

#include <iostream>
#include <fstream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/octree/octree.h>
#include <pcl/kdtree/kdtree_flann.h>

using namespace std;

float eps = 0.5;//邻域距离
int min_pets = 5;//邻域内最少点

class point
{
public:
	float x;
	float y;
	float z;
	int visited = 0;
	int pointtype = 1;//1噪声，2边界点，3核心点
	int cluster = 0;
	vector<int> corepts;//存储邻域内点的索引
	point() {}
	point(float a, float b, float c)
	{
		x = a;
		y = b;
		z = c;
	}
};
vector<point> corecloud;//构建核心点集
vector<point> allcloud;
float distance(point a, point b) {
	return sqrt((a.x - b.x)*(a.x - b.x) + (a.y - b.y)*(a.y - b.y) + (a.z - b.z)*(a.z - b.z));
}

int main(int argc, char** argv)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);//初始化点云
	pcl::io::loadPCDFile<pcl::PointXYZ>("xyz3.pcd", *cloud);//加载pcd点云并放入cloud中
	float resolution = 0.5f;//最低一级octree的最小体素的尺寸
	pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);//初始化octree
	octree.setInputCloud(cloud);
	octree.addPointsFromInputCloud();

	size_t len = cloud->points.size();
	for (size_t i = 0; i < len; i++)
	{
		point pt = point(cloud->points[i].x, cloud->points[i].y, cloud->points[i].z);
		allcloud.push_back(pt);
	}
	//将核心点放在corecloud中,改变allcloud中的pointtype的值
	for (size_t i = 0; i < len; i++)
	{
		vector<int> radiussearch;//存放点的索引
		vector<float> radiusdistance;//存放点的距离平方
		octree.radiusSearch(cloud->points[i], eps, radiussearch, radiusdistance);//八叉树的邻域搜索
		if (radiussearch.size() > min_pets)
			allcloud[i].pointtype = 3;
		corecloud.push_back(allcloud[i]);
	}
	pcl::PointCloud<pcl::PointXYZ>::Ptr corecloud1(new pcl::PointCloud<pcl::PointXYZ>);
	corecloud1->points.resize(corecloud.size());
	for (int i = 0; i < corecloud.size(); i++) {
		corecloud1->points[i].x = corecloud[i].x;
		corecloud1->points[i].y = corecloud[i].y;
		corecloud1->points[i].z = corecloud[i].z;
	}
	pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
	kdtree.setInputCloud(corecloud1);

	for (int i = 0; i<corecloud.size(); i++) {
		vector<int> pointIdxNKNSearch;//存放点的索引
		vector<float> pointRadiusSquaredDistance;//存放点的距离平方
		octree.radiusSearch(corecloud1->points[i], eps, pointIdxNKNSearch, pointRadiusSquaredDistance);//八叉树的邻域搜索		
		for (int j = 0; j < pointIdxNKNSearch.size(); j++) {
			corecloud[i].corepts.push_back(pointIdxNKNSearch[j]);
		}
	}
	//将所有核心点根据是否密度可达归类，改变核心点cluster的值
	int outcluster = 0;
	for (int i = 0; i<corecloud.size(); i++) {
		stack<point*> ps;
		if (corecloud[i].visited == 1) continue;
		outcluster++;
		corecloud[i].cluster = outcluster;
		ps.push(&corecloud[i]);
		point *v;
		//将密度可达的核心点归为一类
		while (!ps.empty()) {
			v = ps.top();
			v->visited = 1;
			ps.pop();
			for (int j = 0; j<v->corepts.size(); j++) {
				if (corecloud[v->corepts[j]].visited == 1) continue;
				corecloud[v->corepts[j]].cluster = corecloud[i].cluster;
				corecloud[v->corepts[j]].visited = 1;
				ps.push(&corecloud[v->corepts[j]]);
			}
		}
	}
	//找出所有的边界点，噪声点，对边界点分类，更改其cluster
	for (int i = 0; i<len; i++) {
		if (allcloud[i].pointtype == 3) continue;
		for (int j = 0; j<corecloud.size(); j++) {
			if (distance(allcloud[i], corecloud[j])<eps) {
				allcloud[i].pointtype = 2;
				allcloud[i].cluster = corecloud[j].cluster;
				break;
			}
		}
	}
	for (int i = 0; i < len; i++)
	{
		if (allcloud[i].pointtype == 1)
			allcloud[i].cluster = 0;
	}
	//输出边界点和噪声点
	char newFileName[256] = { 0 };
	char indexStr[16] = { 0 };
	strcat(newFileName, "border_noise");
	strcat(newFileName, ".txt");
	ofstream outFile(newFileName, ios_base::out);
	for (size_t j = 0; j < len; ++j)
	{
		if (allcloud[j].pointtype != 3)
			outFile << allcloud[j].x << "\t" << allcloud[j].y << "\t" << allcloud[j].z << "\t" << allcloud[j].cluster << endl;
	}
	//输出核心点集
	char newFileName1[256] = { 0 };
	char indexStr1[16] = { 0 };
	strcat(newFileName1, "corepoint");
	strcat(newFileName1, ".txt");
	ofstream outFile1(newFileName1, ios_base::out);
	for (size_t j = 0; j < corecloud.size(); j++)
	{
		outFile1 << corecloud[j].x << "\t" << corecloud[j].y << "\t" << corecloud[j].z << "\t" << corecloud[j].cluster << endl;
	}
}