The kd tree (k-dimensional tree) is a binomial tree data structure containing spatial information, which is a very common tool used to calculate kNN. If the dimension of the feature is D and the number of samples is N, then generally speaking, the complexity of the kd tree algorithm is O(D log(N)), which saves a lot of calculation compared to the poor calculation O(DN) quantity.
1 Build KD tree
A kd-tree is a binary tree in which each node is a k-dimensional point, and all non-leaf nodes can be regarded as a hyperplane to divide the space into two half-spaces (Half-space). Because there are many ways to choose axis-aligned splitting planes, there are many ways to create kd-trees. The most typical methods are as follows:
-
Dimension selection: KD tree construction uses the n-dimensional features of m samples to calculate the variance of the values of n features, and uses the k-th dimensional feature with the largest variance as the root node.
-
Select the median: For this feature, we select the sample corresponding to the median v of the value of the feature as the dividing point
-
Split data: For all samples whose value of the k-th dimension feature is less than v, we divide it into the left subtree, and for samples whose value of the k-th dimension feature is greater than or equal to v, we divide it into the right subtree.
-
Recursive iteration: For the left subtree and right subtree, we use the same method as before to find the feature with the largest variance to make more nodes, and further subdivide the space and data sets until all points are divided.
Let's take an example to build a KD tree for the following data points
The specific steps to build a KD tree are:
-
Determine the segmentation dimension: the data variances of the 6 data points on the x and y dimensions are 39 and 28.63 respectively, and the variance on the x-axis is larger, so the segmentation dimension is x.
-
Determine the split data point: Sort the data according to the value on the x dimension, the median value (that is, the value of the middle size) of the 6 data is 7, so the split data point is (7,2). Thus, the splitting hyperplane of this node is the straight line x=7.
-
The left subtree space and the right subtree space are recursive respectively
The split hyperplane x=7 divides the entire space into two parts: the part of x<=7 is the left subspace, which contains 3 nodes ={(2,3),(5,4),(4,7)}; The other part is the right subspace, which contains 2 nodes = {(9,6), (8,1)}; then the left and right subspaces repeat the above process respectively.
2 Nearest Neighbor Search
The most typical application of kd-tree is the nearest neighbor search, which is of great significance in data search and kNN.
The nearest neighbor search is used to find the point closest to the target point in the tree. The algorithm principle of the kd tree nearest neighbor search is as follows:
-
Starting at the root node, move down recursively. If the target point is on the left side of the partition surface, enter the left child node, and on the right side, enter the right child node, and mark the passed nodes;
-
Before approaching the leaf node, if there is no child node when moving to the left or right, it is forced to enter the child node on the other side to ensure that the leaf node must be finally reached; once it moves to the leaf node, the Nodes are considered "current nearest neighbors".
-
According to the previous node record, backtrack from the leaf node, and recursively perform the following steps for each node passed:
1) If the current node is closer to the input point than the "current nearest neighbor point", it will be changed to the current nearest neighbor point, and its distance is the shortest distance.
2) Check if there is a closer point in the subtree of the current node, and if so, look down from the node. More specifically, (whether you need to search for a subtree on a certain side) check whether the current segmentation hyperplane intersects with "the target point is the center of the sphere, and the distance between the target point and the "current nearest neighbor" is the radius" .
a) If they intersect, there may be a point closer to the target point in the corresponding area of the subtree, and move to the subtree according to the previous step 1.2 until its leaf node;
b) If not intersected, rollback upwards.
-
When falling back to the root node, the search ends. The last "current nearest neighbor" is the nearest neighbor.
The following distances illustrate how the nearest neighbor search works. As shown in the figure below, the target points are the green query points in the figure. Now we need to find the closest point to this target point in the kd-tree.
According to the rules described by the algorithm, first search from the root node, because the target point is located on the left side of the hyperplane determined by point 1, so enter the left child node. Next, because the target point is above point 3, enter the right child of point 3. Since point 6 is a leaf node, treat this node as the "current nearest neighbor" (if there is no 6 node (empty node) at this time, force to enter the opposite node (point 4), and then until a leaf node is found), and Calculate its distance from the target point as the minimum distance.
Then, look back. After node 3, calculate the distance from the target to point 3, and find that the current point 3 is closer to the target point than the "current nearest point" 6, then change 3 to the current nearest point. At this point, you need to consider whether to enter the subtree on the other side of 3 o'clock.
Because we want to discuss whether there is a better nearest neighbor in the left subtree of node 3, and the area corresponding to the left subtree of node 3 is below the segmentation hyperplane determined by node 3. So it depends on whether the ball (or circle) with the target point as the center and the distance from the target point to point 3 as the radius intersects with the area below the segmentation hyperplane determined by node 3. So according to the algorithm description, we check whether the segmentation hyperplane determined by 3 intersects with "the hypersphere whose center is the target point x and whose radius is the distance between the target point x and the "current nearest neighbor point" 3". Obviously intersect, so the left subtree of node 3 needs to be searched.
The left subtree of node 3 is identified by the blue triangle on the right in the figure above. For the node 4 passing through now, the calculated distance from point x to point 4 is obviously greater than the current distance, so there is no need to update the "current nearest neighbor". But we also need to consider whether to continue searching the left and right subtrees of node 4. So we check whether the segmentation hyperplane determined by 4 intersects with "the hypersphere with the target point x as the center of the sphere and the distance between the target point x and the "current nearest neighbor point" 3 as the radius". Obviously intersect, so the left and right subtrees of node 4 need to be searched (non-backtracking process). But its right subtree is empty, so don't consider it.
For the left subtree of node 4, calculate the distance from point 5 to the target point x, and find that it is shorter than the previous distance (from 3 to x), so update 5 as the "current nearest neighbor point".
Then the algorithm rolls back to node 4, to node 3, the left and right subtrees of node 3 and itself have been inspected, and continues to roll back to node 1. For node 1, its left subtree has been inspected, and we consider whether to detect its right subtree. Check whether the segmentation hyperplane determined by 1 intersects the "hypersphere with the target point x as the center and the distance between the target point and the "current nearest neighbor" 5 as the radius". Obviously disjoint, so there is no need to search the right subtree of node 1. Finally, calculate the distance from point 1 to the target point x, and find that it is not shorter than the previous distance (from 5 to x), so the nearest neighbor point is not updated. Because the root node has been rolled back, the search ends. The final "current nearest neighbor" 5 is the nearest neighbor of x.
3 Area-wide search
The main purpose of area (range) search is to find all the points located in a given area (preferably a regular graphic: rectangle, circle, etc., which is convenient for detecting whether a point is in it). It is mainly achieved by performing the following three steps:
Starting from the root node, first determine whether the root node is in a given area;
-
If the root node is completely on the left/lower side of the given area (judging according to the partition dimension), recurse with the right subtree of the root node;
-
If the root node is completely on the right/upper side of the given area (judged by the partition dimension), recurse with the left subtree of the root node;
-
If the root node straddles within a given area (judged according to the partition dimension), recurse with the left and right subtrees of the root node;
Take a look at the example below. Starting from the root node, first check whether point 1 is in the rectangle, the answer is not. So continue to check its subtrees. Because the rectangular box is on the left side of point 1 (that is, the point in the box is smaller than point 1 in the x-axis direction), then continue to search its left subtree, and ignore its right subtree directly.
Next check point 3 (that is, the child node of point 1), the answer is not. So continue to (recursively) examine its subtrees. But because the rectangular box spans the upper and lower subspaces divided by the hyperplane passing through point 3, it is necessary to continue searching its left and right subtrees.
Searching recursively like this, you will finally find that point 5 is located in the rectangle.
In standard cases, the time complexity of Range Search is R+logN, where R is the number of points to return and N is the total number of points in the space. In the worst case, the time complexity of Range Search is R+sqrt(N).
4 PLC example
In the following case, two methods are used to perform neighborhood search. The first method is to search for the nearest K neighbors, and the second method is to search for neighbors by specifying a radius.
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <iostream>
#include <vector>
#include <ctime>
//#include <pcl/search/kdtree.h>
//#include <pcl/search/impl/search.hpp>
#include <pcl/visualization/cloud_viewer.h>
int
main(int argc, char **argv) {
// 用系统时间初始化随机种子
srand(time(NULL));
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
// 生成点云数据1000个
cloud->width = 1000;
cloud->height = 1; // 1 表示点云为无序点云
cloud->points.resize(cloud->width * cloud->height);
// 给点云填充数据 0 - 1023
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);
}
// 创建KdTree的实现类KdTreeFLANN (Fast Library for Approximate Nearest Neighbor)
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
// pcl::search::KdTree<pcl::PointXYZ> kdtree;
// 设置搜索空间，把cloud作为输入
kdtree.setInputCloud(cloud);
// 初始化一个随机的点，作为查询点
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);
// K nearest neighbor search
// 方式一：搜索K个最近邻居
// 创建K和两个向量来保存搜索到的数据
// K = 10 表示搜索10个临近点
// pointIdxNKNSearch 保存搜索到的临近点的索引
// pointNKNSquaredDistance 保存对应临近点的距离的平方
int K = 10;
std::vector<int> pointIdxNKNSearch(K);
std::vector<float> pointNKNSquaredDistance(K);
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
<< " (距离平方: " << pointNKNSquaredDistance[i] << ")" << std::endl;
}
// Neighbors within radius search
// 方式二：通过指定半径搜索
std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSquaredDistance;
// 创建一个随机[0,256)的半径值
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
<< " (距离平方:: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
}
pcl::visualization::PCLVisualizer viewer("PCL Viewer");
viewer.setBackgroundColor(0.0, 0.0, 0.5);
viewer.addPointCloud<pcl::PointXYZ>(cloud, "cloud");
pcl::PointXYZ originPoint(0.0, 0.0, 0.0);
// 添加从原点到搜索点的线段
viewer.addLine(originPoint, searchPoint);
// 添加一个以搜索点为圆心，搜索半径为半径的球体
viewer.addSphere(searchPoint, radius, "sphere", 0);
// 添加一个放到200倍后的坐标系
viewer.addCoordinateSystem(200);
while (!viewer.wasStopped()) {
viewer.spinOnce();
}
return 0;
}The resulting graph is as follows:
