视觉SLAM——特征点法视觉里程计

摘要

本文是对《视觉SLAM十四讲》第七讲“视觉里程计”理论部分的个人总结。重点讨论完成特征匹配后，根据匹配的点对来估计相机的运动。即基于特征点方式的视觉里程计算法。
代码参见：https://download.csdn.net/download/qq_43145072/10899987
根据相机的原理不同，主要分以下几种情况：
1.（2D-2D）:对于单目相机，只知道2D像素坐标，则问题变为根据两组2D点估计运动。一般采用对极几何。
2. (3D-3D):对于双目或者RGB-D相机（深度可获得），则问题变为根据两组3D点对估计相机运动。一般采用ICP方法。
3. (3D-2D):对于已知3D点及其在相机的投影位置，则变为PnP问题。

1、2D-2D问题：对极几何

2D-2D问题是如何从两张图像中已经匹配的特征点对，恢复出两帧之间相机的运动。

1.1、对极约束

以下图为例，我们目标是求取图像i1和i2之间的运动（第一帧到第二帧的R和t）。P为空间中的一点。p1和p2为匹配的一对特征点。

根据几何关系进行推导，可得以下对极约束：（7.7）或者（7.8）.
对极几何约束简洁地给出了两个匹配点的空间关系。
于是相机位姿估计问题可以分为以下两步：

1.根据配对点的像素位置求出E或者F；（式7.7或7.8）
2.根据E或者F，求R,t；（式7.9）

E和F只相差了已知的相机内参。后面均以E为例讨论上述两步问题求解。

1.2、三角测量

根据对极几何求得相机运动后，（单目SLAM）需要通过三角测量的方法估计地图点的深度。

根据匹配的特征点对以及R，t即可求得深度。过程如下：

2、3D-2D问题：PnP

PnP（perspective-n-point）求解的问题是：已知n个3D空间点及其投影位置时，如何估计相机运动。
PnP的求解有很多种方法，如直接线性变换（DLT）、P3P、BA等。

2.1、直接线性变换（DLT）

经过以上推导，可以实现对矩阵T的求导，也就是[t1,t2,t3]，从而完成相机运动的求解。

2.2、P3P

P3P需要三对点的给定关系。输入数据为3对3D-2D匹配点。记3D点为A,B,C，2D点为a,b,c。小写字母代表的点是大写字母代表的点在相机成像平面的投影。
我们知道。A,B,C是世界坐标系的坐标，一旦这些3D点在相机坐标系下的坐标能够算出，就得到了3D-3D对应点，从而将PnP问题转换为ICP问题。
ICP问题的求解在第三节讲述。

2.3、（Bundle Adjustment）BA

在PnP中，BA问题是一个最小化投影误差（Reprojection error）问题，通过构建李代数上的非线性最小二乘问题来求解 。BA不同于2.1和2.2中的线性方法，它采用的是非线性优化，将相机位姿和空间点位置看作优化变量，一起进行优化（而前面两种方法是先计算相机位姿再求空间点位置）。

与之前用R,t表示相机运动不同，这里用李代数表示相机运动，以便计算。
构建最小二乘问题如下：

计上述最小二乘问题中误差为e，则求解该问题的关键是推导雅可比矩阵的形式。
这里e为像素坐标误差（二维），当待优化量为相机位姿（6维）时，雅可比矩阵为2x6的矩阵，其形式为：

当待优化量为空间点的位置（3维）时，雅可比矩阵为2x3的矩阵，其形式为：

至此，得到了关于相机位姿和特征点位置的两个雅可比矩阵，将为优化过程提供重要的梯度方向，指导优化迭代。

3、3D-3D：ICP

上面这个问题可以用迭代最近点（ICP）求解。需要注意的是，这里指的是对已经匹配好的两组点进行运动估计问题。
ICP方法分为两种：利用线性代数的求解（如SVD），以及利用非线性优化方式的求解（类似于BA）。

3.1、SVD方法

式7.49的求解过程参考高翔《视觉slam十四讲》部分。不再详细讨论。

3.2、非线性优化方法

小结

1.如何通过2D-2D的特征点估计相机运动（对极几何）；
2.如何通过2D-2D的匹配估计点的空间位置（三角测量）；
3.3D-2D的PnP问题，包括线性方法（直接线性变换DLT、P3P、）与非线性优化（BA等）；
4.3D-3D的ICP问题，包括线性方法（SVD方法）与非线性优化；