第7讲 视觉里程计1
7.1 特征点法
7.1.2 ORB特征
7.3 2D-3D: 对极几何
7.3.1 对极约束
我们希望求两帧图像$l_1$,$l_2$之间的运动。设第一帧$l_1$到第二帧$l_2$的运动为$R,t$，两个相机中心分别为$O_1,O_2$。考虑$l_1$中一特征点$p_1$，在$l_2$中对应着特征点$p_2$。$P$为连线$\vec{O_1p_1}$和连线$\vec{O_2p_2}$的交点。$O_1O_2P$为极平面。
下式(1)和(2)都称为对极约束，其中$x_1,x_2$是两个像素点$p_1,p_2$的归一化平面上的坐标，将$x_1=K^{-1}p_1, x_2=K^{-1}p_2$代入(1)，得(2)。
$ x^T_2t^\wedge Rx_1=0 \tag{1} $
$ p^T_2K^{-T}t^\wedge RK^{-1}p_1=0 \tag{2} $
对极约束的几何意义：$x_2$、t、$Rx_1$三者混合积为0，表示这三个向量共面，即上图中三角形的三边共面（极平面）。参考
我们把中间部分记作基础矩阵F（Fundamental Matrix）和本质矩阵E（Essential Matrix），于是可以进一步简化对极约束：
$ E=t^\wedge R,\ \ \ \ \ \ F=K^{-T}EK^{-1} \tag{3} $
$ x^T_2Ex_1=p^T_2Fp_1=0 \tag{4} $
对极约束简洁地给出了两个匹配点的空间位置关系，于是相机位姿估计问题变为以下两步：
- 根据配对点的像素位置求出E或F
- 根据E或F求出$R,t$（由于E和F只差了已知的相机内参，所以往往用形式更简单的E）
7.3.2 本质矩阵
本质矩阵E的性质：
- E的尺度等价性：本质矩阵是由对极约束定义的。由于对极约束是等式为零的约束，所以E乘以任意非零常数后，对极约束依然满足。
- E的内在性质：根据$E=t^\wedge R$，可以证明E的奇异值必定是$[\sigma,\sigma,0]^T$的形式。
- E有5个自由度：由于平移和旋转各有3个自由度，故$t^\wedge R$共有6个自由度，但由于尺度等价性，故E实际上有5个自由度
奇异值：设A为$m*n$阶矩阵，q=min(m,n)，$A*A$的q个非负特征值的算术平方根叫作A的奇异值。
根据本质矩阵E计算相机的运动$R,t$，是由奇异值分解（SVD）得到的。设E的SVD分解为（5），其中$U,V$为正交阵，$\Sigma$为奇异值矩阵。根据E的内在性质，$\Sigma=diag(\sigma,\sigma,0)$。
$ E=U\Sigma V^T \tag{5} $
在SVD分解中，对于任意一个E，存在两个可能的$R,t$与它对应（6）（7）。其中$R_Z(\frac\pi2)$表示沿Z轴旋转$90^o$得到的旋转矩阵。由于-E和E等价，所以对任意一个t取负号，也会得到同样的结果。所以从E分解得到$R,t$，一共存在4个可能的解。
$ t^\wedge_1=UR_Z(\frac\pi2)\Sigma U^T,\ \ \ \ \ \ R_1=UR^T_Z(\frac\pi2)V^T \tag{6} $
$ t^\wedge_2=UR_Z(-\frac\pi2)\Sigma U^T,\ \ \ \ \ \ R_2=UR^T_Z(-\frac\pi2)V^T \tag{7} $
4个可能的解如上图P点，不过只有第一种解中，P在两个相机中都具有正的深度。因此只要把任意一点代入4种解中，检测该点在两个相机下的深度，就可以确定哪个解是正确的了。
但是根据线性方程解出的E可能不满足E的内在性质，即它的奇异值不一定为$[\sigma,\sigma,0]^T$的形式。所以在做SVD时会刻意把$\Sigma$矩阵调整成$[\sigma,\sigma,0]^T$形式。通常的做法是，对八点法求得的E进行SVD分解后，得到奇异值矩阵$\Sigma=diag(\sigma_1,\sigma_2,\sigma_3)$，不妨设$\sigma_1\geq\sigma_2\geq\sigma_3$，取式(8)。这相当于把求出来的矩阵投影到E所在的流形上。
$ E=Udiag(\frac{\sigma_1+\sigma_2}2,\frac{\sigma_1+\sigma_2}2,0)V^T \tag{8} $
当然，更简洁的做法是将奇异值矩阵取成$diag(1,1,0)$，因为E具有尺度等价性，所以这样做也是合理的。
7.3.3 单应矩阵
单应矩阵H（Homography）描述了两个平面之间的映射关系。若场景中的特征点都落在同一平面上（如墙、地面等），则可以通过单应性进行运动估计。在无人机携带的俯视相机或扫地机携带的顶视相机中比较常见。
单应矩阵通常描述处于共同平面上的一些点在两张图像之间的变换关系。考虑在图像$l_1$和$l_2$有一对匹配好的特征点$p_1$和$p_2$，这些特征点落在平面P上，设这个平面满足方程（9），整理得（10）。其中n为平面法向量，d为相机系原点到平面距离。
$ n^TP+d=0 \tag{9} $
$ -\frac{n^TP}d=1 \tag{10} $
由于$x_1=K^{-1}p_1, x_2=K^{-1}p_2$，得：
$ p_2=K(RP+t)=K(RP+t(-\frac{n^TP}d))=K(R-\frac{tn^T}d)P=K(R-\frac{tn^T}d)K^{-1}p_1 \tag{11} $
把中间这部分记为H，于是得到一个直接描述图像坐标$p_1$和$p_2$之间的变换：
$ H=K(R-\frac{tn^T}d)K^{-1} \tag{12} $
$ p_2=Hp_1 \tag{13} $
当特征点共面或者相机发生纯旋转时，基础矩阵的自由度下降，这就出现了所谓的退化（degenerate）。现实中的数据总包含一些噪声，这时候如果继续使用八点法求解基础矩阵，基础矩阵多余出来的自由度将会主要由噪声决定。为了能够避免退化现象造成的影响，通常我们会同时估计基础矩阵F和单应矩阵H，选择重投影误差比较小的那个作为最终的运动估计矩阵。
ORB-SLAM的Tracking中计算运动时，平面用H，非平面用F。
7.5 三角测量
三角测量是指，通过在两处观察同一个点的夹角，从而确定该点的距离。
按照对极几何中的定义，设$x_1,x_2$为两个特征点的归一化坐标，则满足（14）。已知$R,t$，想要求解两个特征点的深度$s_1,s_2$。当然，这两个深度可以分开求，比如要算$s_2$，则先对（14）两侧左乘$x^\wedge_1$，得（15）。该式左侧为0，右侧可看成$s_2$的方程。
$ s_1x_1=s_2Rx_2+t \tag{14} $
$ s_1x^\wedge_1x_1=0=s_2x^\wedge_1Rx_2+x^\wedge_1t \tag{15} $
由于噪声，我们估得的$R,t$不一定使（14）为零，所以更常见的做法是求最小二乘解而不是零解。
三角测量是平移得到的，纯旋转无法使用三角测量，因为对极约束将永远满足。
7.7 3D-2D: PnP
PnP(Perspective-n-Point)是求解3D到2D点对运动的方法，它描述了当知道n个3D空间点及其投影位置时，如何估计相机的位姿。
前面说到，2D-2D的对极几何方法需要8个以上的点对（以八点法为例），且存在着初始化、纯旋转和尺度的问题。然而，如果两张图像中其中一张特征点的3D位置已知，那么最少只需3个点对（需要至少一个额外点验证结果）就可以估计相机运动。
特征点的3D位置可以由三角化或者RGB-D相机的深度图确定。因此，在双目或RGB-D的视觉里程计中，我们可以直接使用PnP估计相机运动。而在单目视觉里程计中，必须先进行初始化，然后才能使用PnP。
PnP问题有很多种求解方式：P3P（3对点估计位姿）、DLT（直接线性变换）、EPnP（EfficientPnP）、UPnP、BA（Bundle Adjustment）等等。
7.7.1 直接线性变换
考虑某个空间点 $P=(X,Y,Z,1)^T$，在图像$l_1$中投影到特征点 $x_1=(u_1,v_1,1)^T$，定义增广矩阵$[R|t]$为一个$3\times4$的矩阵，展开形式如下：
用最后一行把s消去，得到两个约束（16）。为了简化表示，定义T的行向量（17）。
$ u_1=\frac{t_1X+t_2Y+t_3Z+t_4}{t_9X+t_{10}Y+t_{11}Z+t_{12}}\ ,\ v_1=\frac{t_5X+t_6Y+t_7Z+t_8}{t_9X+t_{10}Y+t_{11}Z+t_{12}} \tag{16} $
$ t_1=(t_1,t_2,t_3,t_4)^T\ ,\ t_2=(t_5,t_6,t_7,t_8)^T\ ,\ t_3=(t_9,t_{10},t_{11},t_{12})^T \tag{17} $
于是有（18）和（19）：
$ t^T_1P-t^T_3Pu_1=0 \tag{18} $
$ t^T_2P-t^T_3Pv_1=0 \tag{19} $
t是待求的变量，每个特征点提供了2个关于t的线性约束。假设共有N个特征点，则可以列出如下线性方程组。由于t一共12维，因此最少通过6对匹配点即可实现矩阵T的线性求解，这种方法称为直接线性变换（Direct Linear Transform，DLT）。当匹配点大于6对时，也可以用SVD等方法对超定方程求最小二乘解。
在DLT求解中，我们直接将T矩阵看成了12个未知数，忽略了它们之间的联系。因为旋转矩阵$R\in SO(3)$，用DLT求出的解不一定满足该约束，它是一个一般矩阵。对于旋转矩阵R，我们必须针对DLT估计的T左边$3\times3$的矩阵块，寻找一个最好的旋转矩阵对它进行近似，这可以由QR分解完成。相当于把结果从矩阵空间重新投影到$SE(3)$流形上，转换成旋转和平移两部分。
7.7.2 P3P
输入3对3D-2D匹配点，记3D点为A，B，C（在世界坐标系中的坐标），他们在相机成像平面上投影的2D点为a，b，c，如下图所示。此外，P3P还需要一对验证点，用于从可能解中选出正确的那一个，记验证点对为D-d，相机光心为O。
由余弦定理，得
$
OA^2+OB^2-2OA\cdot OB\cdot cos<a,b>=AB^2
\tag{20}
$
$ OB^2+OC^2-2OB\cdot OC\cdot cos<b,c>=BC^2 \tag{21} $
$ OA^2+OC^2-2OA\cdot OC\cdot cos<a,c>=AC^2 \tag{22} $
对以上三式除以$OC^2$，并记$x=\frac{OA}{OC}$，$y=\frac{OB}{OC}$，$v=\frac{AB^2}{OC^2}$，$uv=\frac{BC^2}{OC^2}$，$wv=\frac{AC^2}{OC^2}$，得
$ x^2+y^2-2xycos<a,b>-v=0 \tag{23} $
$ y^2+1^2-2ycos<b,c>-uv=0 \tag{24} $
$ x^2+1^2-2xcos<a,c>-wv=0 \tag{25} $
将式（23）中的v放到等式一边，代入后两式（24）（25），得（26）（27）。该方程组是x,y的二元二次方程，其他均已知。解析求解需要用吴消元法，较为复杂，最多可能得到4个解，可以用验证点计算最可能的解。得到ABC在相机坐标系下的3D坐标，然后根据3D-3D点对，计算相机运动R,t。
$ (1-u)y^2-ux^2-cos<b,c>y+2uxy\ cos<a,b>+1=0 \tag{26} $
$ (1-w)x^2-wy^2-cos<a,c>x+2wxy\ cos<a,b>+1=0 \tag{27} $
7.7.3 Bundle Adjustment
除了使用线性方法之外，我们还可以把PnP问题构建成一个定义于李代数上的非线性最小二乘问题。前面说的线性方法，往往是先求相机位姿，再求空间点位置 ，而非线性优化则是把它们都看成优化变量，放在一起优化。
在PnP中，Bundle Adjustment是一个最小化重投影误差问题。
考虑n个三维空间点P及其投影p，我们希望计算相机的位姿R,t，它的李代数表示为$\xi$。假设某空间点坐标为$P_i=[X_i,Y_i,Z_i]^T$，其投影的像素坐标为$u_i=[u_i,v_i]^T$。根据第5讲（相机与图像）的内容，像素位置与空间点位置关系如下：
写成矩阵形式就是(27)。由于相机位姿未知及观测点的噪声，该等式存在一个误差。因此我们把误差求和，构建最小二乘问题，然后寻求最好的相机位姿，使它最小化(28)。
$ s_iu_i=K\ exp(\xi ^\wedge)P_i \tag{28}$
$ \xi^*=arg\ min_\xi\frac12\sum^n_{i=1}||u_i-\frac1{s_i}K\ exp(\xi^\wedge)P_i||^2_2 \tag{29} $
该问题的误差项，是将像素坐标（观测到的投影位置）与3D点按照当前估计的位姿进行投影得到的位置相比得到的误差，所以称为重投影误差。
7.9 3D-3D: ICP
假设我们有一组配对好的3D点：$P={p_1,\cdots,p_n},\ P’={p’_1,\cdots,p’_n}$，我们想要找一个欧氏变换，使得：$\forall i,\ p_i=Rp’_i+t$。这个问题可以用迭代最近点（Iterative Closest Point，ICP）求解。和PnP类似，ICP的求解也分为两种方式：利用线性代数的求解（主要是SVD），利用非线性优化方式的求解（类似于Bundle Adjustment）。
7.9.1 SVD方法
定义第i对点的误差项（30），然后构建最小二乘问题，求使误差平方和达到极小的R,t（31）。
$ e_i=p_i-(Rp’_i+t) \tag{30} $
$ min_{R,t}\ J=\frac12\sum^n_{i=1}||(p_i-(Rp’_i+t))||^2_2 \tag{31} $
ICP可以分为以下3个步骤求解：
- 计算两组点的质心位置$p,p’$，然后计算每个点的去质心坐标：$q_i=p_i-p,\ q’_i=p’_i-p’$
- 根据以下优化问题计算旋转矩阵：$R^*=arg\ min_R\frac12\sum^n_{i=1}||q_i-Rq’_i||^2$
- 根据第2步的R计算t：$t^*=p-Rp’$
重点是R的计算，展开关于R的误差项，得（32）。其中第一项和R无关，第二项由于$R^TR=I$，也和R无关，因此实际上优化目标变为（33）。
\[\begin{equation} \frac12\sum^n_{i=1}\|\|q_i-Rq'_i\|\|^2=\frac12\sum^n_{i=1}q^T_iq_i+q'^T_iR^TRq'_i-2q^T_iRq'_i \tag{32} \end{equation}\] \[\begin{equation} \sum^n_{i=1}-2q^T_iRq'_i=\sum^n_{i=1}-tr(Rq'_iq^T_i)=-tr(R\sum^n_{i=1}q'_iq^T_i) \tag{33} \end{equation}\]接下来介绍怎样通过SVD解出上述问题中最优的R。先定义矩阵（34），W是一个$3\times3$的矩阵，对W进行SVD分解，得（35）。其中，$\Sigma$为奇异值组成得对角矩阵，对角元素从大到小排列，而U和V为对角矩阵。当W满秩时，R为（36），解得R后，按步骤3求解t即可。
$ W=\sum^n_{i=1}q_iq’^T_i \tag{34} $
$ W=U\Sigma V^T \tag{35} $
$ R=UV^T \tag{36} $
7.9.2 非线性优化方法
非线性优化以迭代的方式去找最优值，与PnP非常相似。以李代数表达位姿时，目标函数可以写成（37）。单个误差项关于位姿的导数在前面已推导（书中有，笔记没有），使用李代数扰动模型即可（38）。
$ min_\xi=\frac12\sum^n_{i=1}||(p_i-exp(\xi^\wedge)p’_i)||^2_2 \tag{37} $
$ \frac{\partial e}{\partial\delta\xi}=-(exp(\xi^\wedge)p’_i)^\odot \tag{38} $
于是在非线性优化中只需不断迭代，就能找到极小值。ICP问题存在唯一解或无穷多解的情况。在唯一解的情况下，只要能找到极小值解，那么这个极小值解就是全局最优值，因此不会遇到局部极小值而非全局最小的情况。这意味着ICP求解可以任意选定初始值，这是已匹配点时求解ICP的一大好处。
需要说明的是，我们这里讲的ICP是指已由图像特征给定了匹配的情况下进行位姿估计的问题。在匹配已知的情况下，这个最小二乘问题实际上具有解析解，所以并没有必要进行迭代优化。ICP的研究者们往往更加关心匹配未知的情况。不过，在RGB-D SLAM中，由于一个像素的深度数据可能测量不到，所以我们可以混合着使用PnP和ICP优化：对于深度已知的特征点，建模它们的3D-3D误差；对于深度未知的特征点，则建模3D-2D的重投影误差。于是，可以将所有的误差放在同一个问题中考虑，使得求解更加方便。
