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