后端2

上讲我们重点介绍了以BA为主的图优化，BA能够精确地优化每个位资与特征点位置。不过更大的场景中，大量特征点存在会严重降低计算效率，导致计算量越来越大以至无法实时化。本讲介绍两种在更大场景下的后端优化方法：位姿图。

位资图（Pose Graph）

Pose Graph的意义

带有相机位姿核空间点的图优化称为BA，能够有效的求解大规模的定位与建图问题。但是随着时间的流逝，机器人的运动轨迹越来越长。像BA这样的方法，计算效率就会不断下降。根据前面的讨论，我们发现特征点在优化问题上占据了绝大多数部分。而实际上，经过若干次观测之后，那些收敛的特征点，空间位置估计就会收敛到一个值保持不动，二发散的外点则通常看不到了。对收敛点再进行优化，似乎有些费力不讨好。因此我们更倾向于在优化几次之后把特征点固定住，只把他们看作位姿估计的约束，而不再实际地优化它们的位置估计。
沿着这个思路往下走，我们发现：是否能够完全不管路标，而只管轨迹呢？我们完全可以构造一个只有轨迹的图优化，而位姿点之间的边，可以由两个关键帧之间通过特征匹配后得到的运动估计来给定初始值。不同的是，一旦初始值估计完成，我们就不再优化那些路标点的位置，而只关心所有的相机位姿之间的联系了。通过这种方式，我们省去了大量的特征点优化的计算，只保留关键帧的轨迹，从而构成了所谓的位资图（Pose Graph）。

我们知道在BA中，特征点数量远大于位姿节点的数量。一个关键帧往往关联数百个关键点，而实时BA的最大计算规模，即使利用稀疏性，在当前主流的CPU上一般也就几万个点左右。这就限制了SLAM的应用场景。所以，当机器人在更长的时间和空间中运动时，必须考虑一些解决方式：要么使用滑动窗口那样，丢掉一些历史数据；要么像POse Graph的做法那样，舍弃对路标点的优化，只保留Pose之间的边，使用Pose Graph.

Pose Graph的优化

那么，Pose Graph图优化中的节点和边都是什么意思?这里的节点表示相机位姿，以$\xi_1,\cdots,\xi_n$来表达。而边，则是两个位姿节点之间相对运动的估计，该估计可能来自于特征点法或直接法，不管如何，我们估计了，比如说$\xi_i$和$\xi_j$之间的一个运动$\triangle\xi_{ij}$。该运动可以由若干种表达方式，我们取比较自然的一种：

$$$\triangle\xi_{ij}=\xi_i^{-1}\circ\xi_j=ln(exp((-\xi_i)^{\wedge})exp(\xi_j^{\wedge}))^{\vee}$$
或者按李群的写法：
$$\triangle T_{ij}=T_i^{-1}T_j$$
按照图哟花的思路来看，实际当中该等式不会精确地成立，因此我们设立最小二传误差，然后和以往一样，讨论误差关于优化变量的导数。这里我们把上式的$\triangle T_{ij}$移至等式右侧，构建误差项$e_{ij}$:
$$e_{ij}=ln(\triangle T_{ij}T_i^{-1}T_j)^{\vee}=ln(exp((-\xi_{ij})^{\wedge})exp((-\xi_i)^{\wedge})exp(\xi_j^{\wedge}))^{\vee}$$
注意优化变量由两个：$\xi_i$和$\xi_j$,因此我们求$e_{ij}$关于这两个变量的导数。按照李代数的求导方式，给$\xi_i$和$\xi_j$各一个左扰动:$\delta\xi_i$和$\delta\xi_j$。于是误差变为：
$$\hat e_{ij}=ln(T_{ij}^{-1}T_i^{-1}exp((-\delta \xi_i)^{\wedge})exp(\delta\xi_j^{\wedge})T_j)^{\vee}.$$
该式中，两个扰动项被夹在了中间。为了利用BCH近似，我们希望把扰动项移至左侧或右侧。
$$exp((Ad(T)\xi)^{\wedge})=Texp(\xi^{\wedge})T^{-1}$$
稍加改变有：
$$exp(\xi^{\wedge})T=Texp((Ad(T^{-1})\xi)^{\wedge})$$
该式表示，通过引入一个伴随项，我们能够交换扰动项左右侧的T。利用他，我们可以将扰动挪到最右（当然最左也可以）。导出右乘形势的雅克比矩阵（挪到左边时形成左乘）：
//TODO