视觉SLAM十四讲|解读《视觉SLAM十四讲》，带你一步一步入门视觉SLAM—— 第 7 讲　视觉里程计1 ( 下 ) 视觉SLAM十四讲|全书解读

上一篇博客我解读了如何根据已经匹配上的特征点对求解相机的运动《解读《视觉SLAM十四讲》，带你一步一步入门视觉SLAM—— 第 7 讲视觉里程计1 ( 中 )》，这一篇博客我们来看一下如果通过已经求得的相机运动恢复出特征点的空间位置，以及一些别的求解相机运动的方法。
解读三角测量两个图像 I 1 , I 2 I_1,I_2 I1?,I2?，它们之间的变换矩阵已知为 T 12 T_{12} T12?，它们图像上分别有一个特征点 p 1 , p 2 p_1,p_2 p1?,p2?，并且它们已经匹配成功了， p 1 , p 2 p_1,p_2 p1?,p2?在归一化平面上的坐标为 x 1 , x 2 \boldsymbol x_1,\boldsymbol x_2 x1?,x2?，那么可以列出如下等式:
s 1 x 1 = s 2 R 12 x 2 + t 12 (0) s_1 \boldsymbol x_1 = s_2 \boldsymbol R_{12} \boldsymbol x_2 + \boldsymbol t_{12} \tag 0 s1?x1?=s2?R12?x2?+t12?(0)根据前面的内容，我们已经求出来了 R 12 , t 12 \boldsymbol R_{12},\boldsymbol t_{12} R12?,t12?，将（０）式子左乘以 x 1 ? \boldsymbol x_1^{\bigwedge} x1??，得到下式：
s 1 x 1 ? x 1 = 0 = s 2 x 1 ? R 12 x 2 + x 1 ? t 12 (1) s_1\boldsymbol x_1^{\bigwedge} \boldsymbol x_1 = 0 = s_2 \boldsymbol x_1^{\bigwedge} \boldsymbol R_{12} \boldsymbol x_2 + \boldsymbol x_1^{\bigwedge} \boldsymbol t_{12} \tag 1 s1?x1??x1?=0=s2?x1??R12?x2?+x1??t12?(1)除了 s 1 , s 2 s_1,s_2 s1?,s2?是未知的，别的都是已知量，所以通过（０）（１）式可以求解出 s 1 , s 2 s_1,s_2 s1?,s2?，也就得到了特征点 p 1 , p 2 p_1,p_2 p1?,p2?“空间位置”。
注意：这里的空间位置是打引号的，它并不是真实空间位置。因为前面求解出来的平移向量 t 12 \boldsymbol t_{12} t12?没有尺度信息，所以整个等式都是没有尺度的，所以最后求的空间位置也是没有尺度的。

还记得书中说的吗？由于 t \boldsymbol t t没有尺度信息，所以一种做法是将第一帧的 t \boldsymbol t t归一化，另外一种做法是初始化的时候令特征点的平均深度为１，那么后续所有三角化产生的特征点深度值，其实都是对于初始化时候的相对值。
提高三角测量精度的方法

1.适当的增大平移距离；
2.提高图像的分辨率。

3D-2D：PnP 前面的内容中，介绍了两张图片当中的匹配点对，可以采用八对点求解对极约束，或者四对点求解单应矩阵，从而获得相机的运动，进而恢复出特征点的3D位置。但是这些方法存在着初始化、纯旋转和尺度问题。

如果我们知道一张图像中的特征点的深度信息，那么是否问题就会变得简单一点儿呢？是不是就不会有尺度和纯旋转等问题了呢？
直接线性变换已知空间中的一个点 P P P，它是相对于世界坐标系（请你要注意这一点），它的齐次坐标是 P = ( X , Y , Z , 1 ) T P=(X,Y,Z,1)^T P=(X,Y,Z,1)T，投影到图像 I 1 I_1 I1?中的特征点为 x 1 = ( u 1 , v 1 , 1 ) T \boldsymbol x_1=(u_1, v_1, 1)^T x1?=(u1?,v1?,1)T（以归一化平面齐次坐标表示），现在要求解 R , t \boldsymbol R,\boldsymbol t 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 ] (0) s \left[ \begin{matrix} u_1 \\ v_1 \\ 1 \end{matrix} \right] = \left[ \begin{matrix} t_1 & t_2 & t_3 & t_4 \\ t_5 & t_6 & t_7 & t_8 \\ t_{9} & t_{10} & t_{11} & t_{12} \end{matrix} \right] \left[ \begin{matrix} X \\ Y \\ Z \\ 1 \end{matrix} \right] \tag 0 s???u1?v1?1????=???t1?t5?t9??t2?t6?t10??t3?t7?t11??t4?t8?t12??????????XYZ1??????(0)对于（0）式子，如果你没有印象了，请看相机模型那一讲，它无非就是将一个世界坐标系的点，投影到相机坐标系下的过程。
通过简单的消元可以得到：
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 , (1) 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 1 u1?=t9?X+t10?Y+t11?Z+t12?t1?X+t2?Y+t3?Z+t4??,v1?=t9?X+t10?Y+t11?Z+t12?t5?X+t6?Y+t7?Z+t8??,(1)
将（1）中的两式写成向量的形式：
t 1 T P ? t 3 T P u 1 = 0 \boldsymbol t_1^T \boldsymbol P - \boldsymbol t_3^T \boldsymbol P u_1=0 t1T?P?t3T?Pu1?=0 t 2 T P ? t 3 T P v 1 = 0 \boldsymbol t_2^T \boldsymbol P - \boldsymbol t_3^T \boldsymbol P v_1=0 t2T?P?t3T?Pv1?=0
其中 t 1 T = [ t 1 , . . . , t 4 ] , t 2 T = [ t 5 , . . . , t 8 ] , t 3 T = [ t 9 , . . . , t 12 ] \boldsymbol t_1^T = [t_1,...,t_4],\boldsymbol t_2^T=[t_5,...,t_8],\boldsymbol t_3^T = [t_9,...,t_{12}] t1T?=[t1?,...,t4?],t2T?=[t5?,...,t8?],t3T?=[t9?,...,t12?]
上面的 t \boldsymbol t t就是我们待求解的量，一个特征点可以提供两个线性方程，只需要6个空间点，就可以解出 R , t \boldsymbol R,\boldsymbol t R,t，如果想要 R , t \boldsymbol R,\boldsymbol t R,t求得更准，则可以更多的点构建超定方程组，然后求解最小二乘解。
注意：直接线性求解由于是直接求解12个未知量，解出来的是一个一般的矩阵，实际上旋转矩阵 R \boldsymbol R R是有约束的，它必须是属于 S O ( 3 ) SO(3) SO(3)，此时还需要对这12个变量组成的矩阵做一些分解操作，才能得到最优的 R , t \boldsymbol R,\boldsymbol t R,t。
P3P 在直接线性变换中，我们可以选择6对匹配点，可以求解出相机的运动，其中每一对匹配点，其中一个是世界坐标系下的路标，一个是与它匹配上的特征点。

在P3P中，只需要３对匹配点，然后就可以通过ICP方法求解出 R , t \boldsymbol R,\boldsymbol t R,t。

P3P方法的原理很简单，大家直接看书上的示意图，但是我们需要好好理解一下这个应用场景。它是应用在这样的条件下：已知世界坐标系下的三个路标点，然后还知道这三个路标点在相机中的投影，至于是怎么找到这三个路标点对应到相机中，大家暂且先不要纠结这个，后续会有办法的。
Bundle Adjustment 除了上面介绍的直接线性变换和P3P之外，还可以采用一种更加通用的优化方法，将PnP问题构造成一个定义在李代数上的最小二乘问题。

Bundle Adjustment是SLAM中非常重要的一个工具，大家一定要好好理解并掌握它，我有很长一段时间一直无法理解到BA的真谛。

Bundle Adjustment也称作是光束平差法，通过名字啥信息也得不到。它的目的就是最小化一个重投影误差。
首先我们考虑有 n n n个三维空间点 P i = [ X i , Y i , Z i ] T \boldsymbol P_i=[X_i,Y_i,Z_i]^T Pi?=[Xi?,Yi?,Zi?]T，以及它们在成像平面的投影 u i = [ u i , v i ] T \boldsymbol u_i=[u_i,v_i]^T ui?=[ui?,vi?]T，当然我们已经知道了哪个空间点对应哪个投影点。根据空间点到相机的投影关系（第五讲的内容），可以得到下面这个等式：
s i [ u 1 v 1 1 ] = K e x p ( ξ ? ) [ X i Y i Z i 1 ] (2) s_i \left[ \begin{matrix} u_1 \\ v_1 \\ 1 \end{matrix} \right] = \boldsymbol K exp(\boldsymbol \xi^{\bigwedge}) \left[ \begin{matrix} X_i \\ Y_i \\ Z_i \\ 1 \end{matrix} \right] \tag 2 si????u1?v1?1????=Kexp(ξ?)?????Xi?Yi?Zi?1??????(2)(2)式子我们已经多次见到了，这次我们把变换矩阵写成了，四元数的形式，目的就是应用上四元数的特性，可以完成无约束的优化。再次给你提示，别忘记了 s i s_i si?表示的是当前点距离相机光心的距离。
将（２）式写成矩阵的形式：
s i u i = K e x p ( ξ ? ) P i (3) s_i \boldsymbol u_i = \boldsymbol K exp(\boldsymbol \xi^{\bigwedge}) \boldsymbol P_i \tag 3 si?ui?=Kexp(ξ?)Pi?(3)由于误差的存在（３）式不可能是一个完美的等式。咱们不妨再想一想，现在已知一个空间点和它在相机平面的投影，再多看两眼这个已知条件，如果小孔模型完美成立，没有噪声，那么（３）式就是严格的等式，但是真实情况是有噪声，所以相机感光原件上测量得到的投影坐标是不准确的，此时在看看等式（３）， P i . u i , K \boldsymbol P_i. \boldsymbol u_i, \boldsymbol K Pi?.ui?,K是常量，如果要想让等式尽可能的成立，那是不是只能通过微调 e x p ( ξ ? ) exp(\boldsymbol \xi^{\bigwedge}) exp(ξ?)，从而达到等式尽可能的成立。（如果你不明白重投影误差，你可以仔细读一下这段话）

如果用数学的语言描述，那就可以构建最小二乘问题，然后求解最好的相机位姿，使得投影误差最小化，也就是如下式：
ξ ? = arg ? min ? ξ 1 2 ∑ i = 1 n ∥ u i ? 1 s i K e x p ( ξ ? ) P i ∥ 2 2 (4) \boldsymbol \xi^* = \mathop{\arg\min}\limits_{\boldsymbol \xi} \frac{1}{2} \sum_{i=1}^n \|\boldsymbol u_i - \frac{1}{s_i} \boldsymbol K exp(\boldsymbol \xi^{\bigwedge}) \boldsymbol P_i \|_2^2 \tag 4 ξ?=ξargmin?21?i=1∑n?∥ui??si?1?Kexp(ξ?)Pi?∥22?(4)对于（4）式应该很好理解吧，我们将所有点的投影误差求和，然后求出一个使得误差之和最小的的位姿 ξ ? \boldsymbol \xi^{\bigwedge} ξ?。
吐槽：不知道是不是我理解上的问题，感觉作者这里对于重投影误差的讲解并不是很形象，尽管我已经理解了重投影误差，但是再去看作者书中的内容，还是觉得有一些不顺畅。可能是作者既要追求完整又要追求简单，所以就会造成一些理解问题。如果你看了作者的内容没有理解，看了我的解读也还没有理解，那我建议你多看看博客，一定要把这个弄明白，它十分有用。
写出了（4）式这样的最小二乘形式之后，就可以采用非线性优化求解最小值了，还记得我们前面的关于非线性优化的内容吗？（它也很重要，各个学科都在广泛使用）
下面我们来看一下这个最小二乘问题的求解过程：
在使用GN或者LM优化方法之前，我们需要知道每个误差项关于优化变量的导数，也就是要将误差项在很小范围内线性化：
e ( x + Δ x ) ≈ e ( x ) + J Δ x (5) \boldsymbol e(\boldsymbol x + \Delta \boldsymbol x) \approx \boldsymbol e(x)+J \Delta \boldsymbol x \tag 5 e(x+Δx)≈e(x)+JΔx(5)在这里求解出雅克比 J J J是尤为重要的，误差函数如果展开来，可以写成两个等式，六个未知数，那么雅克比 J J J就肯定是2x6的矩阵了（如果你不清楚，请看一下雅克比的定义）。
还记得李代数求导吗？我们可以对 ξ ? \boldsymbol \xi^{\bigwedge} ξ?左乘以一个微小扰动量 δ ξ \delta \boldsymbol \xi δξ，那么
? e ? δ ξ = lim ? δ ξ → 0 e ( δ ξ ? ξ ) δ ξ (6) \frac{\partial \boldsymbol e}{\partial \delta \boldsymbol \xi}=\lim_{\delta \boldsymbol \xi \to 0} \frac{e(\delta \boldsymbol \xi \bigoplus \boldsymbol \xi)}{\delta \boldsymbol \xi} \tag 6 ?δξ?e?=δξ→0lim?δξe(δξ?ξ)?(6)为了方便起见，根据链式法则，先取一个中间变量
P ′ = ( e x p ( ξ ? ) P ) 1 : 3 = [ X ′ , Y ′ , Z ′ ] T (7) \boldsymbol P'=(exp(\boldsymbol \xi^{\bigwedge}) \boldsymbol P)_{1:3}=[X',Y',Z']^T \tag 7 P′=(exp(ξ?)P)1:3?=[X′,Y′,Z′]T(7)上式子（7）只是将世界坐标系的空间点 P \boldsymbol P P转换到相机坐标系下 P ′ \boldsymbol P' P′，且只取出前3维。我们再次写出相机投影模型：
s u = K P ′ (8) s \boldsymbol u = \boldsymbol K P' \tag 8 su=KP′(8)利用（8）式的矩阵第三行消去 s s s，得到：
u = f x X ′ Z ′ + c x ,v = f y Y ′ Z ′ + c y (9) u = f_x \frac{X'}{Z'}+c_x,v=f_y \frac{Y'}{Z'}+c_y \tag 9 u=fx?Z′X′?+cx?,　v=fy?Z′Y′?+cy?(9)
所以对于（6）式加入中间变量之后的导数就变成了如下形式：
? e ? δ ξ = lim ? δ ξ → 0 e ( δ ξ ? ξ ) δ ξ = ? e ? P ′ ? P ′ ? δ ξ (10) \frac{\partial \boldsymbol e}{\partial \delta \boldsymbol \xi}=\lim_{\delta \boldsymbol \xi \to 0} \frac{e(\delta \boldsymbol \xi \bigoplus \boldsymbol \xi)}{\delta \boldsymbol \xi} = \frac{\partial \boldsymbol e}{\partial \boldsymbol P'}\frac{\partial \boldsymbol P'}{\partial \delta \boldsymbol \xi} \tag{10} ?δξ?e?=δξ→0lim?δξe(δξ?ξ)?=?P′?e??δξ?P′?(10)其中 ? e ? P ′ \frac{\partial \boldsymbol e}{\partial \boldsymbol P'} ?P′?e?根据（9）式子可以很容求出来， ? P ′ ? δ ξ \frac{\partial \boldsymbol P'}{\partial \delta \boldsymbol \xi} ?δξ?P′?可以根据第四讲的李代数求导得到。
将这两项导数相乘可以得到完整的误差函数的雅克比矩阵：
? e ? δ ξ = ? [ f x Z ′ 0 ? f x X ′ Z ′ 2 ? f x X ′ Y ′ Z ′ 2 f x ? f x X ′ 2 Z ′ 2 ? f x Y ′ Z ′ 0 f y Z ′ ? f y Y ′ Z ′ 2 ? f y ? f y Y ′ 2 Z ′ 2 f y X ′ Y ′ Z ′ 2 f y X ′ Z ′ ] (11) \frac{\partial \boldsymbol e}{\partial \delta \boldsymbol \xi} = - \left[\begin{matrix} \frac{f_x}{Z'}& 0 & -\frac{f_xX'}{Z'^2} & -\frac{f_xX'Y'}{Z'^2} &f_x - \frac{f_xX'^2}{Z'^2} & -\frac{f_xY'}{Z'} \\0 & \frac{f_y}{Z'} & -\frac{f_yY'}{Z'^2} & -f_y-\frac{f_yY'^2}{Z'^2} & \frac{f_yX'Y'}{Z'^2} & \frac{f_yX'}{Z'} \end{matrix}\right] \tag{11} ?δξ?e?=?[Z′fx??0?0Z′fy????Z′2fx?X′??Z′2fy?Y′???Z′2fx?X′Y′??fy??Z′2fy?Y′2??fx??Z′2fx?X′2?Z′2fy?X′Y′???Z′fx?Y′?Z′fy?X′??](11)这个推导过程，如果你愿意，我建议最好还是自己去手动推算一遍，非常重要的一个式子，以后不管学习哪个SLAM系统都可能用得上。
上面是对位姿求导，如果我们对特征点的空间位置求导，那么就可以对空间位置进行优化了，对于空间位置的求导依然采用链式法则：
? e ? δ P = ? e ? P ′ ? P ′ ? δ P (12) \frac{\partial \boldsymbol e}{\partial \delta \boldsymbol P}=\frac{\partial \boldsymbol e}{\partial \boldsymbol P'}\frac{\partial \boldsymbol P'}{\partial \delta \boldsymbol P} \tag{12} ?δP?e?=?P′?e??δP?P′?(12)对于（12）式的第一项，上面已经推导出来了，对于第二项，按照定义：
P ′ = e x p ( ξ ? ) P = R P + t (13) \boldsymbol P' = exp(\boldsymbol \xi^{\bigwedge}) \boldsymbol P=\boldsymbol R\boldsymbol P+\boldsymbol t \tag{13} P′=exp(ξ?)P=RP+t(13)对（13）式求导之后，就只剩下 R \boldsymbol R R，所以（12）式最后得到
? e ? δ P = [ f x Z ′ 0 ? f x X ′ Z ′ 2 0 f y Z ′ ? f y Y ′ Z ′ 2 ] R (14) \frac{\partial \boldsymbol e}{\partial \delta \boldsymbol P} = \left[\begin{matrix} \frac{f_x}{Z'}& 0 & -\frac{f_xX'}{Z'^2} \\0 & \frac{f_y}{Z'} & -\frac{f_yY'}{Z'^2} \end{matrix}\right] \boldsymbol R \tag{14} ?δP?e?=[Z′fx??0?0Z′fy????Z′2fx?X′??Z′2fy?Y′??]R(14)(11)(14)两个式子非常重要，请你务必要手动推导并且理解一下，后面对于SLAM的学习中，它俩发挥了很大的作用。如果你觉得推导的过程有点儿难，也请你按部就班多推导几次，最好能背住。
总结直接线性变换、P3P、BA 我们是时候将前面的内容总结一下了：

首先我们先考虑了，我们知道很多对空间点和图像的特征点对应，我们采用直接线性变换，求解出 R , t \boldsymbol R,\boldsymbol t R,t.
然后又提出了使用三对空间点和图像特征点（已经匹配上），通过相似三角形求出空间点在相机坐标系下的3D坐标，然后通过别的方法求解出 R , t \boldsymbol R,\boldsymbol t R,t
上面两种方在通用性、鲁棒性上都有一些问题，所以我们又提出了Bundle Adjustment方法，BA优化的方法通用性、鲁棒性更强。
可以结合上面三种方法，前两种方法为BA优化提供初始值，然后用BA优化对 R , t \boldsymbol R,\boldsymbol t R,t或者空间点位置进行进一步的优化。

实践在实践：求解PnP部分，作者代码的大致思路是：

1.提取特征点；
2.匹配特征点；
3.通过PnP求解位姿的初始值；
4.然后将初始值带入BA优化中，得到更好的结果。

请注意，如果你使用的是《视觉SLAM十四讲》第一版，而你g2o安装的是最新版，你会遇到代码报错，主要是因为g2o更新导致的。你需要按照下面的方式修改代码（注释的代码作者原来的代码，改成下面形式就可以运行了）：

typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block; // pose 维度为 6, landmark 维度为 3//Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse(); // 线性方程求解器 std::unique_ptr linearSolver ( new g2o::LinearSolverCSparse()); //Block* solver_ptr = new Block ( linearSolver ); //std::unique_ptr solver_ptr ( new Block ( linearSolver)); std::unique_ptr solver_ptr ( new Block ( std::move(linearSolver))); // 矩阵块求解器//g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr); g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( std::move(solver_ptr));

我建议你把g2o的源代码看一看，大概了解一下整体，不用在意实现的细节，这样对你大有好处。
3D-3D：ICP 这部分的理论知识，应该不难了，书中的推导很详细，我就不在赘述了。在运行对应的代码的时候，同样还是因为g2o版本更新的问题会保存，修改如下：

// 初始化g2o typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block; // pose维度为 6, landmark 维度为 3// Block::LinearSolverType* linearSolver = new g2o::LinearSolverEigen(); // 线性方程求解器 std::unique_ptr linearSolver ( new g2o::LinearSolverEigen()); //Block* solver_ptr = new Block( linearSolver ); // 矩阵块求解器 std::unique_ptr solver_ptr ( new Block ( std::move(linearSolver))); //g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr ); g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton ( std::move(solver_ptr)); g2o::SparseOptimizer optimizer; optimizer.setAlgorithm( solver );

这一讲的内容非常多，而且是SLAM前端的核心，如果你不把这一讲给理解明白，可以说你还没有入SLAM的大门，所以务必反复研读这一讲的内容，如果你愿意挑战，可以把习题部分给完成，特别是打星号的代码题，如果能够独立完成，我觉得这一讲你肯定没有问题了，后面的学习会越来越容易，加油，一起进步！