拆分基础slam（2）-前端视觉里程计
二
- 前端几何原理
-
- 对极几何 2D-2D
- PNP 3D-2D
-
- 直接线性变换（DLT）
- 非线性优化：Bundle Adjustment
- P3P
- ICP(Iterative Closest Point) 3D-3D
-
- 线性优化方法
- 非线性优化方法
- 总结
前端几何原理
在前一章中，我讲了一个例子：有两帧图像我用什么方法来对相机姿态做一个估计，这种情况一般是单目相机。如果是双目相机或者RGB—D相机，这种可以获取到深度的相机那么处理方式就有区别。
对极几何 2D-2D
在前一章中我主要讲了对极几何的求解归一化坐标的方法，使用了经典的八对点法，其实也可以使用五点法（相机有六个移动自由度，但有一个尺度等价，跟九点转八点原理类似），但是这种方法会增加计算的复杂度，所以我们更倾向于使用八点法。这种方法用的两张图像都是2D的归一化坐标所以叫做2D-2D，具体内容可以参考上一篇文章：
链接: 静态相机前端.
PNP 3D-2D
PnP方法解决的是当我们知道n个3D空间点和他们的投影位置时，求解姿态的问题。当我们2D-2D时，只知道归一化坐标，但是3D-2D时其中一张图上的特征点深度是知道的，这个时候需要的点对就会从八对减少成为三对。
直接线性变换（DLT）
在空间中又个点的坐标为P=（X,Y,Z,1），现在这个点投影到图像中坐标为（u,v,1），这个转换关系中就需要包含我们要求解到R和t，那么可以建立如下所示的关系式：
s ( u 1 v 1 1 ) = ( t 1 t 2 t 3 t 4 t 5 t 6 t 7 t 8 t 9 t 10 t 11 t 12 ) ( X Y Z 1 ) s\left( %左括号 \begin{array}{ccc} %该矩阵一共3列，每一列都居中放置 u_{1}\\ %第一行元素 v_{1}\\ %第二行元素 1\\ \end{array} \right)=\left( %左括号 \begin{array}{ccc} %该矩阵一共3列，每一列都居中放置 t_{1} \quad t_{2}\quad t_{3}\quad t_{4}\\ %第一行元素 t_{5} \quad t_{6}\quad t_{7}\quad t_{8}\\ %第二行元素 t_{9} \quad t_{10}\quad t_{11}\quad t_{12}\\ \end{array} \right)\left( %左括号 \begin{array}{ccc} %该矩阵一共3列，每一列都居中放置 X\\ %第一行元素 Y\\ %第二行元素 Z\\ 1 \end{array} \right) s⎝⎛u1v11⎠⎞=⎝⎛t1t2t3t4t5t6t7t8t9t10t11t12⎠⎞⎝⎜⎜⎛XYZ1⎠⎟⎟⎞
上式中由于左侧是归一化坐标，所以s可以用X、Y、Z消掉，u和v两个未知量就可以用两个方程表示：
u 1 = t 1 X + t 2 Y + t 3 Z + t 4 t 9 X + t 10 Y + t 11 Z + t 12 v 1 = t 5 X + t 6 Y + t 7 Z + t 8 t 9 X + t 10 Y + t 11 Z + t 12 u_{1}=\frac{t_{1}X+t_{2}Y+t_{3}Z+t_{4}}{t_{9}X+t_{10}Y+t_{11}Z+t_{12}}\qquad v_{1}=\frac{t_{5}X+t_{6}Y+t_{7}Z+t_{8}}{t_{9}X+t_{10}Y+t_{11}Z+t_{12}} u1=t9X+t10Y+t11Z+t12t1X+t2Y+t3Z+t4v1=t9X+t10Y+t11Z+t12t5X+t6Y+t7Z+t8
在上面两个方程中，t是我们需要求的，其中包含的就是R和t的信息，T矩阵一共有12个未知数，所以在DLT方法中我们需要六对匹配点来建立12个方程来求解。
非线性优化：Bundle Adjustment
直接线性优化是将求解姿态问题转化为解线性方程组。这种方法流程是先求得姿态R，t再求空间点位置，DLT就是这个思路。非线性优化将姿态和空间点同时作为优化变量，因为关系不是用线性方程描述的所以叫非线性优化。BA是一种常用的方法，我介绍的重点放在思路，数学含量过高的内容会省略易懂。
BA的思路是最小化重投影误差：
这个图我上一节提到过，P11和P2的差距叫做重投影误差，我们就需要建立关系来最小化，我建立P和P11点的关系如下：
s i [ u i v i 1 ] = K e x p ( ξ ^ ) [ X i Y i Z i 1 ] + e s_{i}\left[ %左括号 \begin{array}{ccc} %该矩阵一共3列，每一列都居中放置 u_{i}\\ %第一行元素 v_{i}\\ %第二行元素 1\\ \end{array} \right]=Kexp(\hat \xi)\left[ %左括号 \begin{array}{ccc} %该矩阵一共3列，每一列都居中放置 X_{i}\\ %第一行元素 Y_{i}\\ %第二行元素 Z_{i}\\ 1 \end{array} \right]+e si⎣⎡uivi1⎦⎤=Kexp(ξ^)⎣⎢⎢⎡XiYiZi1⎦⎥⎥⎤+e
上式对初学者来说难以理解，左侧是空间点的归一化坐标，右侧是空间点乘以李代数（姿态）后加上一个误差，这个李代数冒号后变成了反对称矩阵，再指数变换后得到李群也就是外参姿态变换。我们建立的优化关系就是最小化这个误差。我们对误差建立最小二乘问题，寻找相机最好的位姿来最小化误差：
ξ ∗ = a r g m i n 1 2 ∑ i = 1 n ∣ ∣ u i − 1 s i K e x p ( ξ ^ ) P i ∣ ∣ 2 2 \xi^*=argmin\frac{1}{2}\sum\limits_{i = 1}^n||u_{i}-\frac{1}{s_{i}}Kexp(\hat \xi)P_{i}||_{2}^2 ξ∗=argmin21i=1∑n∣∣ui−si1Kexp(ξ^)Pi∣∣22
i表示第i个点，将一批点都放进来进行这个优化就叫做批调整BA。要求解这个问题需要的是数值分析等学科的数学知识。用高斯牛顿或者LM方法对这个方程求解。我们可以通过线性化得到重投影误差关于相机位姿李代数的一阶关系，还有重投影误差关于空间点P的导数，我们得到了这两个优化关系后就可以在方程中对其优化了。
P3P
这是另一种PNP方法，这种方法只使用三对匹配点，利用的是空间中相似三角形的几何关系。
现在由余弦定理建立关系表达式：
O A 2 + O B 2 − 2 O A ⋅ O B ⋅ c o s < a , b > = A B 2 OA^{2}+OB^{2}-2OA·OB·cos
O B 2 + O C 2 − 2 O B ⋅ O C ⋅ c o s < b , c > = B C 2 OB^{2}+OC^{2}-2OB·OC·cos
O A 2 + O C 2 − 2 O A ⋅ O C ⋅ c o s < a , b > = A C 2 OA^{2}+OC^{2}-2OA·OC·cos
整理上式，并且按照图中的关系对公式进行整理：
x 2 + y 2 − 2 x ⋅ y ⋅ c o s < a , b > − v = 0 （ ∗ ） x^{2}+y^{2}-2x·y·cos
y 2 + 1 − 2 y c o s < b , c > − u v = 0 y^{2}+1-2ycos
x 2 + 1 − 2 x c o s < a , c > = 0 x^{2}+1-2xcos
我们用第一个式子消掉二三式子中的v：
（ 1 − u ） y 2 − u x 2 − c o s < b , c > y + 2 u x y c o s < a , b > + 1 = 0 （1-u）y^{2}-ux^2-cos
（ 1 − w ） y 2 − w y 2 − c o s < a , c > y + 2 w x y c o s < a , b > + 1 = 0 （1-w）y^{2}-wy^2-cos
这个式子是关于x和y的一个二元二次方程，其他的量像w，u和cos都可以用已知的三维坐标和角度信息得出来。所以我们仅用三个点建立了求x和y的两个等式。得到了x和y后我们看（*）式子，我们可以解出v来，通过v，我们就知道了OC是多少，那么OAOB就知道了。我们就由此计算出了所有点信息。此时就是把3D点投影到2D归一化坐标那个计算公式求得Rt。
ICP(Iterative Closest Point) 3D-3D
ICP指的是用匹配好的两组点来估计空间运动。3D-3D的意思就是我们知道了三位空间中一组匹配好的点（如果涉及了图像怎么能叫3D呢），同样，我们的目的是计算相机位姿，那么自然而然就是如下点变换公式：
p i = R p i ′ + t p_{i}=Rp_{i}^{'}+t pi=Rpi′+t
此处也有两种方法，一是线性而是非线性优化。
线性优化方法
先说线性优化，我们建立方程组：
m i n J = 1 2 ∑ i = 1 n ∣ ∣ p i − ( R p i ′ + t ) ∣ ∣ 2 2 minJ=\frac{1}{2}\sum\limits_{i = 1}^n||p_{i}-(Rp_{i}^{'}+t)||_{2}^2 minJ=21i=1∑n∣∣pi−(Rpi′+t)∣∣22
在上面这个式子中未知量是相机的位姿Rt，我们的直接优化项就是Rt。这个公式很容易和PnP问题中的非线性优化BA做对比，两个优化的式子很相似，但跟PnP问题类似，线性解法先解出Rt再计算空间点。
我省略求解方程其中的数学过程，否则会增加主干内容以外的内容。简要来说就是三步：
q i = p i − p q i ′ = p i ′ − p ′ q_{i}=p_{i}-p \qquad q_{i}^{'}=p_{i}^{'}-p^{'} qi=pi−pqi′=pi′−p′
上式中没下标的两个P代表P的质心，就是每个点的平均位置，第一步的意思是取出一个点，计算这个点的偏移量。
R ∗ = a r g m a x 1 2 ∑ i = 1 n ∣ ∣ q i − ( R q i ′ ) ∣ ∣ 2 2 R^{*}=argmax\frac{1}{2}\sum\limits_{i = 1}^n||q_{i}-(Rq_{i}^{'})||_{2}^2 R∗=argmax21i=1∑n∣∣qi−(Rqi′)∣∣22
第二步的意思是把所有点取出来，进行第一步计算出偏移量，然后针对每个点优化R使其满足误差最小。
t ∗ = p − R p ′ t^{*}=p-Rp^{'} t∗=p−Rp′
第三步利用刚才计算出来的R和P的质心，计算t。
从此ICP问题的线性解法说完了。数学公式推导可以参考教材，我只需要讲清楚大家在应用的时候是什么流程就行了。
非线性优化方法
与BA的思路很类似，都是求解：
ξ ∗ = a r g m i n 1 2 ∑ i = 1 n ∣ ∣ p i − e x p ( ξ ^ ) P i ∣ ∣ 2 2 \xi^*=argmin\frac{1}{2}\sum\limits_{i = 1}^n||p_{i}-exp(\hat \xi)P_{i}||_{2}^2 ξ∗=argmin21i=1∑n∣∣pi−exp(ξ^)Pi∣∣22
总结
我前端提到的这几种方法从几种已知情况下分析了如何求解相机的位姿。首先是单目两张图，这种情况下使用对极几何用常用的八点法或者五点法可以求得E。分解后得Rt。3D投影到2D情况下首先可以用类似于八点法的线性方法求解，需要六对点；BA优化方法可以同时优化空间点和位姿，P3P可以用三对点三角函数解决姿态估计。3D-3D的ICP方法可以解决的是已知3D空间中两个点匹配的情况下求解位姿，线性方法利用最小二乘法求解位姿最优值。