视觉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 ∧ R E = t^{\wedge}R E=t∧R
t是平移变量，R是旋转变量。可以理解为本质就是t和R。
基础矩阵就是加上相机内参矩阵K，如下：
F = K − T E K − 1 F=K^{-T}EK^{-1} F=K−TEK−1
本质矩阵的求解使用的是8点法，细节这里不再展开复述，使用的公式如下：
x 2 T E x 1 = p 2 T F p 1 = 0 x_2^{T}Ex_1=p_2^{T}Fp_1 = 0 x2TEx1=p2TFp1=0
单应矩阵的引出
当在所有特征点都在同一个平面上，例如相机看到的棋盘格，无人机俯视看到的地面等。两个不同姿态下的相机，看到同一个平面的特征时，这两张图像之间的旋转平移关系（R和t）可以用单应矩阵表示。
单应矩阵可以使用4点法来求解，这里不再复述，使用的公式如下：
p 2 = H p 1 p_2 = Hp_1 p2=Hp1
三角测量的引出：
在SLAM中，我们主要用三角化来估计像素点的距离。
按照对极几何中的定义，设 x 1 x_1 x1, x 2 x_2 x2为两个特征点的归一化坐标，那么他们满足：
s 1 x 1 = s 2 R x 2 + t s_1x_1 = s_2Rx_2+t s1x1=s2Rx2+t
现在我们已经知道了R，t，想要求解的是两个特征点的深度 s 1 s_1 s1 s 2 s_2 s2。如果要算 s 2 s_2 s2，那么先对上式两侧左乘一个 x 1 ∧ x_1^{\wedge} x1∧，得：
s 1 x 1 ∧ x 2 = 0 = s 2 x 1 ∧ R x 2 + x 1 ∧ t s_1x_1^{\wedge}x_2 = 0 = s_2x_1^{\wedge}Rx_2+x_1^{\wedge}t s1x1∧x2=0=s2x1∧Rx2+x1∧t
可以根据上式直接求解 s 2 s_2 s2。
三角化的矛盾：
平移太小，则三角化精度不够，而平移过大，会导致匹配失效。
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 {\begin{bmatrix} u_i \\ v_i \\ 1 \end{bmatrix}} = Kexp({\xi}^{\wedge}){\begin{bmatrix} X_i \\ Y_i \\ Z_i \\ 1 \end{bmatrix}} si uivi1 =Kexp(ξ∧) XiYiZi1
注意这里的 ξ \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 \begin{Vmatrix} u_i-\frac{1}{s_i}Kexp({\xi}^{\wedge})P_i \end{Vmatrix}^{2} ξ∗=argmin21i=1∑n ui−si1Kexp(ξ∧)Pi 2
首先记变换到相机坐标系下的空间点坐标为 P ′ P^{'} P′，并且将其前3维取出来：
P ′ = ( e x p ( ξ ∧ ) P ) 1 : 3 = [ X ′ , Y ′ , Z ′ ] P^{'}=(exp({\xi}^{\wedge})P)_{1:3}={\begin{bmatrix}X^{'},Y^{'},Z^{'} \end{bmatrix}} P′=(exp(ξ∧)P)1:3=[X′,Y′,Z′]
相机投影模型相对于 P ′ P^{'} P′为
s ∗ u = K P ′ s*u = KP^{'} s∗u=KP′
展开可以得到
u = f x X ′ Z ′ + c x u=f_x{\frac{X^{'}}{Z^{'}}}+c_x u=fxZ′X′+cx
u = f y Y ′ Z ′ + c y u=f_y{\frac{Y^{'}}{Z^{'}}}+c_y u=fyZ′Y′+cy
在定义了中间变量后，我们对 ξ ∧ \xi^{\wedge} ξ∧左乘扰动量 δ ξ \delta{\xi} δξ，然后考虑e的变化关于扰动量的导数。利用链式法则，可以列写如下：
ϑ e ϑ δ ξ = lim δ ξ → 0 e ( δ ξ ⨁ ξ ) δ ξ = ϑ e ϑ P ′ ϑ P ′ ϑ δ ξ \frac{\vartheta{e}}{\vartheta{\delta\xi}} = \lim_{{\delta{\xi}{\to}0}}{\frac{e({\delta{\xi}{\bigoplus}{\xi}})}{\delta{\xi}}}= \frac{\vartheta e}{\vartheta P^{'}} \frac{\vartheta P^{'}}{\vartheta {\delta} {\xi}} ϑδξϑe=δξ→0limδξe(δξ⨁ξ)=ϑP′ϑeϑδξϑP′
这里写一下误差关于投影点的导数
ϑ e ϑ P ′ = − [ ϑ u ϑ X ′ ϑ u ϑ Y ′ ϑ u ϑ Z ′ ϑ v ϑ X ′ ϑ v ϑ Y ′ ϑ v ϑ Z ′ ] \frac {\vartheta e} {\vartheta P^{'}} = - \begin{bmatrix}\frac{\vartheta u}{\vartheta X^{'}} & \frac{\vartheta u}{\vartheta Y^{'}} &\frac{\vartheta u}{\vartheta Z^{'}} \\ \frac{\vartheta v}{\vartheta X^{'}} & \frac{\vartheta v}{\vartheta Y^{'}} & \frac{\vartheta v}{\vartheta Z^{'}} \end{bmatrix} ϑP′ϑe=−[ϑX′ϑuϑX′ϑvϑY′ϑuϑY′ϑvϑZ′ϑuϑZ′ϑv]
后面关于误差关于李代数及空间点的导数，细节不再复述。经过这个步骤，我们推导出了观测相机方程关于相机位姿与特征点的两个导数矩阵。它们之分重要，能够在优化过程中提供重要的梯度方向，指导优化的迭代。
3D-3D：ICP
假设我们有一组配对好的3D点（比如我们对两幅RGB-D图像进行了匹配），我呢就可以使用ICP求解。
ICP的求解也分为两种方式：利用线性代数的求解（主要是SVD），以及利用非线性优化的方式求解。这里不再一一复述。