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

最新推荐文章于 2024-04-03 11:47:53 发布

一点儿也不萌的萌萌

最新推荐文章于 2024-04-03 11:47:53 发布

阅读量686

点赞数 1

分类专栏：视觉SLAM十四讲全书解读文章标签：算法 slam 人工智能机器学习图论

本文链接：https://blog.csdn.net/u011341856/article/details/103856660

版权

视觉SLAM十四讲全书解读专栏收录该内容

19 篇文章 93 订阅

订阅专栏

解读

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

Pose Graph的优化

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

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

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

实践

g2o原生位姿图

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

李代数上的位姿图优化

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

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

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;
}