pcl实现三维简单均值漂移算法

首先回顾一下均值漂移的思路：
在高维空间所有样本点中任选一个P作为起点，在每一维度中，以常量r为半径，查找半径范围之内的所有点，将这些点的每一维坐标求平均值，得到新的点P‘。如此反复迭代，当达到精度要求后退出循环，此时P达到均值处。
为了便于理解，可以做个类比：一个质量分布不均匀的物体，求其质心的过程，就可以看作是一次均值漂移，只不过它将所有点作为查找对象，一次查找就能确定质心，而均值漂移算法每次选取部分点，逐渐逼近“质心”。

下面用pcl实现三维样本点的均值漂移：
（完整代码在文末）
首先尝试对平面点做均值漂移。

为了实现样本点的不均匀分布，这里用了ln()函数值来构造散点坐标：

for (int y = 0; y < a; ++y) {
			for (int x = 0; x < a; ++x) {
				cloud->points[y*a + x].x = log(x + 1) * 5;//[0,19.560]
				cloud->points[y*a + x].y = log(y + 1) * 5;
				cloud->points[y*a + x].z = 0;
				cloud->points[y*a + x].r = 255;
				cloud->points[y*a + x].g = 255;
				cloud->points[y*a + x].b = 255;
			}
		}

构造出的正方形如下图：（x,y越大的地方越密集）

接下来为了便于色彩对比，将初始颜色改成绿色。
使用kd树进行邻近点查找：（将查找到的点赋为红色，这样可以看出中心点的移动趋势）

do {
		cloud->points[pp] = temp;//符合条件则赋值，第一次多余
		cout << "Neighbors within radius search at (" << temp.x << " " << temp.y << " " << temp.z << ") with radius=" << radius << endl;
		float sum_x = 0, sum_y = 0, sum_z = 0;
		if (kdtree.radiusSearch(temp, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)  //如果找到了，就返回正数。
		{
			for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i) {
				cloud->points[pointIdxRadiusSearch[i]].r = 255;  //标记为红色
				cloud->points[pointIdxRadiusSearch[i]].g = 0;
				cloud->points[pointIdxRadiusSearch[i]].b = 0;
				sum_x += cloud->points[pointIdxRadiusSearch[i]].x;
				sum_y += cloud->points[pointIdxRadiusSearch[i]].y;
				sum_z += cloud->points[pointIdxRadiusSearch[i]].z;
				cout << "point " << i + 1 << ": " << sqrt(pointRadiusSquaredDistance[i]) << endl;  //输出距离
			}
			float disx = sum_x / pointIdxRadiusSearch.size(), disy = sum_y / pointIdxRadiusSearch.size(), disz = sum_z / pointIdxRadiusSearch.size();
			distance = sqrt(pow((disx-temp.x),2) + pow((disy-temp.y),2) + pow((disz-temp.z),2));
			temp.x = disx;
			temp.y = disy;
			temp.z = disz;
			count++;
			cout << "distance " << count << " = " << distance << endl;
		}
		else break;//如果没找到就退出
	} while (distance > delta);

最终查找效果：（红色终止于最密集处）

在此基础上进行三维均值漂移：
只需要对z坐标赋值即可。
下图是进行11次迭代后，达到点( 17.4077, 16.9699, 17.2516 )的效果：


完整代码（三维）：

#include <iostream>
#include <pcl/point_types.h>  //点类型相关定义
#include <pcl/io/pcd_io.h>  //文件输入输出
#include <pcl/visualization/cloud_viewer.h>  //可视化相关定义
#include <pcl/visualization/pcl_visualizer.h>
#include<ctime>//随机数用到
#include<cstdlib>//随机数用到
#include <pcl/kdtree/kdtree_flann.h>  //kdtree近邻搜索
#include<cmath>//开根号，用pi，三角函数（弧度制）,log

using namespace std;
int main() {
	typedef pcl::PointXYZRGB Point6;
	typedef pcl::PointCloud<Point6> Cloud6;

	Cloud6::Ptr cloud(new Cloud6);
	int a = 50;//边长
	int num = a*a*a;//正方体
	cloud->width = num;
	cloud->height = 1;
	cloud->is_dense = false;
	cloud->points.resize(cloud->width*cloud->height);

	for (int z = 0; z < a; ++z) {
		for (int y = 0; y < a; ++y) {
			for (int x = 0; x < a; ++x) {
				cloud->points[z*a*a + y*a + x].x = log(x + 1) * 5;//[0,19.560]
				cloud->points[z*a*a + y*a + x].y = log(y + 1) * 5;
				cloud->points[z*a*a + y*a + x].z = log(z + 1) * 5;
				cloud->points[z*a*a + y*a + x].r = 0;//初始绿色
				cloud->points[z*a*a + y*a + x].g = 255;
				cloud->points[z*a*a + y*a + x].b = 0;
			}
		}
	}
	

	//确定随机初始点
	srand((int)time(0));
	int pp = rand() % num;

	pcl::KdTreeFLANN<Point6> kdtree;  //建立kdtree对象
	kdtree.setInputCloud(cloud); //设置需要建立kdtree的点云指针
	vector<int> pointIdxRadiusSearch;  //保存每个近邻点的索引
	vector<float> pointRadiusSquaredDistance;  //保存每个近邻点与查找点之间的欧式距离平方
	float radius = 4;  //设置查找半径范围

	Point6 temp = cloud->points[pp];//改变temp的位置，迭代赋给pp处点
	float distance;//temp与pp处点距离
	float delta = 0.5;//阈值
	int count = 0;//计数

	do {
		cloud->points[pp] = temp;//符合条件则赋值，第一次多余
		cout << "Neighbors within radius search at (" << temp.x << " " << temp.y << " " << temp.z << ") with radius=" << radius << endl;
		float sum_x = 0, sum_y = 0, sum_z = 0;
		if (kdtree.radiusSearch(temp, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)  //如果找到了，就返回正数。但总能找到，因为这个函数计入中心点。
		{
			for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i) {
				cloud->points[pointIdxRadiusSearch[i]].r = 255;  //标记为红色
				cloud->points[pointIdxRadiusSearch[i]].g = 0;
				cloud->points[pointIdxRadiusSearch[i]].b = 0;
				sum_x += cloud->points[pointIdxRadiusSearch[i]].x;
				sum_y += cloud->points[pointIdxRadiusSearch[i]].y;
				sum_z += cloud->points[pointIdxRadiusSearch[i]].z;
				cout << "point " << i + 1 << ": " << sqrt(pointRadiusSquaredDistance[i]) << endl;  //输出距离
			}
			float disx = sum_x / pointIdxRadiusSearch.size(), disy = sum_y / pointIdxRadiusSearch.size(), disz = sum_z / pointIdxRadiusSearch.size();
			distance = sqrt(pow((disx-temp.x),2) + pow((disy-temp.y),2) + pow((disz-temp.z),2));
			temp.x = disx;
			temp.y = disy;
			temp.z = disz;
			count++;
			cout << "distance " << count << " = " << distance << endl;
		}
		else break;//如果没找到就退出
	} while (distance > delta);
	
	cout << "meanshift finished after " << count << " times" << endl;
	cout << "final position: ( " << cloud->points[pp].x << ", " << cloud->points[pp].y << ", " << cloud->points[pp].z << " )" << endl;

	//显示
	pcl::visualization::PCLVisualizer viewer("cloud viewer");
	viewer.addPointCloud<Point6>(cloud, "sample");

	while (!viewer.wasStopped())
	{
		viewer.spinOnce();  
	}

	//点云输出
	pcl::PCDWriter writer;
	writer.writeASCII<Point6>("3dmeanshift.pcd", *cloud);
}