PCL KD树的原理及应用（K邻域搜索、半径搜索）（Kdtree_kdtree）

注意：KD树常用来进行K邻域搜索、半径搜索。

本文章中3.注意事项的内容为工作中常犯的错误，需留意。

1.原理

1.1 KD树数据结构

（1）又称K维树，用来组织表示K维空间中的点集合。是一种带有约束条件的二分查找树。在PCL中，通常只使用三个维度（xyz），因此PCL中的KD树可直接看做三维KD树。

（2）构造KD树时，如下图所示，不断使用垂直于坐标轴的超平面将K维空间一分为二，切分之后，节点左边的子树代表在超平面左边的点，节点右边的子树代表在超平面右边的点。

1.2 构造KD树

如：我们需对二维平面上的一些点组织KD树。点位如下：(2,3），（5,4），（9,6），（4,7），（8,1），（7,2）。

（1）组织结果如下图：

（2）组织步骤：

1）将上述六个点的点集按照二维平面的第一维（x轴）对数据进行排序，排序结果为（2,3），（4,7），（5,4），（7,2），（8,1），（9,6）

2）取得上述点集的中位数处的点，偶数个数的中位数一般取大的那个，在上述点集中取到的为（7,2），在该点处对平面进行划分，如图1中（7,2）位置画上一条竖线

3）将由（7,2）划分的左右两平面中的剩余点按照二维平面的第二维（y轴）对数据排序，排序结果为左枝：（2,3），（5,4），（4,7）和右枝（8,1），（9,6）

4）将左枝和右枝分别取中位数点作为新的节点，按照第一维继续排序，直致每一个子枝上只剩一个点，这个点被称作叶子。

5）得到KD树。

组织步骤图如下：

1.3 KD树的最邻近查找

以查询数据Q举例：

（1）将查询数据Q从根结点开始，按照Q与各个结点的比较结果向下访问Kd-Tree，直至达到叶子结点。

Q与结点的比较：将Q对应于结点中的k维度上的值与中值m进行比较，若Q(k) < m，则访问左子树，否则访问右子树。达到叶子结点时，计算Q与叶子结点上保存的数据之间的距离，记录下最小距离对应的数据点，记为当前最近邻点nearest和最小距离dis。

（2）进行回溯操作，已查询是是否有离Q更近的“最近邻点”。即判断未被访问过的分支里是否还有离Q更近的点，它们之间的距离小于dis。

1）如果Q与其父结点下的未被访问过的分支之间的距离小于dis，则认为该分支中存在离P更近的数据，进入该结点，进行（1）步骤一样的查找过程，如果找到更近的数据点，则更新为当前的最近邻点nearest，并更新dis。

2） 如果Q与其父结点下的未被访问过的分支之间的距离大于dis，则说明该分支内不存在与Q更近的点。

 回溯的判断过程是从下往上进行的，直到回溯到根结点时已经不存在与P更近的分支为止。

2.使用场景

K邻域搜索（搜索距离输入点Q，最邻近的K个点）

半径搜索（搜索输入点Q半径R内的所有点）

3.注意事项

PCL中，如果使用建立KD树的点云本身的点进行K邻域搜索搜索最近点：

1）若只搜索1个点，那么搜索到的点为输入点本身【距离=0】。

因此想要搜索距离某点云点最邻近的点，需要K领域搜索2个点。PCL会按照距离，由远到近将搜索出来的结果点云的索引自动排序。索引为1的点，即为你要搜索的最邻近点。

K邻域搜索搜索最近点示例代码如下：

#include <iostream>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>

int main()
{
	/****************添加点云********************/
	// 原始点云
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::io::loadPCDFile("D:/code/csdn/data/bunny.pcd", *cloud);    // 加载原始点云数据

	/****************使用点云本身的点进行最邻近点查找********************/
	pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;							//	建树
	kdtree.setInputCloud(cloud);

	pcl::PointXYZ searchPoint = cloud->points[0];					// 设置查找点
	int K = 2;														// 设置需要查找的近邻点个数=2
	std::vector<int> pointIdxNKNSearch(K);							// 保存每个近邻点的索引
	std::vector<float> pointNKNSquaredDistance(K);					// 保存每个近邻点与查找点之间的欧式距离平方

	if (kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)		// kd树查找
	{
		int minIdx = pointIdxNKNSearch[1];							// 最邻近点索引
		float minDisSqu = pointNKNSquaredDistance[1];				// 最邻近点距离（欧式距离的平方）

		std::cout << "【索引0为点云本身点】索引：" << pointIdxNKNSearch[0] << std::endl;
		std::cout << "【索引0为点云本身点】欧式距离的平方：" << pointNKNSquaredDistance[0] << std::endl;

		std::cout << "【最邻近点】索引：" << pointIdxNKNSearch[1] << std::endl;
		std::cout << "【最邻近点】欧式距离的平方：" << pointNKNSquaredDistance[1] << std::endl;
	}
}

结果打印：

注意：是使用建立KD树的点云本身的点进行K邻域搜索，才会发生上述情况。

4.关键函数

（1）K邻域搜索（搜索距离输入点Q，最邻近的K个点）

int pcl::KdTreeFLANN< PointT, Dist >::nearestKSearch  ( const PointT &  point,  
  unsigned int  k,  
  Indices &  k_indices,  
  std::vector< float > &  k_sqr_distances  
 )  const 

（2）半径搜索（搜索输入点Q半径R内的所有点）

int pcl::KdTreeFLANN< PointT, Dist >::radiusSearch  ( const PointT &  point,  
  double  radius,  
  Indices &  k_indices,  
  std::vector< float > &  k_sqr_distances,  
  unsigned int  max_nn = 0  
 )  const 

5.代码

（1）K邻域搜索

#include <iostream>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>

int main()
{
	/****************添加点云********************/
	// 原始点云
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::io::loadPCDFile("D:/code/csdn/data/bunny.pcd", *cloud);    // 加载原始点云数据

	/****************K邻域搜索********************/
	pcl::KdTreeFLANN<pcl::PointXYZ> kdtreeK;						// 建树
	kdtreeK.setInputCloud(cloud);

	pcl::PointXYZ searchPointK = cloud->points[0];					// 设置查找点
	int K = 500;														// 设置需要查找的近邻点个数
	std::vector<int> pointIdxNKNSearch(K);							// 保存每个近邻点的索引
	std::vector<float> pointNKNSquaredDistance(K);					// 保存每个近邻点与查找点之间的欧式距离平方
	bool bFlagK = false;											// 是否搜索成功

	if (kdtreeK.nearestKSearch(searchPointK, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)		// kd树查找
	{
		if (pointIdxNKNSearch.size() == K)
		{
			bFlagK = true;
		}
	}

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudK(new pcl::PointCloud<pcl::PointXYZ>);			// K邻近搜索结果
	if (bFlagK)
	{
		std::cout << "K邻域搜索成功" << std::endl;
		std::cout << "K邻域搜索得到点个数为：" << pointIdxNKNSearch.size() << std::endl;

		pcl::copyPointCloud(*cloud, pointIdxNKNSearch, *cloudK);
	}
	
	// 展示
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewK(new pcl::visualization::PCLVisualizer("raw"));
	viewK->addPointCloud<pcl::PointXYZ>(cloud, "raw cloud");
	viewK->addPointCloud<pcl::PointXYZ>(cloudK, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(cloudK, 255, 0, 0), "k cloud");
	viewK->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "raw cloud");
	while (!viewK->wasStopped())
	{
		viewK->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}
}

（2）半径搜索

#include <iostream>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>

int main()
{
	/****************添加点云********************/
	// 原始点云
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::io::loadPCDFile("D:/code/csdn/data/bunny.pcd", *cloud);    // 加载原始点云数据

	/****************半径搜索********************/
	pcl::KdTreeFLANN<pcl::PointXYZ> kdtreeR;						// 建树
	kdtreeR.setInputCloud(cloud);
	pcl::PointXYZ searchPoint = cloud->points[0];					// 设置查找点
	float radius = 0.03;											// 查找半径
	std::vector<int> pointIdxRadiusSearch;							// 保存每个近邻点的索引
	std::vector<float> pointRadiusSquaredDistance;					// 保存每个近邻点与查找点之间的欧式距离平方
	bool bFlagR = false;											// 是否搜索成功

	if (kdtreeR.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)				// kd树查找
	{
		if (pointIdxRadiusSearch.size() > 1)
		{
			bFlagR = true;
		}
	}

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudR(new pcl::PointCloud<pcl::PointXYZ>);			// 半径搜索结果
	if (bFlagR)
	{
		std::cout << "半径搜索成功" << std::endl;
		std::cout << "半径搜索得到点个数为：" << pointIdxRadiusSearch.size() << std::endl;
		pcl::copyPointCloud(*cloud, pointIdxRadiusSearch, *cloudR);
	}

	// 展示
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewR(new pcl::visualization::PCLVisualizer("raw"));
	viewR->addPointCloud<pcl::PointXYZ>(cloud, "raw cloud");
	viewR->addPointCloud<pcl::PointXYZ>(cloudR, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(cloudR, 0, 255, 0), "r cloud");
	viewR->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "raw cloud");
	while (!viewR->wasStopped())
	{
		viewR->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}
}

6.结果展示

（1）K邻域搜索

1）结果打印：

2）结果可视化（红色部分为K邻域搜索得到的结果）：

（2）半径搜索

1）结果打印：

2）结果可视化（绿色部分为半径搜索得到的结果）：