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

最新推荐文章于 2024-04-25 14:39:56 发布

无用闲士

最新推荐文章于 2024-04-25 14:39:56 发布

阅读量2.6k

点赞数 5

分类专栏：计算机视觉 SLAM 文章标签：计算机视觉算法

本文链接：https://blog.csdn.net/u010307048/article/details/115911129

版权

计算机视觉同时被 2 个专栏收录

37 篇文章 38 订阅

订阅专栏

SLAM

8 篇文章 2 订阅

订阅专栏

在这里插入图片描述
论文地址

摘要

我们提出了第一个快速且可证明的算法，用于存在大量外点对应的情况下两组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 ++实现

补充材料

视频: https://youtu.be/xib1RSUoeeQ

主要贡献

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

一个通用的框架，用于解耦尺度，旋转和平移估计。我们方法的创新有四个部分：(i)我们开发了估计尺度的不变测量量，(ii)我们在噪声未知但有界的假设下，将解耦形式化，(iii)我们提供了一个通用的图理论框架，用于推导这些不变测量量，(iv)我们展示这个框架通过寻找不变测量量定义的图的最大派系，允许修剪大量的外点。解耦允许级联地求解尺度、旋转和平移。然而每个子问题仍然是组合的。
证明(i)使用adaptive voting机制能够在多项式时间内精确地求解标量例子的TLS估计问题，这就能够高效地进行尺度和分量形式平移的估计；(ii)我们能够建立一个紧的半定规划(SDP)松弛去估计旋转，同时建立一个后验条件去检测松弛的质量。我们注意到本文中求解的旋转子问题在视觉(所谓的旋转搜索)和航空航天(所谓的Wahba问题)。我们的SDP松弛是第一个用于鲁棒旋转搜索问题的可证明算法。
验证我们称为截断最小二乘估计和半定松弛算法返回解的质量的一系列理论结果。在无噪声例子中，我们提供了易于检查的条件，其中TEASER在外点存在的情况下恢复出了点云之间的变换。在有噪声例子中，我们在TEASER估计和真值变换之间的距离上提供了界。据我们所知，这些是有外点几何估计问题中第一个非渐进误差界，虽然统计学的鲁棒估计文献通常在欧氏空间中研究更简单的问题并且聚焦渐进界。
实现TEASER的一个快速版本，称为TEASER++，使用渐进非凸(GNC)估计旋转而不需要求解大规模SDP。我们展示了TEASER++是可证明的，特别地我们使用Douglas-Rachford Splitting设计一个可扩展的最优证实算子，该算子能够断言GNC返回的估计值的全局最优性。我们发布了一个快速开源C++的TEASER++的实现。
在标准benchmarks和在目标检测、扫描匹配的真实数据集上进行了大量的验证。特别地，我们展示了(i)TEASER和TEASER++统治了最先进的算法(如RANSAC，branch-&-bound，启发式)，当尺度已知时它们对于超过99%外点率的情况都是鲁棒的，(ii)TEASER++毫秒级运行，是目前最快的鲁棒配准算法，(iii)TEASER++非常鲁棒使得它能够求解对应点未知的问题(如假设所有对所有的对应点情况)，并且它显著地超越ICP，比Go-ICP更精确，同时要快几个数量级，(iv)当和基于深度学习的关键点检测和匹配相结合时，TEASER++能够提升配准性能。
在我们之前的工作中引入了TEASER和旋转子问题的基于四元数松弛。本文则将TEASER带向成熟(i)在TEASER性能上提供显著的理论结果，(ii)提供一个快速的最优性证实方法，(iii)开发一个快速的算法，TEASER++，使用GNC估计旋转并且无需求解SDP，同时仍然是可证明的，(iv)发表了一个更全面的实验验证，包括在3DMatch数据集上的实际测试，以及没有匹配点的配准例子。这些是同时在理论和实际方面的主要提升。

算法流程

使用截断最小二乘代价函数的鲁棒配准
1.1 给定两组3D点云 $a_i, b_i$ ，测量噪声 $\epsilon_i$ ，inlier-outlier向量 $o_i$ ，原始问题为：

1.2 问题的非线性最小二乘版本：

1.3 假设噪声未知但有界，为了对外点不敏感，则变换为截断最小二乘(TLS)配准：

截断最小二乘估计是NP-hardness，穷举搜索全局最优解的时间复杂度为指数时间 $O(2^N)$
解耦尺度，旋转和平移估计
重新转换测量值以得到尺度、旋转和平移变换的不变量
2.1 平移不变测量(TIMs)
求取两组点云 $b_i, b_j$ 的相对位置，这样相减就消掉了平移 $t$ ：

平移不变测量(TIM)为 $\bar{a}_{ij} \overset{.}{=} a_j-a_i, \bar{b}_{ij} \overset{.}{=} b_j-b_i, \epsilon_{ij} \overset{.}{=} \epsilon_j - \epsilon_i$ ，满足以下模型，这个模型只依赖于未知的尺度 $s$ 和旋转 $R$ ：

2.2 平移和旋转不变测量
计算TIM的范数：

因为测量噪声有界，可以得到：

两边同时除以 $||\bar{a}_{ij}||$ ，则得到新的测量 $s_{ij} \overset{.}{=} \frac{||\bar{b}_{ij}||}{||\bar{a}_{ij}||}$ ：

2.3 上述不变测量的总结
截断最小二乘估计和半定松弛(TEASER)
3.1 TRIMs 估计尺度 $\hat{s}$ ，第4部分会详细介绍

3.2 $\hat{s}$ 和 TIMs 估计旋转 $\hat{R}$ ，第5部分会详细介绍

3.3 在原始的TLS问题中使用 $\hat{s}$ ， $\hat{R}$ 从 $a_i, b_i)$ 估计平移 $\hat{t}$ 的三个分量，第4部分会详细介绍

3.4 上述TEASER算法
鲁棒的尺度和平移估计：Adaptive voting
4.1 标量TLS估计的adaptive voting
给定标量尺度 $s$ ，定义一致集 $\mathcal{I}(s)=\left\{ k: \frac{(s-s_k)}{\alpha^2_k} \le \bar{c}^2 \right\}$ 。对任何 $s$ ，最多有 $2 K - 1$ 个不同的非空一致集。采用adaptive voting求解各个一致集：

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

第17行计算尺度估计值公式：
鲁棒的旋转估计：半定松弛和快速证实
5.1 使用单位四元数将旋转TLS(12)改写为：

5.2 将四元数TLS问题转换为混合整数规划问题
引入二值变量 $\theta_k$ ，将(15)改写为：

5.3 将混合整数转换为四元数形式
定义 $q_k \overset{.}{=} \theta_k q$ ，则(17)改写为Binary Cloning：

5.4 将四元数形式转换为QCQP
定义 $x = [q^T q_1^T ... q_K^T]^T$ ，则(18)转换为QCQP：

这是非凸的。
5.5 半定松弛
定义 $Z=xx^T$ ，则QCQP(19)可以转换为非凸秩约束规划：

然后舍弃rank-1约束，增加冗余约束，则得到带冗余约束的SDP松弛：

5.6 快速全局最优证实
产生次优性界 $\mu$ 的算法

得到次优性界 $\mu$ 之后，就可以验证可行解 $(\hat{q}, \hat{\theta}_1, ..., \hat{\theta}_K)$ 的最优性：

其中 $\hat{\mu}=\hat{x}^TQ\hat{x}, \hat{x}=[\hat{q}^T, \hat{\theta}_1\hat{q}^T, ..., \hat{\theta}_K\hat{q}^T]^T$ ， $\mu^{*}$ 是TLS旋转估计问题(15)和QCQP问题(19)中未知的全局最小值。
TEASER和TEASER++求解子问题的对比

TEASER

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

TEASER++

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

实验结果

测试TEASER和TEASER++模块
测试尺度、旋转和平移估计，maximal clique inlier selection和最优性证实
标准benchmarks测试
与Fast Global Registration (FGR) 、 Guaranteed Outlier REmoval (GORE)和RANSAC两种变体进行比较

精度方面：

效率方面：
同时位姿和对应点(SPC)
与ICP和Go-ICP进行比较
应用1：目标位姿估计和定位
给定来自FPFH特征描述子的对应点
应用2：扫描匹配

在这里插入图片描述

TEASER++代码解读与运行

代码地址

https://github.com/MIT-SPARK/TEASER-plusplus
提供了C++，Python和Matlab的程序

代码整体框架
（1）输入为两组点云的匹配对和噪声上界
（2）使用adaptive voting进行尺度估计，如果尺度已知，则进行已知尺度修剪外点操作
（3）产生内点图
（4）在内点图中寻找最大团从而选择最大内点集
（5）最大内点集传入GNC-TLS模块进行旋转估计
（6）使用adaptive voting进行平移估计
（7）输出为尺度，旋转和平移
在这里插入图片描述

代码解读

首先是载入点云文件，读入第一组点云数据：

  teaser::PLYReader reader;
  teaser::PointCloud src_cloud;
  auto status = reader.read("./example_data/bun_zipper_res3.ply", src_cloud);
  int N = src_cloud.size();

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

  Eigen::Matrix<double, 3, Eigen::Dynamic> src(3, N);
  for (size_t i = 0; i < N; ++i) {
    src.col(i) << src_cloud[i].x, src_cloud[i].y, src_cloud[i].z;
  }

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

  Eigen::Matrix<double, 4, Eigen::Dynamic> src_h;
  src_h.resize(4, src.cols());
  src_h.topRows(3) = src;
  src_h.bottomRows(1) = Eigen::Matrix<double, 1, Eigen::Dynamic>::Ones(N);

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

  T << 9.96926560e-01,  6.68735757e-02, -4.06664421e-02, -1.15576939e-01,
      -6.61289946e-02, 9.97617877e-01,  1.94008687e-02, -3.87705398e-02,
      4.18675510e-02, -1.66517807e-02,  9.98977765e-01, 1.14874890e-01,
      0,              0,                0,              1;
  Eigen::Matrix<double, 4, Eigen::Dynamic> tgt_h = T * src_h;
  Eigen::Matrix<double, 3, Eigen::Dynamic> tgt = tgt_h.topRows(3);

对第二组点云数据添加噪声和外点

  addNoiseAndOutliers(tgt);

此处是一个添加噪声和外点的函数：

首先对每个数据添加随机噪声，这里的噪声bound为0.05，也就是对随机噪声有一个0.05的尺度缩放，避免产生过大噪声

  Eigen::Matrix<double, 3, Eigen::Dynamic> noise =
      Eigen::Matrix<double, 3, Eigen::Dynamic>::Random(3, tgt.cols()) * NOISE_BOUND;
  NOISE_BOUND / 2;
  tgt = tgt + noise;

接着是添加外点，对噪声点云数据添加随机平移量

  std::random_device rd;
  std::mt19937 gen(rd());
  std::uniform_int_distribution<> dis2(0, tgt.cols() - 1); 
  std::uniform_int_distribution<> dis3(OUTLIER_TRANSLATION_LB,
                                       OUTLIER_TRANSLATION_UB); 
  std::vector<bool> expected_outlier_mask(tgt.cols(), false);
  for (int i = 0; i < N_OUTLIERS; ++i) {
  	// 产生需作为外点的点云数据索引，由std::mt19937 gen(rd())随机产生
    int c_outlier_idx = dis2(gen);
    assert(c_outlier_idx < expected_outlier_mask.size());
    expected_outlier_mask[c_outlier_idx] = true;
    // 添加随机平移量，由std::mt19937 gen(rd())随机产生
    tgt.col(c_outlier_idx).array() += dis3(gen); 
  }

配准算子参数设定，最大噪声界(noise_bound，欧氏距离的L2-norm )为0.05，TLS中 $\bar{c}^2$ (cbar2) 为1，是否估计尺度参数(estimate_scaling)默认为不估计false(如需要估计则设为true)，GNC旋转估计最大迭代次数(rotation_max_iterations)为100，GNC控制参数更新比值为1.4 (即控制参数 $\mu$ 每次更新的调整程度， $\mu \leftarrow \mu/1.4 或 1.4 \mu$ ，相关内容可以参考作者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。

  teaser::RobustRegistrationSolver::Params params;
  params.noise_bound = 0.05;
  params.cbar2 = 1;
  params.estimate_scaling = false;
  params.rotation_max_iterations = 100;
  params.rotation_gnc_factor = 1.4;
  params.rotation_estimation_algorithm =
      teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::GNC_TLS;
  params.rotation_cost_threshold = 0.005;

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

  teaser::RobustRegistrationSolver solver(params);
  std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();
  solver.solve(src, tgt);
  std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();
  auto solution = solver.getSolution();

运行结果，期望(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，有理论创新（数学证明），有实际实现（代码规范），非常值得学习。