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. 我写这篇文章是为了引起大家对他们工作的关注，同时也带点私心地想聊聊我在重新实现他们的工作时学到的东西。简而言之，他们设计了一种用于点云碰撞检测的数据结构，该结构运行速度极快，且在内存占用和构建时间上都非常高效。如果你不关心细节，可以直接跳转到论文或原始的 C++ 实现。我也发布了一个带有我个人优化的 Rust 实现，源代码托管在 GitHub 上，包已发布在 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. 一台 Franka Emika Panda 机器人及其球体化的碰撞检测表示。我花了很多时间思考运动规划：寻找让机器人从起始状态到目标状态找到无碰撞运动的方法。解决运动规划问题的方法有上百万种，但当你读过足够多的论文后，它们看起来都大同小异。你采样一些配置，测试它们是否有效，并尝试在所有可能的配置中进行大规模的路径搜索。

Every one of those algorithms requires configuration validation: given a robot’s configuration $q$, determine whether a robot in position $q$ 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. 每一个算法都需要配置验证：给定机器人的配置 $q$，确定处于位置 $q$ 的机器人是否与世界几何体发生碰撞。由于机器人通常在感知到的环境中工作，这些世界几何体通常以点云的形式呈现。如果我们将机器人的几何形状简化为一堆球体，我们就可以进一步将问题简化为球体碰撞检测：对于任何配置，只需检查机器人上的任何球体是否与感知到的点云发生碰撞。

Problem statement: Given some list of points $P$ and a set of spheres $S$, determine whether any sphere in $S$ collides with $P$ in minimal time. 问题陈述：给定一些点列表 $P$ 和一组球体 $S$，确定 $S$ 中的任何球体是否在最短时间内与 $P$ 发生碰撞。

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 $k$-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 $k$-d tree with a batch-parallel search algorithm, supporting SIMD-accelerated branchless queries. 几年前，我提出了一种名为 CAPT 的数据结构，旨在使针对点云的配置验证变得非常快。简而言之，它是一个球体与点云之间的碰撞检测器。它是一种类似于 $k$-d 树的最近邻搜索结构，但我们在构建时做了额外的工作，以避免在搜索树中进行回溯。最终的结果是，我们拥有了一个带有批量并行搜索算法的 $k$-d 树，支持 SIMD 加速的无分支查询。

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 $O(N^2)$, 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. CAPT 的大问题在于构建时间：稠密点云需要大量重复数据来避免回溯。一旦点云变得足够稠密，CAPT 的构建复杂度就会达到 $O(N^2)$，这对于希望在控制循环频率下进行规划的用户来说是灾难性的。CAPT 的数据布局要求搜索树的每个叶子节点（代表空间中的某个区域）存储点云中许多点的重复副本。这些重复副本开始占据数据结构的主要空间，进而导致构建时间激增。

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. 通过 Chen 和 Yeh 的基于体素的碰撞检测方案。Ching Chen 和 Tsung-Tai Yeh 这两位机器人研究人员决定自行解决 CAPT 的问题。为此，他们首先完全放弃了最近邻搜索树。与其使用空间划分树，不如将空间切割成体素网格，每个体素存储其包含的点列表。

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. 这样做有两个好处：首先，你可以通过简单的算术运算判断查询球体位于哪个体素中；其次，你不需要复制任何点，因为查找相邻体素非常简单。但是，天真地存储工作空间中的每个体素是行不通的。如果工作空间在每个维度上都有 100 个体素长，那么为了记录一个点云，你就必须存储一百万个体素的信息，而这些点云在过滤后可能只有几千个点。

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 with all the points contained in the voxel for a free constant speedup. 为了控制规模，Chen 和 Yeh 采用稀疏存储方式，仅在三层稀疏树中存储被占用的体素，每一层按一个维度进行分割。结合一些轴对齐包围盒（AABB）测试，最终形成的结构就是多级体素表（MVT）。与 CAPT 一样，MVT 可以使用单指令多数据流（SIMD）进行并行化。对于任何给定的体素，碰撞检测器可以对体素内包含的所有点进行大规模批量碰撞检查，从而获得免费的常数级加速。

Patching some flat tiers

扁平化层级

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. MVT 的原始实现包含一些棘手的 C++ 特性：即体素表使用了指向每一行表的指针网络。这不仅在总体上显得有些混乱，还使得内存管理变得非常困难，且空间效率不高。原始的 C++ 实现还有一堆奇怪的手动内存池管理，一旦点云变得过大，就会导致灾难性的崩溃。

To make things easier to implement in Rust, I simplified things a little bit: we just back everything with a Box<[]>. 为了让在 Rust 中实现变得更容易，我稍微简化了一下：我们直接用 Box<[]> 来承载所有数据。

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. 搜索逻辑变得非常简单：使用表来确定你属于哪个体素，并在 voxels 中查找。然后使用你的体素来找到需要进行碰撞检查的点范围，最后进行暴力检查。