视觉SLAM十四讲第十一讲笔记

视觉SLAM十四讲第十一讲笔记

带有相机位姿和空间点的图优化称为BA，能够有效地求解大规模的定位与建图问题。但是，随着时间的流逝，机器人的运动轨迹将越来越长，地图规模也将不断增长。像BA这样的方法，计算效率就会（令人担忧地）不断下降。 本讲主要描述了一个方法，用于更大型场景下使用的后端优化算法。

一、位姿图(pose grap)

BA中的特征点数量远大于位姿节点。因此在Pose grap中，舍弃了对路标点的优化，只保留Pose之间的边，使用Pose grap。Pose grap图优化的节点表示相机位姿，以${ϵ}_1,...,{ϵ}_n$表示，边表示两个位姿节点之间相对运动的估计。 这个运动估计可以写为:
$$\Delta {ϵ}_{ij}={ϵ}_i^{-1}\circ {ϵ}_j=ln(exp((-{ϵ}_i{)}^{\wedge })exp({ϵ}_j^{\wedge }){)}^{\vee }$$

或者按照李群的写法:
$$\Delta T_{ij}=T_i^{-1}{T}_j$$

按照图优化的思路来看，实际当中该等式不会精确地成立，因此我们设立最小二乘误差，然后和以往一样，讨论误差关于优化变量的导数。这里我们把上式的$\Delta T_{ij}$移至等式右侧，构建误差$e_{ij}$:
$$\begin{array}{rl}{e}_{ij}& =ln(\Delta T_{ij}^{-1}{T}_i^{-1}{T}_j{)}^{\vee }\\ & =ln(exp((-{ϵ}_{ij}{)}^{\wedge })exp((-{ϵ}_i{)}^{\wedge })exp({ϵ}_j^{\wedge }){)}^{\vee }\end{array}$$

求$e_{ij}$关于这两个变量的导数。按照李代数的求导方式，给${ϵ}_i$和${ϵ}_j$各一个左扰动:$\delta {ϵ}_i$和$\delta {ϵ}_j$，于是误差变为:
$${\hat{e}}_{ij}=ln(T_{ij}^{-1}{T}_I^{-1}exp((-\delta {ϵ}_i{)}^{\wedge })exp(\delta {ϵ}_j^{\wedge })T_j{)}^{\vee }$$

上面式子用BHC近似，然后对变量进行求导，具体不再细化，简言之，所有的位姿顶点和位姿—位姿边构成了一个图优化，本质上是一个最小二乘问题。