We're not done with point clouds

If 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 Via Chen and Yeh, a voxel-based collision-checking scheme. Ching Chen and Tsung-Tai Yeh, two other robotics researchers, decided to fix the problems with CAPTs for themselves. To do so, they started by ditching nearest-neighbor search trees entirely. Instead of with a space-partitioning tree, you can cut up the space into a grid of voxels, each storing a list of points that they contain. The benefit here is twofold: first, you can tell which voxel a query sphere lies in with simple arithmetic, and second, you don’t have to duplicate any points, as finding adjacent voxels is trivial. But naïvely just storing every voxel in the workspace doesn’t work. If the workspace is a hundred voxels long in every dimension, then you’d have to store the information for a million voxels to record a single point cloud, which after filtering only contains a few thousand points. To keep things under control, Chen and Yeh sparsely store only occupied voxels in a three-layer sparse tree, where each layer is segmented by one dimension. Put together with a few axis-aligned bounding box tests, the resulting structure is a multilevel voxel table, or MVT. Like the CAPT, MVTs are parallelizable using single-instruction, multiple-data parallelism (SIMD). For any given voxel, the collision checker can do a big batch check for collision withh all the points contained in the voxel for a free constant speedup. Patching some flat tiers Flat as a board The original implementation of MVTs had some gnarly C++-isms: namely, the voxel tables used a tapestry of pointers to each row of tables. In addition to being kind of unhinged in general, this made memory management quite difficult, and also was not very size-efficient. The original C++ implementation also has a bunch of weird manual pool management, which results in disastrous crashes once point clouds get too big. struct MVT { pointers to voxel indices using ZLevelTable = uint32_t*; pointers to z-level tables using YLevelTable = uint32_t**; pointers to y-level tables using XLevelTable = uint32_t***; XLevelTable x_level_table; } To make things easier to implement in Rust, I simplified things a little bit: we just back everything with a Box<[]>. struct Mvt { tells us where to get voxel data from a grid index tables: Box<[u32]>, tells us where to get point lists in `points` voxels: Box<[u32]>, flattened SoA shared buffer of all point data points: [Box<[f32]>; 3] other fields... } struct Voxel { index of first point stored in this voxel in `points` offset: u32, number of points in the voxel count: u32, other fields... } The search logic then becomes super simple: use tables to find out which voxel you belong to, looked up in voxels. Then use your voxel to find which span of points you need to collision-check against, and finally do a brute-force check against those points. Getting mutable In addition to making the search logic way simpler, the new search structure makes it trivial to make MVTs mutable, just by giving each Voxel its own points field, instead of sharing one big buffer. // `points` is removed from `Mvt` struct Voxel { index of first point stored in this voxel in `points` offset: u32, number of points in the voxel count: u32, SoA buffer of points in this voxel points: [Vec<[f32]>; 3] other fields... } Adding mutability comes at a roughly 2x size penalty and a 1.5x construction-time penalty, but it’s a nice feature to have. To keep good performance for people who use Mvts as a single-use structure, I split out the implementation: I wrote both an immutable default Mvt and a MutableMvt structure. Big balls, big problems The spherization of a Fetch robot (left) and the radii (right) , , and shown in red, green, and purple respectively. In order to build an MVT, you need to pick how big your voxels have to be. If voxels are too big, then collision-checking queries will waste too much time searching through far-away points, but if they’re too close, then queries will instead have to cull against dozens of tiny voxels. There are a few plausible candidates, however. On each robot’s spherized geometry, we can pick out the biggest sphere of the robot, whose radius is . Alternately, we could restrict ourselves to just the moving links of the robot, skipping the big spheres on most robots’ base links, yielding . Lastly, we could take a look at the robot’s bounding-volume hierarchy, and then pick out , the size of the largest sphere ever used in a collision-check. Scaling of query speed with voxel width. Each curve shows the average query time for an MVT generated with the voxel width on the X axis, separated by robot. , , and are all shown marked as ●, ■, and ▲ respectively. The original MVT paper recommended using , largely just by waving generally at query times and claiming that performance was good enough with that selection. However, I wanted to get a better answer than that, so I decided to be empirical. For a simulated workload on every robot, I ran a parameter sweep over the voxel width and recorded the average collision-checking time I then rendered the collision checking performance in the plot shown above. For Fetch, Panda, and UR5, is indeed a respectable choice of voxel width, but not totally optimal. However, for the Baxter robot, I found that using as the voxel width is exceedingly slow, yielding query times twenty times slower than with an optimal selection. I suspect the orignal MVT authors never benchmarked against Baxter, or they would have found this, but in any event I will take my free speedup and carry on. Surprisingly enough, the optimal voxel width for all robots always lands roughly between 10 and 20 cm. I suspect this is a consequence of the point cloud filtering process: for a given point cloud density, there is a roughly optimal voxel size to minimize the amount of wasted work. Going sphere for sphere Naturally, you have to actually benchmark your code to tell if it’s fast. To do so, I whipped together a few fun benchmarks: I solved a bunch of motion planning problems, recorded all of the collision checks that the planners made, and then replayed those collision checks to just time the collision checking throughput. For each problem, I recorded the data structure construction and collision checking time across all the data structures I considered: the MVT implementations (both my Rust code and the original C++ version), my old CAPT implementation, and kiddo, a very fast -d tree. Construction time scaling for each data structure. Each line shows average performance of a data structure for a bucket of point clouds. The most obvious win comes from construction time. CAPTs were always slow to build, and they were especially slow in the Rust implementation. In fact, when I benchmarked my end-to-end planning pipelines, CAPT construction was always the slowest step.