0. 简介

最近在看点云匹配相关的知识点，而KD树和八叉树作为点云匹配中最为重要的方法，当然需要好好看看。这里写一篇博客记录一下，便于后面回顾。（最近发现SLAM、ROS方面已经基本粗略的写了一遍，后面会针对一些重要的点去零碎的填坑）。

1. KD-Tree

KD-Tree, 或称 k 维树,是计算机科学中使用的一种数据结构,用来组织表示 k 维空间中的点集合。一般在会基于 FLANN 进行快速最近邻检索。最近邻检索在匹配、特征描述子计算、邻域特征提取中是非常基础的核心操作。 KD-Tree 模块利用 两个类与两个函数实现了利用 KD-Tree数据结构对点云的高效管理和检索。

Kd树有几种常用的搜素方法：

• 体素内近邻搜索 （Neighbors with Voxel Search） 它把查询点所在的体素中的其他点的索引作为查询结果返回；
• K近邻搜索（K Nearest Neighbor Serach）；
• 半径内近邻搜索（Neighbor with Radius Search）

下面展示了在OPENCV中使用FLANN来生成KD-Tree的方法。

int main() {
	int queryNum = 3;//用于设置返回邻近点的个数
	vector<float> vecQuery(2);//存放查询点的容器
	vector<int> vecIndex(queryNum);//存放返回的点索引
	vector<float> vecDist(queryNum);//存放距离
	cv::flann::SearchParams params(32);//设置knnSearch搜索参数
	cv::flann::KDTreeIndexParams indexParams(2);
									   
	vecQuery[0] = 3, vecQuery[1] = 4;

	cv::Mat source;
	vector<cv::flann::Index*> kdtrees;
	{
		vector<cv::Vec2d> features = { { 1,1 },{ 2, 2 },{ 3, 3 },{ 4, 4 },{ 2, 4 } };
		source = cv::Mat(features).reshape(1);
		source.convertTo(source, CV_32F);
		cv::flann::Index* kdtree = new cv::flann::Index(source, indexParams);
		kdtrees.push_back(kdtree);
	}

	kdtrees[0]->knnSearch(vecQuery, vecIndex, vecDist, queryNum, params);
	for (int i = 0; i < vecIndex.size(); i++)
		cout << "nearest id: " << vecIndex[i] << "\tdist:" << vecDist[i] << endl;

	return 0;
}

而PCL也可以使用KD-Tree，但是这个部分KD-Tree由于是PCL库的，所以一般适用于三维特征点。

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

int main (int argc, char**argv)
{
  srand (time (NULL));
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>); // 创建一个PointCloud<PointXYZ> boost共享指针，并进行实例化为cloud
  
  // 随机生成一个1000个点的无序点云
  cloud->width =1000; // 注意因为cloud是指针，所以这里用->
  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; // 创建k-d tree对象
  kdtree.setInputCloud (cloud); // 将cloud设为k-d tree是搜索空间
  
  // 随机生成查询点
  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);

  int K =10;
  std::vector<int>pointIdxNKNSearch(K); // 设置一个整型的<vector>，用于存放第几近邻的索引
  std::vector<float>pointNKNSquaredDistance(K); // 设置一个浮点型的<vector>, 用于存放第几近邻与查询点的平方距离

  std::cout<<"K nearest neighbor search at ("<< searchPoint.x <<" "<< searchPoint.y <<" "<< searchPoint.z <<") with K="<< K <<std::endl;
  
  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;
    }
  }

  std::vector<int> pointIdxRadiusSearch;
  std::vector<float> pointRadiusSquaredDistance;
  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;
  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;
}

2.八叉树

八叉树是一种用于管理稀疏3D数据的树状数据结构，每个内部节点都正好有8个子节点。
八叉树也是一种索引方式，可以基于八叉树（Octrees）对点云进行空间划分和搜索操作。
八叉树结构通过循环递归的划分方法对大小为2n * 2n * 2n的三维空间的几何对象进行剖分，从而构成一个具有根节点的方向图。

…详情请参照古月居