c++实现kdTree创建以及最近邻点查找_最近邻快速从轨迹中找出二维最近点c++

参考：

https://blog.csdn.net/zfjBIT/article/details/95370168

https://www.cnblogs.com/theodoric008/p/9475189.html

Kd-Tree的创建：

（1）在K维数据集合中选择具有最大方差的维度k，然后在该维度上选择中值m为起始节点对该数据集合进行划分，得到两个子集合；同时创建一个树结点node，用于存储；

（2）对两个子集合重复（1）步骤的过程，直至所有子集合都不能再划分为止；

Kd-Tree的最近邻查找：

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

        其中Q与结点的比较指的是将Q对应于结点中的k维度上的值与中值m进行比较，若Q(k) < m，则访问左子树，否则访问右子树。达到叶子结点时，计算Q与叶子结点上保存的数据之间的距离，记录下最小距离对应的数据点，记为当前最近邻点nearest和最小距离dis。
（2）进行回溯操作，该操作是为了找到离Q更近的“最近邻点”。即判断未被访问过的分支里是否还有离Q更近的点，它们之间的距离小于dis。
        如果Q与其父结点下的未被访问过的分支之间的距离小于dis，则认为该分支中存在离P更近的数据，进入该结点，进行（1）步骤一样的查找过程，如果找到更近的数据点，则更新为当前的最近邻点nearest，并更新dis。
        如果Q与其父结点下的未被访问过的分支之间的距离大于dis，则说明该分支内不存在与Q更近的点。
        回溯的判断过程是从下往上进行的，直到回溯到根结点时已经不存在与P更近的分支为止。

注：判断未被访问过的树分支中是否还有离Q更近的点，就是判断"Q与未被访问的树分支的距离|Q(k) - m|“是否小于"Q到当前的最近邻点nearest的距离dis”。从几何空间上来看，就是判断以Q为中心，以dis为半径超球面是否与未被访问的树分支代表的超矩形相交。

测试示例：

假设有6个二维数据点{（2,3），（5,4），（9,6），（4,7），（8,1），（7,2）}，数据点位于二维空间内（如下图中黑点所示）：

                  图1

递归创建Kd-Tree：

1. 分别计算x，y方向上数据的方差，得知x方向上的方差最大；
2. 根据x轴方向的值2,5,9,4,8,7排序选出中值为7，所以该node中的data = （7,2）。这样，该节点的分割超平面就是通过（7,2）并垂直于x轴的直线x = 7；
3. 确定左子空间和右子空间。分割超平面x = 7将整个空间分为两部分，如下图所示。x < =  7的部分为左子空间，包含3个节点{（2,3），（5,4），（4,7）}；另一部分为右子空间，包含2个节点{（9,6），（8,1）}。

k-d树的构建是一个递归的过程。然后对左子空间和右子空间内的数据重复根节点的过程就可以得到下一级子节点（5,4）和（9,6）（也就是左右子空间的’根’节点），同时将空间和数据集进一步细分。如此反复直到空间中只包含一个数据点，如下图所示：

                                     图2

查找点Q(2.1,3.1)：

如下图所示，红色的点即为要查找的点。通过图2二叉搜索，顺着搜索路径很快就能找到当前的最邻近点（2,3）。

                                                              

                                                                               图3

       在上述搜索过程中，产生的搜索路径节点有<（7,2），（5,4），（2,3）>。为了找到真正的最近邻，还需要进行’回溯’操作，首先以（2,3）作为当前最近邻点nearest，计算其到查询点Q（2.1,3.1）的距离dis为0.1414，然后回溯到其父节点（5,4），并判断在该父节点的其他子节点空间中是否有距离查询点Q更近的数据点。以（2.1,3.1）为圆心，以0.1414为半径画圆，如图6所示。发现该圆并不和超平面y = 4交割，即这里：|Q(k) - m|=|3.1 - 4|=0.9 > 0.1414，因此不用进入（5,4）节点右子空间中去搜索。

                                                                 

                                                                                 图. 4

        再回溯到（7,2），以（2.1,3.1）为圆心，以0.1414为半径的圆更不会与x = 7超平面交割，因此不用进入（7,2）右子空间进行查找。至此，搜索路径中的节点已经全部回溯完，结束整个搜索，返回最近邻点（2,3），最近距离为0.1414。

#include <stdio.h>
#include <iostream>
#include <algorithm>
#include <iostream>
#include <vector>
#include <stack>
#include <cmath>
 
#define KDtreeSize 1000
 
#define UL unsigned long
 
using namespace std;
 
struct coordinate
{
	double x = 0;
	double y = 0;
	UL index = 0;
};
 
struct TreeNode
{
	struct coordinate dom_elt;
	UL split = 0;
	struct TreeNode* left = nullptr;
	struct TreeNode* right = nullptr;
};
 
bool cmp1(const coordinate& a, const coordinate& b) {
	return a.x < b.x;
}
 
bool cmp2(const coordinate& a, const coordinate& b) {
	return a.y < b.y;
}
 
bool equal(const coordinate& a, const coordinate& b) {
	return (a.x == b.x && a.y == b.y);
}
 
void ChooseSplit(coordinate exm_set[], UL size, UL &split, coordinate &SplitChoice) {
	/*
	1. 计算每个维度(x,y)的方差,从具有最大方差的维度开始切分，如x方向；
	2. 计算x方向的参数得中值，作为起始节点；
	*/
 
	double tmp1, tmp2;
	tmp1 = tmp2 = 0;
	for (int i = 0; i < size; ++i)
	{
		tmp1 += 1.0 / (double)size * exm_set[i].x * exm_set[i].x;
		tmp2 += 1.0 / (double)size * exm_set[i].x;
	}
	double v1 = tmp1 - tmp2 * tmp2;  //compute variance on the x dimension
 
	tmp1 = tmp2 = 0;
	for (int i = 0; i < size; ++i)
	{
		tmp1 += 1.0 / (double)size * exm_set[i].y * exm_set[i].y;
		tmp2 += 1.0 / (double)size * exm_set[i].y;
	}
	double v2 = tmp1 - tmp2 * tmp2;  //compute variance on the y dimension
 
	split = v1 > v2 ? 0 : 1; //set the split dimension
 
	//排序
	if (split == 0)
	{
		sort(exm_set, exm_set + size, cmp1);
	}
	else {
		sort(exm_set, exm_set + size, cmp2);
	}
 
	//set the split point value:中值
	SplitChoice.x = exm_set[size / 2].x;
	SplitChoice.y = exm_set[size / 2].y;
}
 
//递归创建kdtree
TreeNode* build_kdtree(coordinate exm_set[], UL size, TreeNode* T) {
	//call function ChooseSplit to choose the split dimension and split point
	if (size == 0) {
		return nullptr;
	}
	else {
		UL split;
		coordinate dom_elt;
		ChooseSplit(exm_set, size, split, dom_elt);//返回起始切分方向和起始节点
		coordinate exm_set_right[KDtreeSize];//存储位于左子空间的点
		coordinate exm_set_left[KDtreeSize]; //存储位于左子空间的点
		UL size_left, size_right;
		size_left = size_right = 0;
 
		if (split == 0)
		{
			//起始切分方向为x方向
			for (UL i = 0; i < size; ++i)
			{
				//小于等于节点dom_elt.x的属于左空间
				if (!equal(exm_set[i], dom_elt) && exm_set[i].x <= dom_elt.x)
				{
					exm_set_left[size_left].x = exm_set[i].x;
					exm_set_left[size_left].y = exm_set[i].y;
					size_left++;
				}//大于节点dom_elt.x的属于右空间
				else if (!equal(exm_set[i], dom_elt) && exm_set[i].x > dom_elt.x)
				{
					exm_set_right[size_right].x = exm_set[i].x;
					exm_set_right[size_right].y = exm_set[i].y;
					size_right++;
				}
			}
		}
		else {
			//起始切分方向为y方向
			for (UL i = 0; i < size; ++i)
			{
				//小于等于节点dom_elt.y的属于左空间
				if (!equal(exm_set[i], dom_elt) && exm_set[i].y <= dom_elt.y)
				{
					exm_set_left[size_left].x = exm_set[i].x;
					exm_set_left[size_left].y = exm_set[i].y;
					size_left++;
				}//大于节点dom_elt.y的属于左空间
				else if (!equal(exm_set[i], dom_elt) && exm_set[i].y > dom_elt.y)
				{
					exm_set_right[size_right].x = exm_set[i].x;
					exm_set_right[size_right].y = exm_set[i].y;
					size_right++;
				}
			}
		}
		T = new TreeNode;
		T->dom_elt.x = dom_elt.x;
		T->dom_elt.y = dom_elt.y;
		T->split = split;
		T->left = build_kdtree(exm_set_left, size_left, T->left);    //递归
		T->right = build_kdtree(exm_set_right, size_right, T->right);//递归
		return T;
	}
}
 
//计算两点间距离
double Distance(coordinate a, coordinate b) {
	double tmp = (a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y);
	return sqrt(tmp);
}
 
//搜索最邻近点
void searchNearest(TreeNode * Kd, coordinate target, coordinate &nearestpoint, double & distance) {
 
	//1. 如果Kd是空的，则设dist为无穷大返回
	//2. 向下搜索直到叶子结点
	stack<TreeNode*> search_path;
	TreeNode* pSearch = Kd;
	coordinate nearest;
	double dist;
 
	while (pSearch != nullptr)
	{
		//pSearch加入到search_path中;
		search_path.push(pSearch);
 
		if (pSearch->split == 0)
		{
			if (target.x <= pSearch->dom_elt.x) /* 如果小于就进入左子树 */
			{
				pSearch = pSearch->left;
			}
			else
			{
				pSearch = pSearch->right;
			}
		}
		else {
			if (target.y <= pSearch->dom_elt.y) /* 如果小于就进入左子树 */
			{
				pSearch = pSearch->left;
			}
			else
			{
				pSearch = pSearch->right;
			}
		}
	}
	//取出search_path最后一个赋给nearest
	nearest.x = search_path.top()->dom_elt.x;
	nearest.y = search_path.top()->dom_elt.y;
	search_path.pop();
 
	dist = Distance(nearest, target);
	//3. 回溯搜索路径
	TreeNode* pBack;
 
	while (search_path.empty())
	{
		//取出search_path最后一个结点赋给pBack
		pBack = search_path.top();
		search_path.pop();
 
		if (pBack->left == nullptr && pBack->right == nullptr) /* 如果pBack为叶子结点 */
		{
			if (Distance(nearest, target) > Distance(pBack->dom_elt, target))
			{
				nearest = pBack->dom_elt;
				dist = Distance(pBack->dom_elt, target);
			}
		}
		else
		{
			UL s = pBack->split;
			if (s == 0)
			{
				if (fabs(pBack->dom_elt.x - target.x) < dist) /* 如果以target为中心的圆（球或超球），半径为dist的圆与分割超平面相交， 那么就要跳到另一边的子空间去搜索 */
				{
					if (Distance(nearest, target) > Distance(pBack->dom_elt, target))
					{
						nearest = pBack->dom_elt;
						dist = Distance(pBack->dom_elt, target);
					}
					if (target.x <= pBack->dom_elt.x) /* 如果target位于pBack的左子空间，那么就要跳到右子空间去搜索 */
						pSearch = pBack->right;
					else
						pSearch = pBack->left; /* 如果target位于pBack的右子空间，那么就要跳到左子空间去搜索 */
					if (pSearch != nullptr)
						//pSearch加入到search_path中
						search_path.push(pSearch);
				}
			}
			else {
				if (fabs(pBack->dom_elt.y - target.y) < dist) /* 如果以target为中心的圆（球或超球），半径为dist的圆与分割超平面相交， 那么就要跳到另一边的子空间去搜索 */
				{
					if (Distance(nearest, target) > Distance(pBack->dom_elt, target))
					{
						nearest = pBack->dom_elt;
						dist = Distance(pBack->dom_elt, target);
					}
					if (target.y <= pBack->dom_elt.y) /* 如果target位于pBack的左子空间，那么就要跳到右子空间去搜索 */
						pSearch = pBack->right;
					else
						pSearch = pBack->left; /* 如果target位于pBack的右子空间，那么就要跳到左子空间去搜索 */
					if (pSearch != nullptr)
						// pSearch加入到search_path中
						search_path.push(pSearch);
				}
			}
 
		}
	}
 
	nearestpoint.x = nearest.x;
	nearestpoint.y = nearest.y;
	distance = dist;
}
 
void test_kdtree() {
	coordinate exm_set[6];
	exm_set[0].x = 2;    exm_set[0].y = 3;
	exm_set[1].x = 5;    exm_set[1].y = 4;
	exm_set[2].x = 9;    exm_set[2].y = 6;
	exm_set[3].x = 4;    exm_set[3].y = 7;
	exm_set[4].x = 8;    exm_set[4].y = 1;
	exm_set[5].x = 7;    exm_set[5].y = 2;
 
	struct TreeNode * root = nullptr;
	root = build_kdtree(exm_set, 6, root);
 
	coordinate nearestpoint;
	double distance;
	coordinate target;
	target.x = 2.1;
	target.y = 3.1;
 
	searchNearest(root, target, nearestpoint, distance);
	cout << "The nearest distance is " << distance << ",and the nearest point is " << nearestpoint.x << "," << nearestpoint.y << endl;
}
 
int main() {
	test_kdtree();
	return 0;
}