位姿图优化

引入

在SLAM的优化过程中，特征点在优化问题中消耗了绝大部分时间。实际上，经过若干次迭代之后，收敛的特征点位置变化很小，发散的外点则已被剔除。对收敛点再进行优化，似乎是有些费力不讨好的。因此，我们更倾向于在优化几次之后就把特征点固定住，而不再实际地优化它们的位置估计。
这种把优化变量去除特征点，只剩下位姿的优化问题就构成了位姿图优化。

问题建模

在两个位姿$T_i$和$T_j$之间有一个过度位姿$T_{ij}$即满足以下关系：
$$T_i{T}_{ij}=T_j$$

两个位姿构成了结点，过度位姿构成了位姿图的边。
由于存在噪声，这个式子不会严格相等，于是上式两边左乘$T_i^{-1}{T}_{ij}^{-1}$：
$$I=T_i^{-1}{T}_{ij}^{-1}{T}_j$$
之后转换成李代数，定义误差：
$$e_{ij}=ln(T_i^{-1}{T}_{ij}^{-1}{T}_j{)}^{\vee }$$
理论$e_{ij}=0$，但由于噪声等误差的存在而并不等于0。

分析计算

使用李代数求导的扰动模型：
$${\hat{e}}_{ij}=\ln {\left({\mathbf{T}}_{ij}^{-1}{\mathbf{T}}_i^{-1}\exp \left((-\delta {\mathbf{\xi }}_i{)}^{\wedge }\right)\exp \left(\delta {\mathbf{\xi }}_j^{\wedge }\right){\mathbf{T}}_j\right)}^{\vee }$$
为了使用BCH近似，想把扰动项移到左边或者右边，使用以下的伴随公式：
$$\exp \left((\text{Ad}(\mathbf{T})\mathbf{\xi }{)}^{\wedge }\right)=\mathbf{T}\exp \left({\mathbf{\xi }}^{\wedge }\right){\mathbf{T}}^{-1}$$
即：
$$\{\begin{array}{ll}{\mathbf{T}}_{\mathbf{j}}\exp \left((-\text{Ad}({\mathbf{T}}_{\mathbf{j}}^{-1})\mathbf{\delta }{\mathbf{\xi }}_{\mathbf{i}}{)}^{\wedge }\right){{\mathbf{T}}_{\mathbf{j}}}^{-1}=\exp \left({-\mathbf{\delta }{\mathbf{\xi }}_{\mathbf{i}}}^{\wedge }\right)& \\ {\mathbf{T}}_{\mathbf{j}}\exp \left((\text{Ad}({\mathbf{T}}_{\mathbf{j}}){\mathbf{\xi }}_{\mathbf{j}}{)}^{\wedge }\right)=\exp \left({\mathbf{\delta }{\mathbf{\xi }}_{\mathbf{j}}}^{\wedge }\right){\mathbf{T}}_{\mathbf{j}}& \end{array}$$
代入计算得：
$$\begin{gathered} {\hat{e}}_{ij}=\ln {\left(T_{ij}^{-1}{T}_i^{-1}{T}_j\exp \left((-\text{Ad}(T_j^{-1})\delta {\xi }_i{)}^{\wedge }\right)\exp \left((\text{Ad}(T_j^{-1})\delta {\xi }_j{)}^{\wedge }\right)\right)}^{\vee } \\ \approx \ln {\left(T_{ij}^{-1}{T}_i^{-1}{T}_j\left[I-(\text{Ad}(T_j^{-1})\delta {\xi }_i{)}^{\wedge }+(\text{Ad}(T_j^{-1})\delta {\xi }_j{)}^{\wedge }\right]\right)}^{\vee } \\ \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{gathered}$$
通过比较最后两行的式子，我们求出了误差关于两个位姿的雅可比矩阵：
$$\{\begin{array}{ll}\frac{\partial e_{ij}}{\partial \delta {\xi }_i}=-J_r{e}_{ij}\text{Ad}(T_j^{-1})& \\ \frac{\partial e_{ij}}{\partial \delta {\xi }_j}=J_r{e}_{ij}\text{Ad}(T_j^{-1})& \end{array}$$
由于se(3)上的左右雅可比$J_r/J_l$形式过于复杂，我们通常取它们的近似。如果误差接近零就可以认为它们近似为I或:
$$J_r^{-1}(e_{ij})\approx I+\frac{1}{2}\left[\begin{array}{cc}{\varphi }_e^{\wedge }& {\rho }_e^{\wedge }\\ 0& {\varphi }_e^{\wedge }\end{array}\right].$$
记$\mathcal{E}$为所有边的集合，那么总体目标函数为:
$$E_{ij}=\min \frac{1}{2}\sum _{i,j\in \mathcal{E}}{e}_{ij}^T{\Sigma }_{ij}^{-1}{e}_{ij}.$$
接下来就可以按照前面的最小二乘问题的方法进行求解。用高斯牛顿法等方法。