Derivation of Jacobians in g2o::EdgeSE3Expmap

Jun 19, 2020

I has been asked about how to derive the Jacobians in g2o::EdgeSE3Expmap. Here is my derivation (note that $T_i$ and $T_j$ are the state of pose $i$ and $j$, $e$ is the error function, and $\bar{T}_{ij}$ is the measurment):

\[\begin{aligned} e &= Log(T_j^{-1} \bar{T}_{ij} T_i) \\ e(\delta \xi_i) &= Log(T_j^{-1} \bar{T}_{ij} Exp(\delta \xi_i) T_i) \\ &= Log(T_j^{-1} \bar{T}_{ij} Exp(\delta \xi_i) (T_j^{-1} \bar{T}_{ij})^{-1}T_j^{-1} \bar{T}_{ij} T_i) \\ &= Log(Exp(Adj( T_j^{-1} \bar{T}_{ij}) \delta \xi_i ) T_j^{-1} \bar{T}_{ij} T_i) \\ &= Log(Exp(Adj( T_j^{-1} \bar{T}_{ij}) \delta \xi_i ) Exp(e)) \\ &= Adj( T_j^{-1} \bar{T}_{ij}) \delta \xi_i + e \\ \frac{\partial e}{\partial \delta \xi_i} &= Adj( T_j^{-1} \bar{T}_{ij}) \\ e(\delta \xi_j) &= Log(T_j^{-1} Exp(-\delta \xi_j) \bar{T}_{ij}T_i) \\ &= Log(T_j^{-1}\bar{T}_{ij} T_i \, (\bar{T}_{ij}T_i)^{-1} Exp(-\delta \xi_j) \bar{T}_{ij}T_i) \\ &= Log(Exp(e) \, T_i^{-1}\bar{T}_{ij}^{-1} Exp(-\delta \xi_j) (T_i^{-1}\bar{T}_{ij}^{-1})^{-1}) \\ &= Log(Exp(e) Exp(- Adj(T_i^{-1}\bar{T}_{ij}^{-1})\delta \xi_j)) \\ &= e - Adj(T_i^{-1}\bar{T}_{ij}^{-1})\delta \xi_j\\ \frac{\partial e}{\partial \delta \xi_j} &= - Adj(T_i^{-1}\bar{T}_{ij}^{-1}) \end{aligned}\]
License (CC) BY-NC-SA | Subscribe RSS | Email sansui_z@126.com