PCL笔记四：k-d tree和八叉树；近邻搜索；半径搜索；体素内搜索；空间变化检测；

点云数据主要是表征目标表面的海量点集合，并不具备传统实体网格数据的几何拓扑信息。

点云数据处理中最为核心的问题就是建立离散点间的拓扑关系，实现基于邻域关系的快速查找。

建立空间索引在点云数据处理中已被广泛应用，常见空间索引一般是自顶向下逐级划分空间的各种空间索引结构：BSP树、k-d tree、KDB树、R树、R+树、CELL树、四叉树、八叉树等。

---

k-d tree对于区间和近邻搜索十分有用。

PCL中k-d tree 库提供了k-d tree数据结构，基于FLANN进行快速最近邻检索。最近邻检索在匹配、特征描述子计算、领域特征提取中是非常基础的核心操作。

---

一：近邻搜索

• KNN算法：用k-d tree 树找到具体点或空间位置的k近邻，
• R半径搜索：找到用户指定的某一半径内的所有近邻。

近邻搜索需要用的kdtree头文件，kdtree是依赖于FLANN实现的。

#include<iostream>
#include<vector>
#include<ctime>
#include<pcl/point_cloud.h>
#include<pcl/kdtree/kdtree_flann.h>
using namespace std;

int main()
{
	srand(time(NULL));  // 利用系统时间初始化rand()函数的种子。
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

	// 点云生成
	cloud->width = 1000;
	cloud->height = 1;
	cloud->points.resize(cloud->width * cloud->height);
	for (size_t i = 0; i < cloud->points.size(); i++)
	{
		cloud->points[i].x = 1024.0f * rand() / (RAND_MAX + 1.0f);
		cloud->points[i].y = 1024.0f * rand() / (RAND_MAX + 1.0f);
		cloud->points[i].z = 1024.0f * rand() / (RAND_MAX + 1.0f);
	}
	pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;  // 创建一个KdTreeFLANN对象，输入点云。
	kdtree.setInputCloud(cloud);

	pcl::PointXYZ searchPoint;   // 创建一个searchPoint变量作为查询点，并且为它分配随机坐标值。
	searchPoint.x = 1024.0f * rand() / (RAND_MAX + 1.0f);
	searchPoint.y = 1024.0f * rand() / (RAND_MAX + 1.0f);
	searchPoint.z = 1024.0f * rand() / (RAND_MAX + 1.0f);

	// k近邻搜索
	int K = 10;    // 10个近邻点。
	vector<int>pointIdxNKNSearch(K);    // 存储搜索到查询点近邻的索引。
	vector<float>pointNKNSquaredDistance(K);   // 存储对应近邻的平方距离。
	cout << "K nearest neighbor search at (" << searchPoint.x << " " << searchPoint.y
		<< " " << searchPoint.z << ") with K = " << K << endl;

	// kdtree.nearestKSearch函数搜索距离索引点最近的K个点，搜到后返回搜到的点数。
	int res = kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance);
	cout << "nearestKSearch返回值为：" << res << endl;
	if (kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
	{
		// 打印输出。
		for (size_t i = 0; i < pointIdxNKNSearch.size(); i++)
		{
			cout << " " << cloud->points[pointIdxNKNSearch[i]].x << " " << cloud->points[pointIdxNKNSearch[i]].y
				 << " " << cloud->points[pointIdxNKNSearch[i]].z << " " << "(squared distance:"
				 << pointNKNSquaredDistance[i] << ")" << endl;
		}
	}

	cout << endl;

	// R半径搜索：在半径r内搜索近邻。
	vector<int> pointIdxRadiusSearch;  // 存储近邻索引。
	vector<float> pointRadiusSquaredDistance;   // 存储近邻对应的平均距离。
	float radius = 256.0f * rand() / (RAND_MAX + 1.0f);
	cout << "Neighbors within radius search at (" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z
		 << ") with radius=" << radius << endl;
	int b = kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance);
	cout << "R半径搜索方法：kdtree.radiusSearch返回值为：" << b << endl;
	if (b > 0)
	{
		for (size_t i = 0; i < pointIdxRadiusSearch.size(); i++)
		{
			cout << " " << cloud->points[pointIdxRadiusSearch[i]].x << " "
				<< cloud->points[pointIdxRadiusSearch[i]].y << " "
				<< cloud->points[pointIdxRadiusSearch[i]].z << " "
				<< "(squared distance: " << pointRadiusSquaredDistance[i] << ")" &