参考博客《欧式聚类（KD-Tree）详解，保姆级教程》和《(三分钟)学会kd-tree 激光SLAM点云搜索常见》

1. kd树的意义

• kd树是什么？

kd树是一种空间划分的数据结构，对于多个维度的数据，按照某种规则选定某个维度，在该维度下进行排序，选择中间的数据作为划分节点，然后分别对划分节点左边和右边的数据进行上面的划分步骤。

• 为什么需要kd树？

三维点云的数据量较大，使用kd树进行搜索可以减少时间，可以确保点云的关联点寻找和匹配处于实时状态。总而言之，就是利用kd树，可以实现点云数据的快速搜索。

2. kd树的使用

使用kd树一般有两个任务，分别是k近邻搜索和半径内搜索。代码实现主要参考双愚的代码

#include <iostream>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/point_cloud.h>
#include <vector>
#include <ctime>

int main(int argc, char** argv)
{
	srand(time(NULL));
	//第一步：生成点云
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
	cloud->width = 100;
	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::PointXYZ 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);
	
	//第三步：定义kd树
	pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
	kdtree.setInputCloud(cloud); //将点云cloud作为输入
	
	//第四步：采用kdtree.nearestKSearch方法，输出点searchPoint的最近10个点云
	int K = 10;
	std::cout << "K nearest neighbor search at (" << searchPoint.x 
            << " " << searchPoint.y 
            << " " << searchPoint.z
            << ") with K=" << K << std::endl;
	std::vector<int> pointIdxNKNSearch(K);
	std::vector<float> pointNKNSquaredDistance(K);
	if (kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
	{
		for (size_t i=0; i<pointIdxNKNSearch.size(); ++i)
		{
			std::cout << "    "  <<   cloud->points[ pointIdxNKNSearch[i] ].x 
                	<< " " << cloud->points[ pointIdxNKNSearch[i] ].y 
                	<< " " << cloud->points[ pointIdxNKNSearch[i] ].z 
                	<< " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;
		}
	}
	
	//第五步：采用kdtree.radiusSearch方法，输出点searchPoint的给定半径距离内的其他点
	float radius = 256.0f * rand() / (RAND_MAX + 1.0f);
	std::cout << "Neighbors within radius search at (" << searchPoint.x 
            << " " << searchPoint.y 
            << " " << searchPoint.z
            << ") with radius=" << radius << std::endl;
        std::vector<int> pointIdxRadiusSearch;
        std::vector<float> pointRadiusSquaredDistance;
        if (kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
        {
        	for (size_t i = 0; i < pointIdxRadiusSearch.size (); ++i)
        	{
        		std::cout << "    "  <<   cloud->points[ pointIdxRadiusSearch[i] ].x 
		        << " " << cloud->points[ pointIdxRadiusSearch[i] ].y 
		        << " " << cloud->points[ pointIdxRadiusSearch[i] ].z 
		        << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
        	}
        }
        
        return 0;
}

• CMakeLists.txt

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

project(kdtree_search)

find_package(PCL)

add_executable(kdtree_search kdtree_search.cpp)
target_link_libraries(kdtree_search ${PCL_LIBRARIES})

• 运行结果