当前位置 博文首页 > 百把人的博客：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;
