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回顾

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.几年前，我提出了一种名为 CAPT 的数据结构，旨在使针对点云的配置验证变得非常快。 简而言之，它是一个球体与点云之间的碰撞检测器。 它是一种类似于 -d 树的最近邻搜索结构，但我们在构建时做了额外的工作，以避免在搜索树中回溯。 最终结果是，我们拥有了一个带有批量并行搜索算法的 -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 , 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 的构建扩展性就会达到 ，这对于希望以控制循环频率进行规划的用户来说是灾难性的。 CAPT 的数据布局要求搜索树的每个叶子节点（代表空间中的某个区域）存储点云中许多点的重复副本。 这些重复副本开始占据数据结构的大部分空间，进而导致构建时间呈指数级增长。

Thinking inside the box跳出框框思考

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.Ching Chen 和 Tsung-Tai Yeh，另外两位机器人研究人员，决定自行解决 CAPT 的问题。 为此，他们首先完全放弃了最近邻搜索树。 与其使用空间划分树，不如将空间切割成体素网格，每个体素存储其所包含的点列表。 这样做的好处有二：首先，你可以通过简单的算术运算知道查询球体位于哪个体素中；其次，由于查找相邻体素非常容易，你无需复制任何点。

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.但天真地存储工作空间中的每一个体素是行不通的。 如果工作空间在每个维度上都有 100 个体素长，那么为了记录一个点云，你就必须存储 100 万个体素的信息，而这些点云在过滤后可能只包含几千个点。 为了控制规模，Chen 和 Yeh 在一个三层稀疏树中稀疏地存储被占用的体素，每一层由一个维度进行分割。

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.结合一些轴对齐包围盒 (AABB) 测试，最终的结构就是多级体素表 (MVT)。 与 CAPT 一样，MVT 可以使用单指令多数据流 (SIMD) 进行并行化。 对于任何给定的体素，碰撞检测器可以进行大规模批量检查，以检测与体素内包含的所有点的碰撞，从而获得免费的恒定速度提升。

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.MVT 的原始实现有一些令人头疼的 C++ 特性：即体素表使用了指向每一行表的指针挂毯。 这不仅在总体上显得有些疯狂，而且使内存管理变得相当困难，且空间效率不高。 原始的 C++ 实现还有一堆奇怪的手动池管理，一旦点云变得过大，就会导致灾难性的崩溃。

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<[]>.为了让在 Rust 中的实现更容易，我简化了一些东西：我们直接用 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.除了使搜索逻辑变得简单得多之外，新的搜索结构还使得 MVT 的可变性变得轻而易举，只需给每个体素提供它自己的点字段，而不是共享一个大缓冲区。

// `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.增加可变性会带来大约 2 倍的内存开销和 1.5 倍的构建时间开销，但这确实是一个不错的功能。 为了保持那些将 MVT 作为一次性结构使用的用户的良好性能，我拆分了实现：我编写了一个不可变的默认 Mvt 和一个 MutableMvt 结构。

Big balls, big problems大球，大问题

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.为了构建 MVT，你需要选择体素的大小。 如果体素太大，碰撞检测查询将浪费太多时间搜索远处的点；但如果体素太小，查询则必须针对数十个微小的体素进行剔除。

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.不过，有几个合理的候选方案。 在每个机器人的球体化几何体上，我们可以挑选出机器人最大的球体，其半径为 。 或者，我们可以仅限于机器人的运动连杆，跳过大多数机器人基座连杆上的大球体，得出 。 最后，我们可以查看机器人的包围体层次结构，然后选出 ，即碰撞检测中使用的最大球体的尺寸。

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.原始 MVT 论文建议使用 ，这在很大程度上只是通过对查询时间进行泛泛的评估并声称该选择下的性能足够好来确定的。 然而，我想要一个比这更好的答案，所以我决定进行实证研究。 对于每个机器人的模拟工作负载，我运行了一个关于体素宽度的参数扫描，并记录了平均碰撞检测时间。 然后，我将碰撞检测性能绘制在上面的图中。

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.对于 Fetch、Panda 和 UR5， 确实是一个不错的体素宽度选择，但并不完全是最优的。 然而，对于 Baxter 机器人，我发现使用 作为体素宽度非常缓慢，查询时间比使用最优选择慢了 20 倍。 我怀疑原始 MVT 作者从未针对 Baxter 进行过基准测试，否则他们就会发现这一点，但无论如何，我将享受我免费获得的速度提升并继续前进。

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.令人惊讶的是，所有机器人的最优体素宽度总是落在 10 到 20 厘米之间。 我怀疑这是点云过滤过程的结果：对于给定的点云密度，存在一个大致最优的体素大小，可以最大限度地减少浪费的工作量。

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.当然，你必须实际对代码进行基准测试才能知道它是否真的快。 为此，我整理了一些有趣的基准测试：我解决了一堆运动规划问题，记录了规划器进行的所有碰撞检查，然后重放这些碰撞检查以仅计时碰撞检测的吞吐量。 对于每个问题，我记录了我所考虑的所有数据结构的构建和碰撞检测时间：MVT 实现（我的 Rust 代码和原始 C++ 版本）、我旧的 CAPT 实现以及 kiddo（一种非常快的 -d 树）。

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. Since MVTs don’t do nearly as much bookkeeping at construction time, they get a big performance win. Also, since the MVT has linear memory scaling, its construction time is on the order of with point cloud size , instead of for space-partitioning trees, meaning construction times are great even in large point clouds.最明显的胜利来自构建时间。 CAPT 的构建速度一直很慢，在 Rust 实现中尤其慢。 事实上，当我为端到端规划流水线进行基准测试时，CAPT 的构建总是最慢的步骤。 由于 MVT 在构建时不需要进行那么多的簿记工作，它们获得了巨大的性能提升。 此外，由于 MVT 具有线性内存缩放特性，其构建时间随点云大小 的变化呈 级别，而不是空间划分树的 ，这意味着即使在大型点云中，构建时间也非常出色。

Building mutable MVTs comes at some construction cost, since we have to maintain many separate allocations instead of one big pool, but even then, it’s still pretty cheap.构建可变的 MVT 需要一些构建成本，因为我们必须维护许多单独的分配而不是一个大池，但即便如此，它仍然非常便宜。

Even better, MVTs have great query throughput. I had been quite proud of the ten-nanosecond scale throughput for CAPTs, but MVTs manage to do even better. Even more surprisingly, mutable MVTs seem to have marginally better query performance than immutable ones. Most likely, the performance bump comes from some quirk of cache memory: perhaps laying points out in distinct allocations helps.更好的是，MVT 具有极佳的查询吞吐量。 我曾为 CAPT 的十纳秒级吞吐量感到相当自豪，但 MVT 表现得更好。 更令人惊讶的是，可变的 MVT 的查询性能似乎比不可变的略好。 最有可能的原因是缓存内存的一些特性：也许将点放在不同的分配中会有所帮助。

When solving real motion planning problems, MVTs give a respectable speedup across the board. In the above plot, we see that for nearly every problem, planning with the MVT point cloud representation is faster than with a CAPT, and often is even faster than the ground-truth primitive geometry representation.在解决实际运动规划问题时，MVT 全面提供了可观的速度提升。 在上图中，我们可以看到，对于几乎每一个问题，使用 MVT 点云表示进行规划都比使用 CAPT 更快，而且通常甚至比使用地面真值原始几何体表示还要快。

The big problems still aren’t solved大问题仍未解决

MVTs are pretty cool data structures! They’re fast, cheap, and easy to manage. I also had a blast implementing my Rust version of them. However, they only fix one of the big problems with CAPTs, which is their construction time.MVT 是非常酷的数据结构！ 它们快速、廉价且易于管理。 我在实现 Rust 版本时也玩得很开心。 然而，它们只解决了 CAPT 的一个大问题，即构建时间。

Point cloud data is necessarily imperfect: it comes out of a camera, and that camera can only ever see one side of an object. When we plan for real robots, we have to account for this, typically by assuming that unseen space is also invalid. Previous work on perception data, such as octomaps, can do just this, but I had sacrificed occlusion handling for the CAPT work at the altar of performance. Now, users are left with the cruel tradeoff of being safe or being fast: octomaps are wicked slow but so far are the only ones handling occlusion.点云数据必然是不完美的：它来自摄像头，而摄像头只能看到物体的一面。 当我们为真实的机器人进行规划时，我们必须考虑到这一点，通常假设未见过的空间也是无效的。 之前关于感知数据的工作（如 octomaps）可以做到这一点，但我为了性能在 CAPT 工作中牺牲了遮挡处理。 现在，用户面临着安全与速度之间的残酷权衡：octomaps 速度极慢，但到目前为止是唯一能处理遮挡的方案。

In a more general sense, I think the classical formulation of motion planning, where a robot gets to operate in a beautiful still-life painting of the world, is just incorrect. Even the best perception for robots is never better than “bad,” and so our approaches to planning should contend with the fact that we only ever plan against an approximation of the real world. So, much like my dishes, planning from perception remains yet unsolved.从更广泛的意义上讲，我认为运动规划的经典表述——即机器人可以在一幅精美的世界静物画中进行操作——是不正确的。 即使是机器人最好的感知也永远不会比“糟糕”更好，因此我们的规划方法应该承认这样一个事实：我们只是在针对真实世界的近似值进行规划。 所以，就像我的脏碗一样，基于感知的规划仍然悬而未决。