从零学习SLAM（位姿图）

视觉SLAM笔记

 

1. Pose Graph 的意义

带有相机位姿和空间点的图优化称为 BA，能够有效地求解大规模的定位与建图问题
但是，随着时间的流逝，机器人的运动轨迹将越来越长，地图规模也将不断增长
像 BA 这样的方法，计算效率就会（令人担忧地）不断下降

根据前面的讨论，发现特征点在优化问题中占据了绝大多数部分
而实际上，经过若干次观测之后，那些收敛的特征点，空间位置估计就会收敛至一个值保持不动
而发散的外点则通常看不到了
对收敛点再进行优化，似乎是有些费力不讨好的

因此，更倾向于在优化几次之后就把特征点固定住，只把它们看作位姿估计的约束
而不再实际地优化它们的位置估计

沿着这个思路往下走，会发现：是否能够完全不管路标，而只管轨迹呢？
完全可以构建一个只有轨迹的图优化
而位姿节点之间的边，可以由两个关键帧之间通过特征匹配之后得到的运动估计来给定初始值
不同的是，一旦初始估计完成，就不再优化那些路标点的位置，而只关心所有的相机位姿之间的联系了
通过这种方式，省去了大量的特征点优化的计算，只保留了关键帧的轨迹
从而构建了所谓的位姿（Pose Graph）

在 BA 中，特征点数量远大于位姿节点的数量，一个关键帧往往关联了数百个关键点
而实时 BA 的最大计算规模，即使利用稀疏性，在当前的主流 CPU 上一般也就是几万个点左右
这就限制了 SLAM 应用场景
所以，当机器人在更长的时间和空间中运动时，必须考虑一些解决方式：

1. 要么像滑动窗口法那样，丢弃一些历史数据
2. 要么像 Pose Graph 的做法那样，舍弃对路标点的优化，只保留 Pose 之间的边

---

2. Pose Graph 的优化

关于 Pose Graph 图优化中的节点和边
这里的节点表示相机位姿，以 ξ1,……,ξn 来表达
而边，则是两个位姿节点之间相对运动的估计，该估计可能来自于特征点法或直接法
但不管如何，估计了，比如说 ξi 和 ξj 之间的一个运动 ∆ξij
该运动可以有若干种表达方式，取比较自然的一种：

或按李群的写法：

按照图优化的思路来看，实际当中该等式不会精确地成立
因此设立最小二乘误差，然后和以往一样，讨论误差关于优化变量的导数

这里，把上式的 ∆Tij 移至等式右侧，构建误差 eij：

注意优化变量有两个： ξi 和 ξj
因此求 eij 关于这两个变量的导数
按照李代数的求导方式，给 ξi 和 ξj 各一个左扰动： δξi 和 δξj
于是误差变为：

该式中，两个扰动项被夹在了中间
为了利用 BCH 近似，希望把扰动项移至式子左侧或右侧
暂时把它当作想当然的东西：

稍加改变，有：

该式表明，通过引入一个伴随项，能够“交换”扰动项左右侧的 T
利用它，可以将扰动挪到最右（当然最左亦可），导出右乘形式的雅可比矩阵（挪到左边时形成左乘）：

因此，按照李代数上的求导法则，求出了误差关于两个位姿的雅可比矩阵
关于Ti 的：

以及关于 Tj 的：

由于 se(3) 上的左右雅可比 Jr 形式过于复杂，通常取它们的近似
如果误差接近于零，就可以设它们近似为 I 或：

理论上来说，即使在优化之后，由于每条边给定的观测数据并不一致，误差通常也不见得近似于零
所以简单地把这里的 Jr 设置为 I 会有一定的损失

了解雅可比求导后，剩下的部分就和普通的图优化一样了
简而言之，所有的位姿顶点和位姿——位姿边构成了一个图优化
本质上是一个最小二乘问题，优化变量为各个顶点的位姿，边来自于位姿观测约束

记 E 为所有边的集合，那么总体目标函数为：

依然可以用 Gauss-Newton、 Levenberg-Marquardt 等方法求解此问题
除了用李代数表示优化位姿以外，别的都是相似的
根据先前的经验，这自然可以用 Ceres 或 g2o进行求解

 

---