参考
A micro Lie theory for state estimation in robotics
manif issues 116
常用求导公式
| Operation | 左雅克比 | 右雅克比 |
|---|---|---|
| X − 1 \mathcal{X}^{-1} X−1 | J X X − 1 = − I \mathbf{J}_{\mathcal{X}}^{\mathcal{X}^{-1}}=\mathbf{-I} JXX−1=−I | J X X − 1 = − A d X \mathbf{J}_{\mathcal{X}}^{\mathcal{X}^{-1}}=-\mathbf{Ad}_{\mathcal{X}} JXX−1=−AdX |
| X ∘ Y \mathcal{X}\circ\mathcal{Y} X∘Y | J X X ∘ Y = I ∣ J Y X ∘ Y = A d X \mathbf{J}_{\mathcal{X}}^{\mathcal{X} \circ \mathcal{Y}}=\mathbf{I}\mid\mathbf{J}_{\mathcal{Y}}^{\mathcal{X} \circ \mathcal{Y}}=\mathbf{Ad}_{\mathcal{X}} JXX∘Y=I∣JYX∘Y=AdX | J X X ∘ Y = A d Y − 1 ∣ J Y X ∘ Y = I \mathbf{J}_{\mathcal{X}}^{\mathcal{X} \circ \mathcal{Y}}=\mathbf{Ad}_{\mathcal{Y}}^{-1}\mid\mathbf{J}_{\mathcal{Y}}^{\mathcal{X} \circ \mathcal{Y}}=\mathbf{I} JXX∘Y=AdY−1∣JYX∘Y=I |
| E x p ( τ ) Exp(\boldsymbol{\tau}) Exp(τ) | J τ E x p ( τ ) = J l ( τ ) \mathbf{J}_{\boldsymbol{\tau}}^{Exp(\boldsymbol{\tau})}=\mathbf{J}_{l}(\boldsymbol{\tau}) JτExp(τ)=Jl(τ) | J τ E x p ( τ ) = J r ( τ ) \mathbf{J}_{\boldsymbol{\tau}}^{Exp(\boldsymbol{\tau})}=\mathbf{J}_{r}(\boldsymbol{\tau}) JτExp(τ)=Jr(τ) |
| L o g ( X ) Log(\mathcal{X}) Log(X) | J X L o g ( X ) = J l − 1 ( τ ) \mathbf{J}_{\mathcal{X}}^{Log(\mathcal{X})}=\mathbf{J}_{l}^{-1}(\boldsymbol{\tau}) JXLog(X)=Jl−1(τ) | J X L o g ( X ) = J r − 1 ( τ ) \mathbf{J}_{\mathcal{X}}^{Log(\mathcal{X})}=\mathbf{J}_{r}^{-1}(\boldsymbol{\tau}) JXLog(X)=Jr−1(τ) |
| Plus | J X τ ⊕ X = A d E x p ( τ ) ∣ J τ τ ⊕ X = J l ( τ ) \mathbf{J}_{\mathcal{X}}^{\boldsymbol{\tau}\oplus\mathcal{X}}=\mathbf{Ad}_{Exp(\boldsymbol{\tau})}\mid\mathbf{J}_{\boldsymbol{\tau}}^{\boldsymbol{\tau}\oplus\mathcal{X}}=\mathbf{J}_{l}(\boldsymbol{\tau}) JXτ⊕X=AdExp(τ)∣Jττ⊕X=Jl(τ) | J X X ⊕ τ = A d E x p ( τ ) − 1 ∣ J τ X ⊕ τ = J r ( τ ) \mathbf{J}_{\mathcal{X}}^{\mathcal{X}\oplus\boldsymbol{\tau}}=\mathbf{Ad}_{Exp(\boldsymbol{\tau})}^{-1}\mid\mathbf{J}_{\boldsymbol{\tau}}^{\mathcal{X}\oplus\boldsymbol{\tau}}=\mathbf{J}_{r}(\boldsymbol{\tau}) JXX⊕τ=AdExp(τ)−1∣JτX⊕τ=Jr(τ) |
| Minus | J X X ⊖ Y = − J r − 1 ( τ ) ∣ J Y X ⊖ Y = J l − 1 ( τ ) \mathbf{J}_{\mathcal{X}}^{\mathcal{X}\ominus\mathcal{Y}}=-\mathbf{J}_{r}^{-1}(\boldsymbol{\tau})\mid\mathbf{J}_{\mathcal{Y}}^{\mathcal{X}\ominus\mathcal{Y}}=\mathbf{J}_{l}^{-1}(\boldsymbol{\tau}) JXX⊖Y=−Jr−1(τ)∣JYX⊖Y=Jl−1(τ) | J X X ⊖ Y = J r − 1 ( τ ) ∣ J Y X ⊖ Y = − J l − 1 ( τ ) \mathbf{J}_{\mathcal{X}}^{\mathcal{X}\ominus\mathcal{Y}}=\mathbf{J}_{r}^{-1}(\boldsymbol{\tau})\mid\mathbf{J}_{\mathcal{Y}}^{\mathcal{X}\ominus\mathcal{Y}}=-\mathbf{J}_{l}^{-1}(\boldsymbol{\tau}) JXX⊖Y=Jr−1(τ)∣JYX⊖Y=−Jl−1(τ) |
公式中的伴随矩阵
对于SO3:
A
d
R
=
R
\mathbf{Ad_{R}} = \mathbf{R}
AdR=R
对于SE3:
M
=
[
R
t
0
1
]
A
d
M
=
[
R
⌊
t
⌋
×
R
0
R
]
\begin{aligned} \mathbf{M} &= \begin{bmatrix} \mathbf{R} & \mathbf{t}\\ \mathbf{0} & 1 \end{bmatrix} \\ \mathbf{Ad_{M}} &= \begin{bmatrix} \mathbf{R} & \left \lfloor \mathbf{t} \right \rfloor_{\times}\mathbf{R} \\ \mathbf{0} & \mathbf{R} \end{bmatrix} \end{aligned}
MAdM=[R0t1]=[R0⌊t⌋×RR]
公式中的左右雅克比 J r \mathbf{J}_r Jr、 J l \mathbf{J}_l Jl
对于SO3:
对于SE3:
