第11讲 后端2
11.1 位姿图
11.1.1 Pose Graph的意义
带有相机位姿和空间点的图优化称为BA，能够有效地求解大规模的定位与建图问题。但是，随着时间的流逝，计算效率会不断下降。根据前面的讨论，我们发现特征点在优化问题中占据了绝大部分。而实际上，经过若干次观测之后，那些收敛的特征点，空间位置估计会收敛至一个值保持不动，而发散的外点则通常看不到了。因此，可以在优化几次之后就把特征点固定住，只把它们看作位姿估计的约束，而不再实际地优化它们的位置估计。
我们完全可以构建一个只有轨迹不管路标的图优化，而位姿节点之间的边，可以由两个关键帧之间通过特征匹配之后得到的运动估计来给定初始值。不同的是，一旦初始估计完成，我们就不再优化那些路标点的位置，而只关心所有的相机位姿之间的联系了。通过这种方式，我们省去了大量的特征点优化的计算，只保留了关键帧的轨迹，从而构建了所谓的位姿图（Pose Graph）：
11.1.2 Pose Graph的优化
Pose Graph的节点表示相机位姿，以$\xi_1,\cdots,\xi_n$来表达；边则是两个位姿节点之间相对运动的估计，如说$\xi_i$和$\xi_j$之间的一个运动$\Delta\xi_{ij}$（1）或按李群的写法（2）。
$ \Delta\xi_{ij}=\xi_i^{-1}\circ\xi_j=\ln(\exp((-\xi_i)^\wedge)\exp(\xi_j^\wedge))^\vee \tag{1} $
$ \Delta T_{ij}=T_i^{-1}T_j \tag{2} $
所有的位姿顶点和位姿—位姿边构成了一个图优化，本质上是一个最小二乘问题，优化变量为各个顶点的位姿，边来自于位姿观测约束。记$\varepsilon$为所有边的集合，那么总体目标函数为（3）。
\[\begin{equation} min_\xi\frac12\sum_{i,j\in\varepsilon}e^T_{ij}\Sigma^{-1}_{ij} e_{ij} \tag{3} \end{equation}\]我们依然可以用高斯牛顿法、列文伯格—马夸尔特方法等求解此问题，除了用李代数表示优化位姿以外，别的都是相似的。根据先前的经验，这自然可以用Ceres或g2o进行求解。
