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);//初始化octreeoctree.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]]);}}}//找出所有的边界点，噪声点，对边界点分类，更改其clusterfor (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;}}