视觉SLAM十四讲学习笔记-第七讲-视觉里程计-ICP和实践

   专栏汇总

视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第二讲-开发环境搭建_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第三讲-旋转矩阵和Eigen库_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第三讲-旋转向量、欧拉角、四元数_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第三讲-相似、仿射、射影变换和eigen程序、可视化演示_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记---前三讲学习笔记总结之SLAM的作用、变换和位姿表示_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第四讲-李代数求导与扰动模型_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第四讲-Sophus实践、相似变换群与李代数_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第五讲-相机模型_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第五讲-图像和实践_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第四讲---第五讲学习笔记总结---李群和李代数、相机_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第六讲-非线性优化的非线性最小二乘问题_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第六讲-非线性优化的实践-高斯牛顿法和曲线拟合_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第六讲学习笔记总结（1）---非线性优化原理_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第六讲学习笔记总结（2）---非线性优化应用_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第七讲-视觉里程计-特征点发和特征提取和匹配实践_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第七讲-视觉里程计-对极几何和对极约束、本质矩阵、基础矩阵

视觉SLAM十四讲学习笔记-第七讲-视觉里程计-单应矩阵和实践_goldqiu的博客-CSDN博客

​​​​​​​​​​​​视觉SLAM十四讲学习笔记-第七讲-视觉里程计-三角测量和实践_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第七讲-视觉里程计-PnP和实践_goldqiu的博客-CSDN博客

7.9 3D−3D：ICP

假设有一组配对好的 3D 点（比如对两幅 RGB-D 图像进行了匹配）：

想要找一个欧氏变换 R, t，使得：

这个问题可以用迭代最近点（Iterative Closest Point，ICP）求解。3D−3D 位姿估计问题和相机模型并没有关系。在激光 SLAM 中也会碰到 ICP，不过由于激光数据特征不够丰富，无从知道两个点集之间的匹配关系，只能认为距离最近的两个点为同一个，所以这个方法称为迭代最近点。而在视觉中，特征点提供了较好的匹配关系，所以整个问题就变得更简单了。在 RGB-D SLAM 中， 可以用这种方式估计相机位姿。ICP 的求解也分为两种方式：利用线性代数的求解（主要是 SVD），以及利用非线性优化方式的求解（类似于 Bundle Adjustment）。

7.9.1 SVD 方法

定义第 i 对点的误差项：

然后构建最小二乘问题，求使误差平方和达到极小的R, t：

求解:

首先，定义两组点的质心：

随后，在误差函数中做如下的处理：

交叉项部分中

在求和之后为零，因此优化目标函数可以简化为

左边只和旋转矩阵 R 相关，而右边既有 R 也有 t，但只和质心相关。只要获得了R，令第二项为零就能得到 t。

于是ICP可以分为以下三个步骤求解：

1. 计算两组点的质心位置 p, p′，然后计算每个点的去质心坐标：

2. 根据以下优化问题计算旋转矩阵：

3. 根据第 2 步的 R 计算 t： t∗ = p − Rp′

求出了两组点之间的旋转，平移量很容易得到。

展开关于 R 的误差项，得：

第一项和 R 无关，第二项由于 RTR = I，也与 R 无关。因此实际上优化目标函数变为

下面通过 SVD 解出上述问题中最优的 R：

先定义矩阵：

W 是一个 3 × 3 的矩阵，对 W 进行 SVD 分解，得： W = UΣVT

其中，Σ 为奇异值组成的对角矩阵，对角线元素从大到小排列，而 U 和 V 为对角矩阵。当 W 满秩时，R = UVT

然后按t∗ = p − Rp′ 求解 t。

7.9.2 非线性优化方法

以李代数表达位姿时，目标函数可以写成

使用李代数扰动模型：

在非线性优化中只需不断迭代，就能找到极小值。ICP 问题存在唯一 解或无穷多解的情况。在唯一解的情况下，只要能找到极小值解，那么这个极小值就是全局最优值，不会遇到局部极小的情况。这意味着已匹配点时求解 ICP可以任意选定初始值。

这里的ICP是指已由图像特征给定了匹配的情况下进行位姿估计的问题。 在匹配已知的情况下，这个最小二乘问题具有解析解，所以并没有必要进行迭代优化。对于深度已知的特征点，建模它们的 3D−3D 误差；对于深度未知的特征点，则建模 3D−2D 的重投影误差。可以将所有的误差放在同一个问题中考虑，使得求解更加方便。

7.10 实践：求解 ICP

7.10.1 SVD 方法

使用两幅 RGB-D 图像，通过特征匹配获取两组 3D 点，用 ICP 计算它们的位姿变换。

代码：slambook/ch7/pose_estimation_3d3d.cpp

调用 Eigen 进行 SVD，然后计算 R, t 矩阵。输出匹配后的结果。

对比ICP 与 PnP、对极几何的运动估计结果之间的差异，在这个过程中使用了越来越多的信息，从没有深度到有一个图的深度到有两个图的深度。因此在深度准确的情况下，得到的估计也将越来越准确。但是由于 Kinect 的深度图存在噪声，而且有可能存在数据丢失的情况，使得不得不丢弃一些没有深度数据的特征点。这可能导致 ICP 的估计不够准确，并且，如果特征点丢弃得太多，可能引起由于特征点太少，无法进行运动估计的情况，即退化的情况。

7.10.2 非线性优化方法

使用李代数来表达相机位姿，与 SVD 思路不同的地方在于，在优化中不仅考虑相机的位姿，同时会优化 3D 点的空间位置。RGB-D 相机每次可以观测到路标点的三维位置，从而产生一个 3D 观测数据。g2o/sba 中没有提 供 3D 到 3D 的边，所以自定义一 种这样的边，并向 g2o 提供解析求导方式。

代码：slambook/ch7/pose_estimation_3d3d.cpp

这是一个一元边，写法类似于g2o::EdgeSE3ProjectXYZ，不过观测量从 2 维变成了 3 维，内部没有相机模型，并且只关联到一个节点。雅可比矩阵给出了关于相机位姿的导数，是一个 3 × 6 的矩阵。 调用 g2o 进行优化的代码是相似的，设定好图优化的节点和边即可。

优化的结果：

只迭代一次后总体误差稳定不变，说明仅在一次迭代之后算法即已收敛。从位姿求解的结果可以看出，它和前面 SVD 给出的位姿结果几乎一模一样，这说明 SVD 已经给出了优化问题的解析解。所以可以认为 SVD 给出的结果是相机位姿的最优值。 在这个ICP 中，使用了在两个图都有深度读数的特征点。然而事实上只要其中一个图深度确定，就能用类似于 PnP 的误差方式，把它们也加到优化中来。同时，除了相机位姿之外，将空间点也作为优化变量考虑，也是一种解决问题的方式。实际的求解是非常灵活的，不必拘泥于某种固定的形式。如果同时考虑点和相机，整个问题就变得更自由了，可能会得到其他的解。比如可以让相机少转一些角度，而把点多移动一些。这从另一侧面反映出，在 Bundle Adjustment 里面，会希望有尽可能多的约束，因为多次观测会带来更多的信息，更准确地估计每个变量。

7.11 小结

基于特征点的视觉里程计中的几个重要的问题，包括：

1. 特征点如何提取并匹配
2. 如何通过 2D−2D 的特征点估计相机运动。
3. 如何从 2D−2D 的匹配估计一个点的空间位置。
4. 3D−2D 的 PnP 问题，其线性解法和 Bundle Adjustment 解法。
5. 3D−3D 的 ICP 问题，其线性解法和 Bundle Adjustment 解法。

这省略了大量关于某些特殊情况的讨论。例如，如果在对极几何求解过程中给定的特征点共面，会发生什么情况？共线又会发生什么情况？在 PnP 和 ICP 中若给定这样的解，又会导致什么情况？求解算法能否识别这些特殊的情况，并报告所得的解可能不可靠？

在工程实现中，这些情况很少出现。

习题

1. 除了ORB 特征点，SIFT 或 SURF 的原理，并对比它们与 ORB 之间的优劣。
2. 设计程序调用 OpenCV 中的其他种类特征点。统计在提取 1000 个特征点时在机器上所用的时间。
3. OpenCV 提供的 ORB 特征点在图像当中分布不够均匀。有没让特征点分布更加均匀的方法？
4. 研究 FLANN 为何能够快速处理匹配问题。除了 FLANN 之外，还有哪些可以加速匹配的手段？
5. 在 PnP 优化中，将第一个相机的观测也考虑进来，程序应如何书写？最后结果会有何变化？
6. 在 ICP 程序中，将空间点也作为优化变量考虑进来，程序应如何书写？最后结果会有何变化？
7. 在特征点匹配过程中，不可避免地会遇到误匹配的情况。如果把错误匹配输入到 PnP 或 ICP 中，会发生怎样的情况？能想到哪些避免误匹配的方法？
8. 使用 Sophus 的 SE3 类，自己设计 g2o 的节点与边，实现 PnP 和 ICP 的优化。
9. 在 Ceres 中实现 PnP 和 ICP 的优化。