g2o之SE3-SE3约束

最新推荐文章于 2022-11-25 20:41:13 发布

魏新slam

最新推荐文章于 2022-11-25 20:41:13 发布

阅读量1.5k

点赞数 3

分类专栏：图优化(graph optimization) 文章标签： slam 算法

本文链接：https://blog.csdn.net/qq_17693963/article/details/104820415

版权

图优化(graph optimization) 专栏收录该内容

1 篇文章 1 订阅

订阅专栏

g2o之SE3-SE3约束

1. g2o之SE3-SE3约束

1. g2o之SE3-SE3约束

假设位姿图中某两帧的位姿为 $T_i$ 和 $T_j$ ，且由两帧之间的测量约束为 $T_{ij}$ ，则有下式：
$\Delta T=T_{ij}^{-1}T_i^{-1}T_j \tag{1.1}$
利用对数映射，可得到该约束对应的残差为：
$e_{ij}=\ln(\Delta T)^{\vee}=\ln(T_{ij}^{-1}T_i^{-1}T_j)^{\vee}\in R^6 \tag{1.2}$
在利用Gauss-Newton或Levenberg-Marquat算法求解非线性最小二乘问题时，需要求解残差向量 $e_{ij}$ 相对于SE节点 $T_i$ 和 $T_j$ 的Jacobian。为求解Jacobian，给 $T_i$ 添加左扰动 $\exp(\xi_i^{\wedge})$ ，带入公式1.2则有：
$\Delta T(\xi_i)=T_{ij}^{-1}T_i^{-1}\exp(-\xi_i^{\wedge})T_j \tag{1.3}$
$e_{ij}(\xi_i)=\ln(T_{ij}^{-1}T_i^{-1}\exp(-\xi_i^{\wedge})T_j)^{\vee} \tag{1.4}$
则残差 $e_{ij}$ 相对于 $T_i$ 的Jacobian可表示为：
$\frac{\partial e_{ij}}{\partial T_i}=\frac{\partial e_{ij}(\xi_i)}{\partial \xi_i}_{\mid \xi_i=0} \tag{1.5}$
利用如公式1.6所示的伴随矩阵的性质，可以将公式1.5化简为公式1.7:
$A\exp(b^\wedge)=\exp(A_{adj}b^\wedge)A \tag{1.6}$
$e_{ij}(\xi_i)=\ln(\exp(-((T_{ij}^{-1}T_i^{-1})_{adj}\xi_i^\wedge))T_{ij}^{-1}T_i^{-1}T_j)^{\vee} \tag{1.7}$
将公式1.2带入公式1.7，可进一步化简为：
$e_{ij}(\xi_i)=\ln(\exp(-((T_{ij}^{-1}T_i^{-1})_{adj}\xi_i^\wedge))\exp(e_{ij}^{\wedge}))^\vee \tag{1.8}$
为进一步化简上式，回顾一下BCH公式：
$\exp(a^\wedge+b^\wedge)\approx\exp(J_l(b)a^\wedge)exp(b^\wedge) \tag{1.9}$
式中，a为无穷小量， $J_l$ 为SE(3)的左Jacobian。则也有：
$\exp(a^\wedge)\exp(b^\wedge)\approx\exp(J_l^{-1}(b)a^\wedge+b^\wedge) \tag{1.10}$
利用公式1.10，可以将公式1.8化简为：
$e_{ij}(\xi_i)\approx\ln(\exp((e_{ij}-J_l^{-1}(e_{ij})(T_{ij}^{-1}T_i^{-1})_{adj}\xi_i)^\wedge))^\vee \tag{1.11}$
进一步化简为：
$e_{ij}(\xi_i)=e_{ij}-J_{l(e_{ij})}^{-1}(T_{ij}^{-1}T_i^{-1})_{adj}\xi_i \tag{1.12}$
则 $e_{ij}$ 相对于 $T_i$ 的Jacobian等于：
$\frac{\partial e_{ij}}{\partial T_i}=\frac{\partial e_{ij}(\xi_i)}{\partial \xi_i}_{\mid \xi_i=0}=-J_{l(e_{ij})}^{-1}(T_{ij}^{-1}T_i^{-1})_{adj} \tag{1.13}$