解读《视觉SLAM十四讲》，带你一步一步入门视觉SLAM—— 第 11 讲 后端 2

解读

在上一讲的解读中，我介绍了基于图的后端优化，我们也了解了SLAM优化问题的稀疏性，基于稀疏性我们可以在很短的时间内对特征点位置相机姿态进行优化。不过更大的场景下，大量的特征点的存在会严重的降低计算效率，导致计算量越来越大以至于无法实时化1 。
　　
　　经过观察发现，在做BA优化的时候，特征点的优化占了计算资源的大部分，而实际上经过几轮优化之后，空间位置估计会收敛到一个值保持不动，而发散的外点通常也就看不到了。因此常常的做法是经过几次优化之后，将特征点固定住，然后只对位姿进行优化。

Pose Graph的优化

通过上述方法，我们极大的降低了计算量，于是乎按照上面的思路我们可以构建一个只有轨迹的图优化，而位姿节点之间的边，可以由两个关键帧之间通过特征匹配之后得到的运动估计来给定初始值。于是这种图优化叫做：位姿图（Pose Graph）。
　　
　　相信经过前面对于图优化的多次实践，你应该有这样的印象了，那就图优化需要定义顶点、边、优化项，已经雅克比矩阵。那么就让我们沿着这个思路来推导出图优化（Pose Graph）的公式吧！
　　
　　非线性优化最首要的就是定义误差项，我们先来看一下Pose Graph的误差项：
　　
　　图优化的节点就是待优化的变量，那么对于Pose Graph的节点就是相机的位姿，我们以ξ1,⋯ ,ξn\xi_1,\cdots,\xi_nξ1​,⋯,ξn​来表示。而边则是两个位姿节点之间相对运动的估计，比方说在位姿ξi,ξj\xi_i,\xi_jξi​,ξj​之间有一个运动，我们经过前面说的特征点法或者直接法也好，得到了一个运动估计ξij\xi_{ij}ξij​，我们求出来的运动肯定和实际的真实运动之间存在误差，我们目的就像要让这个误差最小。
　　
　　我们将上面的运动，表示出来：
Δξij=ξi−1∘ξj=ln⁡(exp⁡((−ξi)∧exp⁡(ξj)∧)∨(0) \Delta \xi_{ij}=\xi_i^{-1} \circ \xi_j=\ln(\exp((-\xi_i)^{\wedge}\exp(\xi_j)^{\wedge})^{\vee} \tag 0 Δξij​=ξi−1​∘ξj​=ln(exp((−ξi​)∧exp(ξj​)∧)∨(0)你应该能想到（0）式是咋来的吧！
　　
　　理论上来说，（０）式的左右两边应该是相等的，但是实际上我们Δξij\Delta \xi_{ij}Δξij​是根据VO的方法计算出来的，它并不会那么幸运没有一点儿误差就刚好等于真实值。
　　上面的（０）式子，我们写成李群的形式，就是变换矩阵的形式：
Tij=Ti−1Tj(1) T_{ij}=T_i^{-1}T_{j} \tag 1 Tij​=Ti−1​Tj​(1)既然（0）式因为误差不能相等，那么我们就将它们的差值构建成误差的形式：
eij=ln⁡(Tij−1Ti−1Tj)∨=ln⁡(exp⁡((−ξij)∧)exp⁡((−ξi)∧)exp⁡(ξj∧))∨(2) e_{ij}=\ln(T_{ij}^{-1}T_i^{-1}T_j)^{\vee} \\ =\ln(\exp((-\xi_{ij})^{\wedge})\exp((-\xi_i)^{\wedge})\exp(\xi_j^{\wedge}))^{\vee} \tag2 eij​=ln(Tij−1​Ti−1​Tj​)∨=ln(exp((−ξij​)∧)exp((−ξi​)∧)exp(ξj∧​))∨(2)还记得变换矩阵的性质吗？它对加法不封闭，所以求它的误差项，只能用乘法！

我们要优化的变量是ξj,ξj\xi_j,\xi_jξj​,ξj​，所以就必须得求得误差项关于它的导数。根据李代数的左扰动模型，我们可以得到误差项关于这两个变量的偏导数：
∂eij∂δξi=−Jr−1(eij)Ad(Tj−1) \frac{\partial e_{ij}}{\partial \delta \xi_i}=-\mathcal{J}_r^{-1}(e_{ij})Ad(T_j^{-1}) ∂δξi​∂eij​​=−Jr−1​(eij​)Ad(Tj−1​)∂eij∂δξj=Jr−1(eij)Ad(Tj−1)(3) \frac{\partial e_{ij}}{\partial \delta \xi_j}=\mathcal{J}_r^{-1}(e_{ij})Ad(T_j^{-1}) \tag3 ∂δξj​∂eij​​=Jr−1​(eij​)Ad(Tj−1​)(3)由于Jr\mathcal{J}_rJr​的计算过于复杂，所以这里采用近似：
Jr−1(eij)≈I+12[ϕe∧ρe∧0ϕe∧](4) \mathcal{J}_r^{-1}(e_{ij}) \approx I + \frac{1}{2} \left[ \begin{matrix} \phi_e^{\wedge} & \rho_e^{\wedge} \\ 0 & \phi_e^{\wedge} \end{matrix} \right] \tag 4 Jr−1​(eij​)≈I+21​[ϕe∧​0​ρe∧​ϕe∧​​](4)　　上式子当中，SE(3)上的扰动项的李代数为：δξ=[δρ,δϕ]\delta \xi=[\delta \rho,\delta \phi]δξ=[δρ,δϕ]，书中没有解释（4）式变量的意义，实际上在第四讲中对于SE(3)上李代数的求导中，用到了上面的两个变量。
　　
　　有了每一个误差项的雅克比矩阵之后，我们就可以构建Pose Graph优化了。本质上还是一个最小二乘问题，优化变量为各个顶点（相机位姿），边来自于位姿观测约束。记ε\varepsilonε为所有边的集合，那么总体目标函数为：
min⁡ξ12∑i,j∈εeijTΣij−1eij(5) \min_\xi \frac{1}{2} \sum_{i,j \in \varepsilon} e_{ij}^{T} \Sigma_{ij}^{-1} e_{ij} \tag 5 ξmin​21​i,j∈ε∑​eijT​Σij−1​eij​(5)上式中Σij−1\Sigma_{ij}^{-1}Σij−1​是信息矩阵，它表示的是协方差矩阵的逆。信息矩阵可以理解成我们对误差各分量重视程度的不一样。

既然代价函数也有了，每一项的雅克比也有了，那么只要构造成图优化，就可以进行优化求解了。

实践

g2o原生位姿图

这部分的代码，主要就是将

sphere.g2o

中的数据读入内存中，然后使用g2o自带的顶点和边进行优化，代码非常简单，就不在多解释了。已经到了11讲了，我觉得大家应该对g2o的用法差不多熟悉了。

李代数上的位姿图优化

这个代码的主要目的是通过李代数，自己定义顶点和边。

Jr−1(eij)\mathcal{J}_r^{-1}(e_{ij})Jr−1​(eij​)的构造代码如下：

Matrix6d JRInv( SE3 e )
{
Matrix6d J;
J.block(0,0,3,3) = SO3::hat(e.so3().log());
J.block(0,3,3,3) = SO3::hat(e.translation());
J.block(3,0,3,3) = Eigen::Matrix3d::Zero(3,3);
J.block(3,3,3,3) = SO3::hat(e.so3().log());
J = J*0.5 + Matrix6d::Identity();
return J;
}

如果你还是没有习惯g2o的使用方式，那么你就先模仿着写，多实践几次你就会使用了，对着这种循规蹈矩的编程方式，一旦你习惯了，就会非常容易。

*因子图优化初步

由于因子图，我还在学习中，所以这部分内容，我暂时就不解读了，等我学习了这部分内容之后，我再写！

这里推荐大家一本比较全面的讲因子图优化的书：
《机器人感知：因子图在SLAM中的应用》

如果你对因子图没兴趣，你可以跳过这部分，因为这部分的应用并没有图优化广泛。

最好的年纪不要辜负最美的自己.

1. 《视觉SLAM十四讲 从理论到实践》 ↩︎

• 点赞
• 收藏
• 分享
• 文章举报

一点儿也不萌的萌萌 发布了32 篇原创文章 · 获赞 71 · 访问量 3591 私信 关注