K-D Tree Search Algorithm for SLAM
A K-D Tree (also called as K-Dimensional Tree) is a binary search tree where data in each node is a K-Dimensional point in space. In short, it is a space partitioning for organizing points in a K-Dimensional space. Every non-leaf node can be thought of as implicitly generating a splitting hyperplane that divides the space into two parts, known as half-spaces. Points to the left of this hyperplane are represented by the left subtree of that node and points to the right of the hyperplane are represented by the right subtree.
import numpy as np rng = np.random.RandomState(0) X = rng.random_sample((10, 3)) # 10 points in 3 dimensions tree = KDTree(X, leaf_size=2) dist, ind = tree.query(X[:1], k=3) print(ind) print(dist)
[0 3 1] [ 0. 0.19662693 0.29473397]