nanoflann: A Header-Only KD-Tree Library for Exact Nearest-Neighbor Search on Compile-Time Product Manifolds

Abstract

Nearest-neighbor (NN) search underlies countless robotics algorithms, but many state spaces are not Euclidean: orientations live on $SO(2)$ or $SO(3)$ and poses on $SE(2)$ or $SE(3)$, and a Euclidean KD-tree on their raw coordinates returns wrong neighbors. We present a header-only, dependency-free C++ KD-tree library for exact NN search on user-defined product manifolds declared at compile time: the space is composed from base manifolds ($\mathbb{R}^n$, $SO(2)$, $SO(3)$, $S^n$; products $SE(2)$, $SE(3)$, tori) with a single type alias, and the library synthesizes a metric whose pruning is provably admissible, wrap-aware for circular coordinates and double-cover-aware for unit quaternions. Experiments confirm exactness (recall $1.0$ vs an exhaustive scan under the same product metric), where the Euclidean-on-raw shortcut collapses to recall $\approx 0.5^{m}$ for $m$ rotation blocks, with up to three orders of magnitude speedup over brute force. We further contribute an incremental, self-balancing single-tree index for points under heavy churn (e.g. sliding-window LiDAR maps) or $SE(3)$ trajectories: bounded memory and faster updates than prior work, with an optional asynchronous tail-latency-bounding mode. The implementation is open source: https://github.com/jlblancoc/nanoflann