SLAM——位姿图优化

Posted on Edited on In

SLAM中另外一个用到的最多的后端优化方法叫做位姿图（Pose Graph）优化。想象一下，对于路标的优化，可能进行几次之后就已经收敛了，这时候每次插入一个帧都再次进行一次BA仿佛有点用力过猛。而且实际中，路标的数量远远大于位姿数量，因此BA在大规模建图时，它的计算量可能会越来越大，使得实时计算变得困难。这里我们介绍的位姿图优化，就是省去了对路标的优化，仅仅调整位姿的一种做法。

我们将pose graph的优化转换成图的问题，那么图的节点就是一个个位姿，用$\xi_1,…,\xi_n$来表示，而边则是两个位姿之间相对运动的估计，这个估计可能来自与特征点法或者是直接法。比如$\xi_i,\xi_j$之间一个相对运动$\Delta \xi_ {ij}$，则：
$$
\Delta \xi_ {ij} = \xi_i^{-1}\circ \xi_j = \ln(\exp((-\xi_i)^{\hat{} }) \exp (\xi_j^{\hat{} }))^{\hat{} }
$$
或者按照李群的写法：
$$
T_ {ij} = T_ {i}^{-1} T_j.
$$
我们知道，实际中上式不会精确成立，因此我们需要设立最小二乘误差，然后讨论关于优化变量的导数。这里我们将上式的$T_ {ij}$移到右侧，为了让其满足误差最小为0的设定，加上一个$\ln$:
$$
e_ {ij} = \ln(T_ {ij}^{-1}T_i^{-1}T_j)^{\hat{} } = \ln(\exp((-\xi_ {ij})^{\hat{} })\exp((-\xi_ {i})^{\hat{} })\exp(\xi_j^{\hat{} }))^{\hat {} }
$$
值得注意的是这里的优化变量有两个：$\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)^{\hat{} })\exp(\delta\xi_j)^{\hat{} }T_j)^{\hat{} }
$$
我们希望将扰动项移到左侧或者右侧，需要利用到一个伴随性质：
$$
\exp((Ad(T)\xi)^{\hat{} }) = T\exp(\xi^{\hat{} })T^{-1}.
$$
稍加改变，得到：
$$
\exp(\xi^{\hat{} })T = T\exp((Ad(T^{-1})\xi)^{\hat{} }).
$$
这说明通过引入一个伴随项，我们能够交换扰动项左右侧的$T$，利用它可以将扰动项移到最右，导出右乘形式的雅科比矩阵：
$$
\begin{aligned} \hat e_ {ij} &=\ln\left (T_ {ij}^{-1}T_i^{-1}\exp((-\delta\xi_i)^{\hat{} })\exp(\delta\xi_j^{\hat{} })T_j\right)^{\hat{} }\ &=\ln\left(T_ {ij}^{-1}T_i^{-1}T_j \exp\left((-Ad(A_j^{-1})\delta\xi_i)^{\hat{} }\right)\exp\left((Ad(T_j^{-1})\delta \xi_j)^{\hat{} }\right)\right)\ &\approx \ln(T_ {ij}^{-1}T_i^{-1}T_j[I - (Ad(T_j^{-1})\delta \xi_i)^{\hat{} } + (Ad(T_j^{-1})\delta\xi_j)^{\hat{} }])^{\hat{} }\ &\approx e_ {ij}+\frac{\partial e_ {ij} }{\partial \delta \xi_i}\delta \xi_i + \frac{\partial e_ {ij} }{\partial \delta \xi_j}\delta \xi_j \end{aligned}
$$
因此，按照李代数上的求导规则，我们求出了误差关于两个位姿的雅科比矩阵。关于$T_i$的：
$$
\frac{\partial e_ {ij} }{\partial \delta \xi_i} = -\mathcal{J}r^{-1}(e {ij})Ad(T_j^{-1}).
$$
关于$T_j$的：
$$
\frac{\partial e_ {ij} }{\partial \delta \xi_i} = \mathcal{J}r^{-1}(e {ij})Ad(T_j^{-1}).
$$
这部分的理解有点困难，可以回顾之前的李群李代数。之前也说过，由于李群（se(3)）上的雅科比矩阵形式过于复杂，我们通常取近似。如果误差接近于0，可以取近似：
$$
\mathcal{J}r^{-1}(e {ij}) \approx I + \frac 1 2 \begin{bmatrix} \phi e ^{\hat{} } & \rho e^{\hat{} }\ 0 & \phi e ^{\hat{} } \end{bmatrix}.
$$
了解了雅科比计算之后，其余的部分就是普通的图优化了。记$\varepsilon$为所有边的集合，总体的目标函数为：
$$
\min\xi \frac{1}{2} \sum {i,j \in \varepsilon} e {ij}^T\Sigma_ {ij}^{-1}e_ {ij}.
$$
我们依然可以利用列文伯格或者高斯牛顿法来解决这个问题。

Note

伴随性质：

  • $$SO(3) R \exp(p^{\hat{} })R^T = \exp((Rp)^{\hat{} }).$$
  • $$SE(3) T\exp(\xi^{\hat{} })T^{-1} = \exp((Ad(T)\xi)^{\hat{} }).$$
    其中： $$Ad(T) = \begin{bmatrix} R & t^{\hat{} }R\ 0 & R \end{bmatrix}.$$