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: this https URL

Robotics (cs.RO); Data Structures and Algorithms (cs.DS)

arXiv:2610.15215 [cs.RO]

(or arXiv:2610.15215v1 [cs.RO] for this version)

https://doi.org/10.48550/arXiv.2610.15215 Focus to learn more arXiv-issued DOI via DataCite (pending registration)

arXivLabs is a framework that allows collaborators to develop and share new arXiv features directly on our website.

Both individuals and organizations that work with arXivLabs have embraced and accepted our values of openness, community, excellence, and user data privacy. arXiv is committed to these values and only works with partners that adhere to them.

Have an idea for a project that will add value for arXiv's community? Learn more about arXivLabs.