PaperlayerView exact revision on arXiv

Source-linked AI summary

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

Clayton W. Ramsey, Zachary Kingston, Wil Thomason, Lydia E. Kavraki

arXiv:2406.02807v1cs.RO

TL;DR

Collision checking against sensed point clouds remains a bottleneck for motion planning, especially when planners operate on CPUs and obstacle geometry is not explicitly known. The paper presents CAPT, an exact point-cloud structure with space-filling-curve filtering. It reports an order-of-magnitude speedup and millisecond planning on a single CPU core, while CAPT’s immutability limits its use for incremental state-space nearest-neighbor search.

  • Problem

    Point-cloud collision checking remains a slow bottleneck for motion planning, while CPU-based planners face synchronization costs with GPU methods and some field robots may not support onboard GPU power requirements.

    Source: §I · p. 1

  • Method

    CAPT stores point clouds in a collision-affording tree with conservative leaf affordance sets for exact collision checks, alongside a space-filling-curve filtering method.

    Source: §I · p. 1, A. The collision-affording point tree · p. 4

  • Results

    The combined approach enables planning from sensed point clouds in milliseconds on a single CPU core, faster than standard 60 FPS camera refresh rates, with an order-of-magnitude speedup over state-of-the-art techniques.

    Source: §VII · p. 9

  • Takeaways & Limitations

    The authors conclude that sampling-based motion planning can run in real time on purely sensed environments using general-purpose low-power hardware.

    Source: §VII · p. 9

    CAPT is immutable after construction, preventing point insertion or deletion and precluding its use for the state-space nearest-neighbor search required by most sampling-based planners.

    Source: §VII · p. 9

Abstract

from arXiv · show

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.

I. INTRODUCTION

Motion planners often rely on state validation, but collision checking against sensed point clouds remains slow and assumes obstacle geometries are known. The paper introduces CAPT and reports sub-10-nanosecond mean queries on a single CPU core.

I. INTRODUCTION

  • State validation is often motion planning’s most time-consuming component, while point-cloud collision checking remains a slow bottleneck when obstacle geometry is sensed rather than precisely known.Earlier point-cloud collision-checking work [13] [14] attempts to relax the precise-geometry assumption.Source: §I · p. 1

II. RELATED WORK

  • GPU methods use parallelism for point-cloud collision checking, but CPU–GPU synchronization limits direct use in CPU-based planners and onboard GPU power may be untenable for field robots.The paper focuses on CPU SIMD parallelism, while noting possible benefit for GPU SIMT planners.Source: §I · pp. 1–2

I. INTRODUCTION

  • CAPTs return point-cloud collision results in a mean time under 10 nanoseconds per query on one consumer desktop CPU core.The reported throughput clouds contained up to 50,000 points, with mean dispersions from 7 mm to 2.2 cm.Source: §I · p. 1
  • CAPT combines a point-cloud data structure with construction and branch-free parallel query algorithms, space-filling-curve down-sampling, correctness proofs, planner integration, and a hardware demonstration.The paper also evaluates collision throughput and error against competitive baselines.Source: §I · pp. 1–2

II. RELATED WORK

  • Prior SIMD-accelerated sampling-based planning reaches microsecond-scale planning times but still requires an explicit, known model of environment geometry.Recent hardware-accelerated approaches motivate higher-throughput methods for environments represented only by point clouds.Source: §I · p. 1, §II · p. 2

A. Planning from sensor data

Prior sensor-data representations include trees, occupancy maps, and signed-distance fields, while space-filling curves have been used for point-cloud sampling and graph construction. CAPT targets branchless, cache-friendly SIMD collision checking for sensed environments.

A. Planning from sensor data

  • k-d trees support point-cloud nearest-neighbor and collision queries, while occupancy maps and voxel-backed signed-distance fields are also common sensor-data representations.OctoMaps use probabilistic voxel occupancy and bounding-volume hierarchies; GPU-parallel planners also use signed-distance fields.Source: A. Planning from sensor data · p. 2

B. Space-filling curves

  • Z-order curves map points to an ordering useful for subsampling and nearest-neighbor graph construction, but cited point-cloud subsampling does not guarantee preservation of overall structure.Nearby projections on the curve are likely to correspond to nearby points in the higher-dimensional space.Source: B. Space-filling curves · p. 2

C. SIMD parallelism and planning

  • Hierarchical sensor-data structures require branch-heavy searches through fragmented memory, making them poorly suited to branchless SIMD collision checking.The paper presents a data structure designed for branchless, cache-friendly access; Figure 1 depicts a filtered point cloud from a cluttered RGB-D tabletop scene.Source: C. SIMD parallelism and planning · pp. 2–3, Figure 1 · p. 3

III. PRELIMINARIES

The paper starts from k-d-tree partitioning and nearest-neighbor collision checks, then modifies the representation and query to support exact, branch-free parallel checking. CAPT leaves store conservative affordance sets for their cells.

III. PRELIMINARIES

  • A k-d tree recursively partitions point space into axis-aligned cells and can be built by median splits in O(kn log n) time.Its nearest-neighbor search uses branch-and-bound, and checking a sphere for collision amounts to comparing its radius with the nearest point’s distance.Source: §III · p. 3

IV. METHOD

  • CAPT replaces recursive backtracking with cache-coherent storage and a conservative affordance set at each leaf, enabling branch-free, parallel exact collision checking.Each leaf’s affordance set includes points that could collide with a query sphere centered in its cell at radius rmax; the query checks the leaf containing the sphere center.Source: §IV · p. 3, A. The collision-affording point tree · p. 4

A. The collision-affording point tree

  • Figure 2 illustrates that a cell affords points within rmax, while a sufficiently small cell can use only its representative point when every center is within rmin of it.The affordance set is a conservative set of possible colliders, and its points are bounded by a separate axis-aligned box.Source: A. The collision-affording point tree · p. 4, Figure 2 · p. 4
  • A CAPT stores a test sequence, bounding boxes, and affordance sets; its tree uses an Eytzinger array layout for contiguous storage and branch-free traversal.The median partitioning cycles through coordinate axes, and point clouds whose size is not a power of two are padded with points at infinity.Source: A. The collision-affording point tree · p. 4

B. Collision-affording point tree construction

CAPT construction extends recursive median partitioning by propagating and filtering affordance sets. It can prune these sets for sufficiently small cells, and the accompanying example shows substantial point-cloud reduction after filtering.

B. Collision-affording point tree construction

  • 126,000 points are reduced to 8,889 at rfilter = 2 cm in the illustrated shelf point cloud.The construction recursively partitions the cloud, propagates affordance sets across child cells, and filters points that are not afforded.Source: B. Collision-affording point tree… · p. 4, Figure 3 · p. 5
  • Each leaf’s final affordance set is the union of its representative point and the points afforded by that leaf cell.The construction maintains nearby external points that could collide with query spheres centered in the cell.Source: B. Collision-affording point tree… · pp. 4–5
  • If every point in a cell lies within rmin of its representative point, the leaf needs only that representative point in its affordance set.Any query sphere centered in the cell with radius at least rmin already collides with the representative point.Source: B. Collision-affording point tree… · p. 5

C. Filtering

The filtering procedure reduces redundant point-cloud samples while preserving a bound on the gaps it can introduce. It uses space-filling-curve locality to find nearby points efficiently.

C. Filtering

  • A cloud point p may be removed only when another point p* lies less than rfilter away, guaranteeing robot penetration into the cloud is limited to rfilter; alternatively, pad robot spheres by rfilter.Source: C. Filtering · p. 5
  • The algorithm maps points to a space-filling curve and checks neighboring points, reducing filtering to O(n log n) time.This avoids the O(n^2) cost of comparing every pair of points; the implementation uses Z-order curves.Source: C. Filtering · p. 5
  • Because nearby points may not be adjacent on one curve, filtering repeats across each permutation of dimensions, costing O(k!n log n).For k = 3, the six permutations add relatively little cost compared with the point-cloud reduction.Source: C. Filtering · p. 5
  • For fixed-base arm robots, points farther from the base link than the arm’s maximum extension can be discarded as non-colliding.The filter must also be computationally cheaper than CAPT construction to yield useful speedup.Source: C. Filtering · p. 5
  • Clouds with over a hundred thousand points can be reduced to fewer than ten thousand in a few milliseconds.The reduction also lowers point density, which reduces expected colliding-set size and can improve CAPT construction and query times.Source: C. Filtering · p. 5

D. Collision querying

Collision querying tests the robot’s spheres against the tree, using parallel checks and early termination to accelerate the search. The query first finds a cell, then rejects or checks its associated points.

D. Collision querying

  • A configuration is invalid if any robot sphere collides, so parallel checks across query spheres can stop as soon as one collision is found.Only one colliding sphere is needed to invalidate the configuration.Source: D. Collision querying · pp. 5–6
  • Branchless tree traversal updates indices from coordinate comparisons and runs a fixed number of iterations, enabling SIMD lanes to search in parallel.The predictable memory-access pattern also supports efficient parallel evaluation.Source: D. Collision querying · p. 6
  • A query sphere is first checked against its cell’s bounding box; spheres that cannot overlap are masked out, and the search ends if none remain.This inexpensive test avoids point checks for spheres provably outside the cell’s affordance set.Source: D. Collision querying · p. 6
  • The final check exhaustively tests the remaining spheres against their cell’s points, parallelizing across contiguous point sets for cache locality.This parallelization is possible because the search identifies the points associated with each cell.Source: D. Collision querying · p. 6

A. Runtime

CAPT construction and query costs depend on the size of the cell-associated point sets. The correctness results preserve collision status and prevent filtering from creating invalid gaps when its radius is appropriately bounded or query spheres are padded.

A. Runtime

  • CAPT construction costs O(kn log n + kna) time and O(kna) memory, reaching O(kn^2) when low dispersion makes each cell afford all n points.Here a is the maximum affordance-set size; high-dispersion clouds tend to have smaller a, yielding O(kn log n) construction time.Source: A. Runtime · p. 6
  • Each query takes O(log n + ka) steps: O(log n) tree comparisons followed by checks of up to a points.The point-check count depends on the largest affordance set.Source: A. Runtime · p. 6

B. Correctness

  • For a cell’s point set, a sphere centered in that cell collides with the point cloud exactly when it collides with that set.This establishes that CAPT preserves the collision status of query spheres.Source: B. Correctness · p. 6
  • Filtering introduces no gap larger than 2rmin when rfilter ≤ rmin − δ(O, PC).The bound follows because retained points stay within rfilter of removed points, while the cloud’s dispersion is δ(O, PC).Source: B. Correctness · p. 7
  • Padding query radii by rfilter compensates for a reduced point-cloud envelope; with this padding, any rfilter may be used.Without the appropriate padding, filtering can somewhat reduce the envelope of the point cloud.Source: B. Correctness · p. 7
  • The lemmas preserve planning-problem feasibility when using CAPT and filtering, with the filtering guarantee subject to correct radius padding.Empirically, aggressive padding or small rfilter can be unnecessarily conservative and detrimental to CAPT construction performance.Source: B. Correctness · p. 7

VI. EXPERIMENTS

The experiments benchmark collision-checking throughput against six implementations and test planning across robots and scenes. The supplied passages describe the evaluation setup but do not report comparative results.

VI. EXPERIMENTS

  • Collision-checking throughput was benchmarked on a 4.5GHz AMD Ryzen 9 7950X against six implementations, including voxel, k-d tree, hyperplane-tree, and approximate-neighbor methods.The CAPT implementation was integrated with a vector-accelerated motion-planning framework and compared with sequential-query variants.Source: §VI · p. 7
  • Planning was benchmarked on three robots across seven MotionBenchMaker scenes, with 100 planning problems per scene.The robots were the 6-DOF UR5, 7-DOF Panda, and 8-DOF Fetch.Source: §VI · p. 7
  • Planning used a single thread, a dynamic-domain balanced RRT-Connect planner, and the same sampled configurations across comparisons.The planner was limited to one million iterations, controlling for differences in sampling order.Source: §VI · p. 7

1) Collision query throughput:

The CAPT is evaluated for point-cloud collision-query throughput and motion-planning performance against alternative backends. Its fast queries support near-real-time planning, while construction and filtering become important costs.

1) Collision query throughput:

  • Throughput tests replayed the same planner queries on the same filtered point clouds, with collision-checking timings isolated from other planning overhead.CAPTs used rmin = 1 cm and rmax = 8 cm; experiments ran on one CPU thread.Source: 1) Collision query throughput · pp. 7–8

2) Motion planning performance:

  • The motion-planning comparison used the same planning system and single-thread CPU tests, attributing speed differences to collision checking rather than sampling order or other planner-efficiency factors.The comparison included sequential and SIMD CAPT queries, OctoMaps, and NanoFLANN, with point clouds filtered at rfilter = 2 cm.Source: 2) Motion planning performance · p. 8

1) Collision query throughput:

  • CAPT construction is slower than other tree-based nearest-neighbor structures but faster than OctoMap, and its superlinear scaling makes point-cloud filtering necessary.Figure 4 reports construction times alongside query times for all, colliding, and non-colliding queries.Source: 1) Collision query throughput · p. 8, Figure 4 · p. 8
  • 9.89 ns per query for CAPT compares with 309 ns per query for NanoFLANN; OctoMap averages 0.01 ms per query.These are average collision-query times reported for the evaluated approaches.Source: 1) Collision query throughput · p. 8, Figure 4 · p. 8

2) Motion planning performance:

  • For the UR5 and Panda, the 95th-percentile end-to-end time is under 16 ms, with filtering and CAPT construction accounting for the largest share.The end-to-end measure includes filtering, building the CAPT, planning, and simplification.Source: Table I · p. 8, 2) Motion planning performance · p. 9

B. Planning from real sensor data

On a real RGB-D scene, the system planned from a filtered point cloud in milliseconds and operated at or above the camera’s 60 FPS rate in a live obstacle-dodging loop. The conclusion notes limits from CAPT immutability and affordance-set duplication.

1) Static scene:

  • In the static UR5 scene, filtering reduced 166,587 points to 2,732, and end-to-end filtering, construction, planning, and simplification took a median 7.166 ms.The simplified path was returned in 575 microseconds, corresponding to roughly 140 Hz for the full pipeline.Source: 1) Static scene · p. 9, B. Planning from real sensor data · p. 9

2) Dynamic scene:

  • In the live UR5 control loop, replanning on each new point cloud ran at or above 60 FPS and qualitatively enabled reactive dodging of unmodeled obstacles.The test used human-moved pool noodles and did not use motion forecasting or obstacle modeling.Source: 2) Dynamic scene · p. 9

VII. CONCLUSION

  • The paper concludes that CAPT collision checking and point-cloud filtering support millisecond planning from sensed environments on a single CPU core, faster than a 60 FPS camera refresh.The authors frame this as enabling real-time sampling-based planning on low-power general-purpose hardware.Source: §VII · p. 9
  • CAPT immutability prevents inserting or deleting points after construction, and affordance-point duplication limits scaling to massive point clouds such as autonomous-vehicle data.The paper suggests incremental updating and compressed or de-duplicated affordance sets as future directions.Source: §VII · p. 9

APPENDIX

Filter-radius experiments found that Panda planning remained faster than the baselines across tested radii, while dispersion measurements inform radius selection. In the UR5 experiment, trajectories were valid for radii up to 2 cm, with fidelity traded against performance.

B. Empirical dispersion of point clouds

C. Real-robot planning experiments

A. Empirical impact of filter radii

Loading 2406.02807v1…