【无标题】

KDTree搜索点云数据最近邻点

• 由于在多维数据点云里进行搜索，这里首先要进行自定义数据结构
• 一、建立二叉树
• • 数据类型
• 二、根据二叉树搜索最近邻点
• • • 1.距离选择，可以自己定义最近点的距离方式，这里选择的是欧氏距离
    • 2.构建二叉树
    • 3.这里有关二叉树的其他函数，例如二叉树的拷贝，二叉树的删除，上述建树过程中使用了关键词new进行申请堆上的空间，程序使用完需要释放掉空间。
• 三、搜索二叉树的最近节点并删除已经选择到的节点
• • • 搜索二叉树的最近节点
    • 删除二叉树的节点
• 选取k个最近邻点，上述程序只能选择一个最近邻的点，如果选择目标点中为最近点的集合就很难做到。因为KdTree的删除是一个很难的事情。
• 总结

---

由于在多维数据点云里进行搜索，这里首先要进行自定义数据结构

提示：这里可以个人需要在数据点添加其他变量：

数据结构主要存储三维点的x,y,z坐标

//点云数据类型
struct Point3D {
	double x;
	double y;
	double z;
	Point3D(const Point3D& rhs) {
		this->x = rhs.x;
		this->y = rhs.y;
		this->z = rhs.z;
	}
	Point3D(double x, double y, double z) :x(x), y(y), z(z) {}

	void operator = (const Point3D& rhs) {
		this->x = rhs.x;
		this->y = rhs.y;
		this->z = rhs.z;
	}
	Point3D() : x(0.0), y(0.0), z(0.0) {}

};

---

提示：以下是本篇文章正文内容，下面案例可供参考

一、建立二叉树 数据类型

//KDTree节点数据类型
/*
* dom_elt : K-dimension tree,kd维空间中的一个样本点，类型为kd维的向量
* split : 分裂维的序号，也是垂直于分割超平面的方向轴序号，类型为整数；
* left : 由位于该节点分割超平面左子空间内所有数据点构成的kd-Tree,类型为KdTree
* right: 由位于该节点分割超平面右子空间内所有数据点构成的kd-Tree,类型为KdTree
*/

struct TreeNode {
	struct Point3D dom_elt;
	int split;
	TreeNode* left;
	TreeNode* right;
	bool exist;
	TreeNode(Point3D point, int x, bool isfalse) : dom_elt(point), split(x), left(NULL), right(NULL), exist(isfalse) {}
	//TreeNode() : d
};

运算符重载

//运算符重载
bool cmpx(Point3D a, Point3D b) {
	return a.x < b.x;
}
bool cmpy(Point3D a, Point3D b) {
	return a.y < b.y;
}
bool cmpz(Point3D a, Point3D b) {
	return a.z < b.z;
}

bool equal(Point3D a, Point3D b) {
	if (a.x == b.x && a.y == b.y && a.z == b.z) {
		return true;
	}
	else return false;
}

选择划分的依据，主要是选择方差最大的维度进行划分，因为方差最大，点云在这个维度的分布相对比较分散
判断方差最大的方向,从几个维度里面选择方差最大的方向，使得选择的方向数据分布更加均匀

//判断方差最大的方向,从几个维度里面选择方差最大的方向，使得选择的方向数据分布更加均匀
void chooseSplit(vector<Point3D>& points, int& split, Point3D& SplitChoice) {
	double temp1x = 0, temp2x = 0;
	double temp1y = 0, temp2y = 0;
	double temp1z = 0, temp2z = 0;
	int len = points.size();
	for (int i = 0; i < len; i++) {
		temp1x += 1.0 / (double)len * points[i].x * points[i].x;
		temp2x += 1.0 / (double)len * points[i].x;

		temp1y += 1.0 / (double)len * points[i].y * points[i].y;
		temp2y += 1.0 / (double)len * points[i].y;

		temp1z += 1.0 / (double)len * points[i].z * points[i].z;
		temp2z += 1.0 / (double)len * points[i].z;
	}
	double vx = temp1x - temp2x * temp2x;
	double vy = temp1y - temp2y * temp2y;
	double vz = temp1z - temp2z * temp2z;

	double maxv = max(vx, max(vy, vz));
	if (maxv == vx) {
		//用split的值分别为1,2,3表示为按照x,y,z方向进行划分超平面
		split = 1;
		sort(points.begin(), points.end(), cmpx);
	}
	else if (maxv == vy) {
		split = 2;
		sort(points.begin(), points.end(), cmpy);
	}
	else {
		split = 3;
		sort(points.begin(), points.end(), cmpz);
	}

	SplitChoice.x = points[len / 2].x;
	SplitChoice.y = points[len / 2].y;
	SplitChoice.z = points[len / 2].z;
}

二、根据二叉树搜索最近邻点 1.距离选择，可以自己定义最近点的距离方式，这里选择的是欧氏距离

//距离公式
double Distance(Point3D point1, Point3D point2) {
	double res = ((double)point1.x - point2.x) * ((double)point1.x - point2.x) + ((double)point1.y - point2.y) * ((double)point1.y - point2.y) + ((double)point1.z - point2.z) * ((double)point1.z - point2.z);
	return sqrt(res);
}

2.构建二叉树

代码如下（示例）：

TreeNode* build_kdtree(vector<Point3D>& points, TreeNode* root) {
	int len = points.size();
	if (len == 0) return NULL;
	int split;
	Point3D dom_elt;
	chooseSplit(points, split, dom_elt);

	vector<Point3D>points_left;
	vector<Point3D>points_right;

	if (split == 1) {
		//说明x方向方差最大，按照x方向进行划分超平面,以超平面为界限递归将数据分为左右子树
		for (int i = 0; i < len; i++) {
			//跳过中点，即选择的为左右划分依据的dom_elt点
			if (equal(points[i], dom_elt)) continue;

			if (points[i].x <= dom_elt.x) {
				points_left.push_back(points[i]);
			}
			else {
				points_right.push_back(points[i]);
			}
		}
	}
	else if (split == 2) {
		for (int i = 0; i < len; i++) {
			if (equal(points[i], dom_elt)) continue;

			if (points[i].y <= dom_elt.y) {
				points_left.push_back(points[i]);
			}
			else {
				points_right.push_back(points[i]);
			}
		}
	}
	else if (split == 3) {
		for (int i = 0; i < len; i++) {
			if (equal(points[i], dom_elt)) continue;

			if (points[i].z <= dom_elt.z) {
				points_left.push_back(points[i]);
			}
			else {
				points_right.push_back(points[i]);
			}
		}
	}

	root = new TreeNode(dom_elt, split, false);
	root->left = build_kdtree(points_left, root->left);
	root->right = build_kdtree(points_right, root->right);
	return root;
}

3.这里有关二叉树的其他函数，例如二叉树的拷贝，二叉树的删除，上述建树过程中使用了关键词new进行申请堆上的空间，程序使用完需要释放掉空间。

深度优先遍历树的所有节点，并将节点值存储到数组中

//深度优先遍历树的所有节点
void dfs(TreeNode* root, vector<Point3D>& points) {
	if (root == NULL) return;
	dfs(root->left, points);
	points.push_back(root->dom_elt);
	dfs(root->right, points);
}

//拷贝二叉树

TreeNode* copyTree(TreeNode* root) {
	TreeNode* node = NULL;
	if (root == NULL) return NULL;
	node = new TreeNode(root->dom_elt, root->split, false);
	node->left = copyTree(root->left);
	node->right = copyTree(root->right);
	return node;
}

//删除二叉树，删除二叉树所占用的空间

void freeTreeRoot(TreeNode* root) {
	if (root->left != NULL) {
		freeTreeRoot(root->left);
	}
	if (root->right != NULL) {
		freeTreeRoot(root->right);
	}
	delete root;
}

---

三、搜索二叉树的最近节点并删除已经选择到的节点 搜索二叉树的最近节点

Point3D searchNearst(TreeNode* root, Point3D target) {
	Point3D res;
	//用栈去存储搜索路径
	stack<TreeNode*> search_path;
	TreeNode* pSearch = root;

	Point3D nearestPoint;
	double dist;
	//递归调用寻找target的最小值，并将路径存到栈中
	searchPath(pSearch, search_path, target);

	//取出栈区的top元素，开始检测并进行回溯
	nearestPoint = search_path.top()->dom_elt;
	search_path.pop();

	dist = Distance(target, nearestPoint);

	TreeNode* pBack;
	while (search_path.size() != 0)
	{
		pBack = search_path.top();
		search_path.pop();
		//判断是否为叶子节点
		if (pBack->left == NULL && pBack->right == NULL) {
			if (Distance(target, pBack->dom_elt) < dist) {
				nearestPoint = pBack->dom_elt;
				dist = Distance(target, pBack->dom_elt);
			}
		}
		else {
			//如果不是叶子节点，搜寻根节点的右子树
			int s = pBack->split;
			if (s == 1) {
				//如果以target为中心的球，半径为dist与分割线相交，则递归到右子树进行搜索
				if (abs(pBack->dom_elt.x - target.x) < dist) {
					if (Distance(target, nearestPoint) > Distance(target, pBack->dom_elt)) {
						nearestPoint = pBack->dom_elt;
						dist = Distance(target, pBack->dom_elt);
					}
					if (target.x <= pBack->dom_elt.x) {
						searchPath(pBack->right, search_path, target);
					}
					else {
						searchPath(pBack->left, search_path, target);
					}
				}
			}
			else if (s == 2) {
				//如果以target为中心的球，半径为dist与分割线相交，则递归到右子树进行搜索
				if (abs(pBack->dom_elt.y - target.y) < dist) {
					if (Distance(target, nearestPoint) > Distance(target, pBack->dom_elt)) {
						nearestPoint = pBack->dom_elt;
						dist = Distance(target, pBack->dom_elt);
					}
					if (target.y <= pBack->dom_elt.y) {
						searchPath(pBack->right, search_path, target);
					}
					else {
						searchPath(pBack->left, search_path, target);
					}
				}
			}
			else if (s == 3) {
				//如果以target为中心的球，半径为dist与分割线相交，则递归到右子树进行搜索
				if (abs(pBack->dom_elt.z - target.z) < dist) {
					if (Distance(target, nearestPoint) > Distance(target, pBack->dom_elt)) {
						nearestPoint = pBack->dom_elt;
						dist = Distance(target, pBack->dom_elt);
					}
					if (target.z <= pBack->dom_elt.z) {
						searchPath(pBack->right, search_path, target);
					}
					else {
						searchPath(pBack->left, search_path, target);
					}
				}
			}
		}

	}

	res = nearestPoint;
	return res;
}

删除二叉树的节点

//删除kd树指定节点，并在相应的数据数组中进行删除
void deleteTreeNode(TreeNode* Oriroot, Point3D target) {
	//先找到最近点对应的子树
	TreeNode* root = Oriroot;
	stack<TreeNode*>stk;
	stk.push(root);
	while (!equal(target, root->dom_elt)) {
		if (root->left && ((root->split == 1 && target.x < root->dom_elt.x) || (root->split == 2 && target.y < root->dom_elt.y) || (root->split == 3 && target.z < root->dom_elt.z)))
		{
			//deleteTreeNode(root->left, target);
			root = root->left;
			stk.push(root);
		}
		if (root->right && ((root->split == 1 && target.x > root->dom_elt.x) || (root->split == 2 && target.y > root->dom_elt.y) || (root->split == 3 && target.z > root->dom_elt.z)))
		{
			//deleteTreeNode(root->right, target);
			root = root->right;
			stk.push(root);
		}
	}

	
	TreeNode* Childroot = stk.top();
	stk.pop();

	if (!root->left && !root->right) {
		if (stk.top()->left != NULL && equal(stk.top()->left->dom_elt, target)) {
			stk.top()->left = NULL;
			delete Childroot;
		}
		else {
			stk.top()->right = NULL;
			delete Childroot;
		}
	}
	else {
		vector<Point3D> pointsleft;
		vector<Point3D> pointsright;
		if (root->left != NULL) {
			dfs(root->left, pointsleft);
		}
		if (root->right != NULL) {
			dfs(root->right, pointsright);
		}
		pointsleft.insert(pointsleft.end(), pointsright.begin(), pointsright.end());
		root = build_kdtree(pointsleft, root);

		if (stk.top()->left != NULL && equal(stk.top()->left->dom_elt, target)) {
			stk.top()->left = root;
		}
		else {
			stk.top()->right = root;
		}
		//删除子树所对应的空间
		freeTreeRoot(Childroot);
	}
}

选取k个最近邻点，上述程序只能选择一个最近邻的点，如果选择目标点中为最近点的集合就很难做到。因为KdTree的删除是一个很难的事情。

一种解决方法如下：

vector<Point3D> searchNearstk(vector<Point3D>& points, Point3D target, int k, TreeNode* Oriroot) {
	vector<Point3D>res;
	int len = points.size();
	TreeNode* root =  NULL;
	root = copyTree(Oriroot);
	if (k > len) {
		cout << "k must not bigger than total training points number!" << endl;
	}
	else
	{
		while (k--)
		{
			Point3D nearestpoint = searchNearst(root, target);
			deleteTreeNode(root, nearestpoint);
			res.push_back(nearestpoint);
		}
	}
	freeTreeRoot(root);
	return res;
}

总结

测试点云在几千点数量级：选取50个最近点，程序运行时间50ms,感觉还可以优化，主要时间浪费在删除搜索树的节点了，原理就是找到要删除的节点，本文采用的方法是，遍历以该节点为根节点的搜索树，存储在数组中，删除该节点下的所有节点，然后重建二叉树并与原来的搜索树进行合并。调试过程中发现，最近点一般在叶子节点或者叶子结点的上一级子树上，此时时间复杂度和空间复杂度比较低。但如果要删除的节点正好是根节点，那么此时消耗的时间与空间数量级增长。