『解读』TEASER | 快速且可证明的点云配准算法和代码解读

来源深蓝学院：深蓝前沿教育

本文是对文章《TEASER：Fast and Certifiable Point Cloud Registration》的解读。

摘要

这篇文章提出了第一个快速且可证明的算法，用于存在大量外点对应的情况下两组3D点的配准。

可证明的算法尝试求解一个困难优化问题(比如带外点的鲁棒估计)，提供相对容易的检测条件验证返回的解是否最优(比如，如果算法在外点存在情况下产生最精确的估计)或者界限解的次优性或精确性。

为了达到这个目的，我们首先使用截断最小二乘(TLS)代价函数将配准问题重新建模，使得估计对大量假对应点不敏感。

然后，我们提供一个通用的图理论框架将尺度、旋转和平移估计解耦，这样就可以级联地求解三个变换。

尽管每一个子问题仍然是非凸和组合的，但我们证明了：

(i) 通过一个adaptive voting机制可以在多项式时间内求解TLS尺度和分量形式平移估计。

(ii) TLS旋转估计可以被松弛为一个半定规划问题(SDP)，同时这个松弛是紧的，甚至是在极端外点率的情况下都可以被松弛。

(iii) 图理论框架通过寻找最大派系允许外点的显著修剪。

我们称结果算法为TEASER(截断最小二乘估计和半定松弛)。虽然求解大规模的SDP松弛通常是比较慢的，但我们开发了第二个快速的且可证明的算法，叫做TEASR++。

该算法使用渐进非凸性求解旋转子问题，同时利用Douglas-Rachford Splitting高效地证明全局最优性。

对于上述的两个算法，我们在估计误差上提供理论的界，这是第一个这样解决鲁棒配准问题的方法。

此外，我们在标准benchmarks，目标检测数据集和3DMatch扫描匹配数据集上测试性能，展示了：

(i) 两个算法统治了最先进的方法(如RANSAC，branch-&-bound，启发式)，当尺度已知时它们对于超过99%外点率的情况都是鲁棒的。

(ii) TEASER++毫秒级运行，是目前最快的鲁棒配准算法。

(iii) TEASER++非常鲁棒使得它能够求解对应点未知的问题(如假设所有对所有的对应点情况)，并且它显著地超越ICP，比Go-ICP更精确，同时要快几个数量级。

我们发布了一个快速开源C++的TEASER ++实现。

主要贡献

这篇文章提出了第一个可证明的算法，用于外点存在下的3D配准问题。我们使用截断最小二乘(TLS)代价函数将配准问题重新建模，使得问题对大量假对应点不敏感，但这导致了一个困难的，组合的和非凸的优化问题。

1.  一个通用的框架，用于解耦尺度，旋转和平移估计。我们方法的创新有四个部分：

(i) 我们开发了估计尺度的不变测量量。

(ii) 我们在噪声未知但有界的假设下，将解耦形式化。

(iii) 我们提供了一个通用的图理论框架，用于推导这些不变测量量。

(iv) 我们展示这个框架通过寻找不变测量量定义的图的最大派系，允许修剪大量的外点。

解耦允许级联地求解尺度、旋转和平移。然而每个子问题仍然是组合的。

2. 证明：

(i) 使用adaptive voting机制能够在多项式时间内精确地求解标量例子的TLS估计问题，这就能够高效地进行尺度和分量形式平移的估计。

(ii) 我们能够建立一个紧的半定规划(SDP)松弛去估计旋转，同时建立一个后验条件去检测松弛的质量。

我们注意到本文中求解的旋转子问题在视觉(所谓的旋转搜索)和航空航天(所谓的Wahba问题)。我们的SDP松弛是第一个用于鲁棒旋转搜索问题的可证明算法。

3. 验证我们称为截断最小二乘估计和半定松弛算法返回解的质量的一系列理论结果。在无噪声例子中，我们提供了易于检查的条件，其中TEASER在外点存在的情况下恢复出了点云之间的变换。

在有噪声例子中，我们在TEASER估计和真值变换之间的距离上提供了界。据我们所知，这些是有外点几何估计问题中第一个非渐进误差界，虽然统计学的鲁棒估计文献通常在欧氏空间中研究更简单的问题并且聚焦渐进界。

4. 实现TEASER的一个快速版本，称为TEASER++，使用渐进非凸(GNC)估计旋转而不需要求解大规模SDP。

我们展示了TEASER++是可证明的，特别地我们使用Douglas-Rachford Splitting设计一个可扩展的最优证实算子，该算子能够断言GNC返回的估计值的全局最优性。我们发布了一个快速开源C++的TEASER++的实现。

5. 在标准benchmarks和在目标检测、扫描匹配的真实数据集上进行了大量的验证。

特别地，我们展示了。

(i) TEASER和TEASER++统治了最先进的算法(如RANSAC，branch-&-bound，启发式)。

当尺度已知时它们对于超过99%外点率的情况都是鲁棒的。

(ii) TEASER++毫秒级运行，是目前最快的鲁棒配准算法。

(iii) TEASER++非常鲁棒使得它能够求解对应点未知的问题(如假设所有对所有的对应点情况)，并且它显著地超越ICP，比Go-ICP更精确，同时要快几个数量级。

(iv) 当和基于深度学习的关键点检测和匹配相结合时，TEASER++能够提升配准性能。

6.在我们之前的工作中引入了TEASER和旋转子问题的基于四元数松弛。

本文则将TEASER带向成熟。

(i)在TEASER性能上提供显著的理论结果。

(ii)提供一个快速的最优性证实方法。

(iii) 开发一个快速的算法，TEASER++，使用GNC估计旋转并且无需求解SDP，同时仍然是可证明的。

(iv) 发表了一个更全面的实验验证，包括在3DMatch数据集上的实际测试，以及没有匹配点的配准例子。这些是同时在理论和实际方面的主要提升。

算法流程

1.使用截断最小二乘代价函数的鲁棒配准

2.解耦尺度，旋转和平移估计

重新转换测量值以得到尺度、旋转和平移变换的不变量。

该不变量思想如图所示

3.截断最小二乘估计和半定松弛(TEASER)

4.鲁棒的尺度和平移估计:Adaptive voting

上述第4行见Fig. 3(a)，第6、12行见Fig. 3(b)

第17行计算尺度估计值公式

5.鲁棒的旋转估计:半定松弛和快速证实

6.TEASER和TEASER++求解尺度、旋转和平移三个子问题的对比

6.1 TEASE

 采用半定规划(SDP)和凸松弛算法估计旋转部分

6.2 TEASER++

采用 Certifiable GNC 算法估计旋转部分，提升了算法效率和可证明能力

主要实验结果

1.标准benchmarks测试

与Fast Global Registration (FGR) 、 Guaranteed Outlier REmoval (GORE)和RANSAC两种变体进行精度和效率的比较

2.应用1:目标位姿估计和定位

给定来自FPFH特征描述子的对应点

3.应用2:扫描匹配

一个有趣的现象：TEASER++会证实一些不正确的解，这些解的旋转误差大多位于90°和180°附近(如右图中蓝色点)，这些点对应于对称的场景，说明对称场景允许多个配准结果。

TEASER++代码解读与运行

1.代码地址

https://github.com/MIT-SPARK/TEASER-plusplus

提供了C++，Python和Matlab的程序。

2.代码整体框架

(1) 输入为两组点云的匹配对和噪声上界；

(2) 使用adaptive voting进行尺度估计，如果尺度已知，则进行已知尺度修剪外点操作；

(3) 产生内点图；

(4) 在内点图中寻找最大团从而选择最大内点集；

(5) 最大内点集传入GNC-TLS模块进行旋转估计；

(6) 使用adaptive voting进行平移估计；

(7) 输出为尺度，旋转和平移；

3.代码解读

(1) 首先是载入点云文件，读入第一组点云数据；

(2) 将读入的第一组点云数据转换为Eigen格式的数据；

(3) 将第一组点云数据变为齐次坐标形式；

(4) 对第一组点云应用一个任意SE(3)变换，产生一组无噪声outlier-free的点云；

(5) 对第二组点云数据添加噪声和外点；

(6) 配准算子参数设定，最大噪声界(noise_bound，欧氏距离的L2-norm )为0.05，TLS中

 为1，是否估计尺度参数(estimate_scaling)默认为不估计false(如需要估计则设为true)；

GNC旋转估计最大迭代次数(rotation_max_iterations)为100，GNC控制参数更新比值为1.4。

(即控制参数

每次更新的调整程度，，相关内容可以参考作者RAL2020 “Graduated Non-Convexity for Robust Spatial Perception: From Non-Minimal Solvers to Global Outlier Rejection”的 Remark 5)

采用TLS框架的GNC算法估计旋转(rotation_estimation_algorithm)，即使用GNC_TLS计算旋转部分的最优解，旋转估计部分的代价阈值(rotation_cost_threshold)为0.005。

(7) TEASER++求解，src和tgt为Eigen矩阵形式的匹配对；

(8) 运行结果，期望(Expected)旋转、平移和估计(Estimated)旋转、平移结果，匹配对(correspondences)数量，外点(outliers)数量，算法运行时间(Time)；

总结

此工作提出了一个快速的可证明的算法，用于极端外点率情况的基于对应点的配准问题。

使用了估计理论中的未知但有界噪声，几何中的不变测量，图理论中的内点选择最大团和优化中的紧SDP松弛。

这篇TRO文章是由作者之前在RSS2019 “A Polynomial-time Solution for Robust Registration with Extreme Outlier Rates” 和 ICCV2019 “A Quaternion-based Certifiably Optimal Solution to the Wahba Problem with Outliers”两个工作撺掇起来的。

使用了前者RSS2019中的不变测量概念，后者ICCV2019中旋转参数化为四元数的想法，整体算法流程类似于前者的框架，总的来说是一个很出色的整体工作。

追溯着看，作者一系列的工作都很solid，有理论创新（数学证明），有实际实现（代码规范），非常值得学习。

---

论文链接：

https://arxiv.org/pdf/2001.07715.pdf

论文视频：

https://youtu.be/xib1RSUoeeQ