GoKawiilIf you wait long enough to solve a problem, someone else might just solve it for you. At least that’s what I tell myself about the dishes in my sink.
While in Vienna for a conference, I found another set of researchers who did just that for me: they took some work I had published two years ago and ran with it, and they beat me on just about every benchmark. I’m writing up this article to draw some attention to their work and, a little selfishly, to yap about the things I learned while reimplementing their work. In short, they made a data structure for collision-checking against point clouds that runs really fast while also being extremely cheap in memory and construction time.
If you don’t care about details, you can jump straight to the paper or to the original C++ implementation. I’ve also published a Rust implementation with my own optimizations, with source code on GitHub and a package on crates.io.
Recapt
A Franka Emika Panda robot and its spherized collision-checking representation.
I spend a lot of my time thinking about motion planning: finding ways for robots to find collision-free motions from a start state to a goal state. There are a million different ways to solve motion planning problems, but once you’ve read enough papers they all kind of look the same. You sample some configurations, test if they’re valid, and try to do a big path search over all possible configurations. Every one of those algorithms requires configuration validation: given a robot’s configuration , determine whether a robot in position collides with the world geometry. Since robots often work in perceived environments, that world geometry typically comes to us as a point cloud. If our robot’s geometry is simplified to a bunch of spheres, we can further simplify the problem to spherical collision checking: for any configuration, just check if any of the spheres on the robot collide with the perceived point cloud.
Problem statement: Given some list of points and a set of spheres , determine whether any sphere in collides with in minimal time.
A few years ago, I proposed a data structure called the CAPT, which is designed to make configuration validation against point clouds really fast. In short, it’s a collision-checker between spheres and point clouds. It’s a nearest-neighbor search structure, much like a -d tree, but we do extra work at construction time to avoid backtracking through the search tree. The net result is that we have a -d tree with a batch-parallel search algorithm, supporting SIMD-accelerated branchless queries.
The big problem with CAPTs was the construction time: dense point clouds require a lot of duplicated data to avoid backtracking. Once point clouds get dense enough, CAPT construction scales at , which is disastrous for a user’s hopes of getting planning at control-loop frequencies. The data layout for CAPTs requires each leaf of the search tree, which represents some region in space, to store duplicate copies of many points in the point cloud. Those duplicate copies start to dominate the data structure’s footprint, which in turn balloons construction time.
Thinking inside the box
... continue reading