Collision-Affording Point Trees: SIMD-Amenable Nearest Neighbors for Fast Collision Checking

TL;DR

The paper tackles the real-time collision-checking bottleneck in sensing-driven, high-DOF robot planning by introducing CAPT, a collision-affording point tree that encodes point clouds in an implicit, SIMD-friendly layout with leaf-specific affordance sets. It combines a median-split, Eytzinger-based construction, a space-filling-curve filtering pass, and a branchless query procedure to deliver exact collision results with near-10 ns per query on CPU hardware, and end-to-end planning times under 16 ms on standard benchmarks. The approach is validated through extensive experiments on MotionBenchMaker datasets and real sensor data, showing dramatic improvements over OctoMaps and NN-based methods, including successful dynamic obstacle avoidance at real-time frame rates. The work also discusses limitations (immutability of CAPTs, occlusion handling) and outlines directions for incremental updates and hierarchical fusion with other spatial representations, highlighting strong practical impact for real-time, sensor-based planning on modest hardware.

Abstract

Motion planning against sensor data is often a critical bottleneck in real-time robot control. For sampling-based motion planners, which are effective for high-dimensional systems such as manipulators, the most time-intensive component is collision checking. We present a novel spatial data structure, the collision-affording point tree (CAPT): an exact representation of point clouds that accelerates collision-checking queries between robots and point clouds by an order of magnitude, with an average query time of less than 10 nanoseconds on 3D scenes comprising thousands of points. With the CAPT, sampling-based planners can generate valid, high-quality paths in under a millisecond, with total end-to-end computation time faster than 60 FPS, on a single thread of a consumer-grade CPU. We also present a point cloud filtering algorithm, based on space-filling curves, which reduces the number of points in a point cloud while preserving structure. Our approach enables robots to plan at real-time speeds in sensed environments, opening up potential uses of planning for high-dimensional systems in dynamic, changing, and unmodeled environments.

Figures (4)

• Figure 1: \ref{['subfig:realsense_color']}, \ref{['subfig:realsense_depth']}: A cluttered tabletop scene, captured as RGB-D with an Intel Realsense D455 sensor. \ref{['subfig:realsense_filtered']}: The point cloud rendering of the same scene, filtered using our proposed space-filling curve method (\ref{['sec:filtering']}).
• Figure 2: \ref{['subfig:affordance']}: The cell containing $p_1$ affords $p_2$ and $p_3$ at radius $r_\text{max}$, but not $p_4$ or $p_5$. The axis-aligned bounding box containing all afforded points is depicted in green. \ref{['subfig:rmin']}: The sphere centered at $p_6$ of radius $r_\text{min}$ contains the entire cell, so no other points need to be included in the cell's affordance set.
• Figure 3: \ref{['subfig:pc_unfiltered']}: A point cloud with 126000 points, created by randomly sampling the surface of a shelf. \ref{['subfig:pc_filtered']}: The same point cloud, filtered to 8889 points using $r_\text{filter} = 2 c m$.
• Figure 4: Construction times and average query throughput for pointcloud collision-checking approaches. Methods were evaluated on exemplary pointclouds from each of the 7 benchmark datasets from MotionBenchMaker chamzas2021mbm, and evaluated against the set of all collision queries attempted by the motion planner. On the left, construction times are presented in milliseconds. On the right, average query time in nanoseconds are shown on a logarithmic scale for all queries, only colliding queries, and queries that are not in collision. 99% confidence intervals are shown over 2nd-order polynomial fitted curves for build time and linear log fit curves for query time.

Theorems & Definitions (4)

• Lemma 5.1
• proof
• Lemma 5.2
• proof