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}.\]