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

arXiv:2610.15215v1 Announce Type: new 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 (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

Source: arXiv cs.RO | 2026-10-14