视觉SLAM14讲笔记-第7讲-视觉里程计1

图像特征点

特征点由关键点和描述子两部分组成。我们说的特征点具有旋转不变性，都是指的是特征点的描述子具有旋转不变性，而描述子指的是一个点周围的特征，一般由一个向量组成，描述一个点附近的特征（比如45度方向是亮的，135度方向是暗的等）
比较著名的图像特征点包括SIFT、SURF、ORB等。
ORB提取速度比较快，所以在特征点法来获得视觉里程计一般使用ORB算子。

特征匹配

当特征点数量很大时，暴力匹配法的运算量将变得很大，特别是当我们想要匹配一个帧和一张地图的时候。OpenCV中我们会用到快速近似最临近（FLANN）算法来处理这个匹配问题，目前已经比较成熟也已经集成到OpenCV中。

计算相机之间的相对位姿

1.当相机为单目时，我们只知道2D的像素坐标，因而问题是根据两组2D点来估计运动，该问题用对极几何来解决。
2.当相机为双目，RGB-D时，或者通过某种方法得到了距离信息，那么问题就是根据两组3D点估计运动。该问题通常用icp来解决。
3.如果有3D点及其在相机的投影位置，也能估计相机的运动。该问题通过PnP求解。

2D-2D

图像运动解析中最出名的就是对极约束。它以公式简洁著称，本质矩阵如下：
$$E=t^{\wedge }R$$
t是平移变量，R是旋转变量。可以理解为本质就是t和R。

基础矩阵就是加上相机内参矩阵K，如下：
$$F=K^{-T}E{K}^{-1}$$

本质矩阵的求解使用的是8点法，细节这里不再展开复述，使用的公式如下：
$$x_2^{T}E{x}_1=p_2^{T}F{p}_1=0$$

单应矩阵的引出
当在所有特征点都在同一个平面上，例如相机看到的棋盘格，无人机俯视看到的地面等。两个不同姿态下的相机，看到同一个平面的特征时，这两张图像之间的旋转平移关系（R和t）可以用单应矩阵表示。
单应矩阵可以使用4点法来求解，这里不再复述，使用的公式如下：
$$p_2=H{p}_1$$

三角测量的引出：
在SLAM中，我们主要用三角化来估计像素点的距离。
按照对极几何中的定义，设$x_1$,$x_2$为两个特征点的归一化坐标，那么他们满足：
$$s_1{x}_1=s_{2}R{x}_2+t$$
现在我们已经知道了R，t，想要求解的是两个特征点的深度$s_1$ $s_2$。如果要算$s_2$，那么先对上式两侧左乘一个$x_1^{\wedge }$，得：
$$s_1{x}_1^{\wedge }{x}_2=0=s_2{x}_1^{\wedge }R{x}_2+x_1^{\wedge }t$$
可以根据上式直接求解$s_2$。

三角化的矛盾：
平移太小，则三角化精度不够，而平移过大，会导致匹配失效。

3D-2D:PnP:（Perspective-n-Point）:透视n点法
是最重要的姿态估计方法！
是最重要的姿态估计方法！
是最重要的姿态估计方法！
重要的事情说3遍。
PnP是求解3D到2D点对运动的方法。它描述了当知道n个3D空间点及其投影位置时，如何估计相机的位姿。前面说到，2D-2D的对极几何方法需要8个或8个以上的点对，且存在着初始化、纯旋转和尺度的问题。然而。如果两张图像中其中一张特征点的3D位置已知，那么最少只需3个点对（需要知道一个额外点验证结果）就可以估计相机运动。特征点的3D位置可以由三角化或者RGB-D相机的深度图确定。因此，在双目或RGB-D的视觉里程计中，我们可以直接使用PnP估计相机运动。而在单目视觉里程计中，必须先进行初始化，然后才能使用PnP。3D-2D方法不需要使用对极约束，又可以在很少的匹配点中获得较好的运动估计，是最重要的一种姿态估计方法。

PnP的计算方法有很多，例如，用3对点估计位姿的P3P、直接线性变换（DLT）、EPnP、UPnP等等。此外，还能用非线性优化的方式，构建最小二乘问题并迭代求解，也就是万金油式的Bundle Adjustment。

这里我们只看一下Bundle Adjustment，学术上也叫光束法平差。

前面说的线性方法，往往是先求相机位姿，再求空间点位置，而非线性优化则是把他们都看成优化变量，放在一起优化。

重投影误差的引出
BA中的残差方程就是重投影误差，考虑n个三维空间点P及其投影p，我们希望计算相机的位姿R，t，它的李代数表示为$\xi$。
s i [ u i v i 1 ] = K e x p ( ξ ∧ ) [ X i Y i Z i 1 ] s_i {

} = Kexp({\xi}^{\wedge}){} si​ ​ui​vi​1​ ​=Kexp(ξ∧) ​Xi​Yi​Zi​1​ ​

注意这里的$\xi$代表的是se（3）。
构建残差方程：
ξ ∗ = a r g m i n 1 2 ∑ i = 1 n ∥ u i − 1 s i K e x p ( ξ ∧ ) P i ∥ 2 {\xi}^{*} = arg min\frac{1}{2}\sum_{i=1}^n

^{2} ξ∗=argmin21​i=1∑n​ ​ui​−si​1​Kexp(ξ∧)Pi​​ ​2

首先记变换到相机坐标系下的空间点坐标为$P^{{}^{\prime }}$，并且将其前3维取出来：
P ′ = ( e x p ( ξ ∧ ) P ) 1 : 3 = [ X ′ , Y ′ , Z ′ ] P^{'}=(exp({\xi}^{\wedge})P)_{1:3}={

} P′=(exp(ξ∧)P)1:3​=[X′,Y′,Z′​]

相机投影模型相对于$P^{{}^{\prime }}$为
$$s\ast u=K{P}^{{}^{\prime }}$$

展开可以得到
$$u=f_x\frac{X^{{}^{\prime }}}{Z^{{}^{\prime }}}+c_x$$
$$u=f_y\frac{Y^{{}^{\prime }}}{Z^{{}^{\prime }}}+c_y$$

在定义了中间变量后，我们对${\xi }^{\wedge }$左乘扰动量$\delta \xi$，然后考虑e的变化关于扰动量的导数。利用链式法则，可以列写如下：
$$\frac{\vartheta e}{\vartheta \delta \xi }=\underset{\delta \xi \to 0}{\lim }\frac{e(\delta \xi ⨁\xi )}{\delta \xi }=\frac{\vartheta e}{\vartheta P^{{}^{\prime }}}\frac{\vartheta P^{{}^{\prime }}}{\vartheta \delta \xi }$$

这里写一下误差关于投影点的导数
ϑ e ϑ P ′ = − [ ϑ u ϑ X ′ ϑ u ϑ Y ′ ϑ u ϑ Z ′ ϑ v ϑ X ′ ϑ v ϑ Y ′ ϑ v ϑ Z ′ ] \frac {\vartheta e} {\vartheta P^{'}} = -

ϑP′ϑe​=−[ϑX′ϑu​ϑX′ϑv​​ϑY′ϑu​ϑY′ϑv​​ϑZ′ϑu​ϑZ′ϑv​​]

后面关于误差关于李代数及空间点的导数，细节不再复述。经过这个步骤，我们推导出了观测相机方程关于相机位姿与特征点的两个导数矩阵。它们之分重要，能够在优化过程中提供重要的梯度方向，指导优化的迭代。

3D-3D：ICP

假设我们有一组配对好的3D点（比如我们对两幅RGB-D图像进行了匹配），我呢就可以使用ICP求解。
ICP的求解也分为两种方式：利用线性代数的求解（主要是SVD），以及利用非线性优化的方式求解。这里不再一一复述。