李群李代数常用结论
- e x p ( ⌊ δ θ ⌋ × ) = I + ⌊ δ θ ⌋ × exp(\left\lfloor\delta\boldsymbol{\theta}\right\rfloor_{\times})=\mathbf{I}+\left\lfloor\delta\boldsymbol{\theta}\right\rfloor_{\times} exp(⌊δθ⌋×)=I+⌊δθ⌋×
- e x p ( ⌊ δ θ ⌋ × ) R = R e x p ( ⌊ R T δ θ ⌋ × ) exp(\left\lfloor\delta\boldsymbol{\theta}\right\rfloor_{\times})\mathbf{R}=\mathbf{R}exp(\left\lfloor \mathbf{R}^{T}\delta\boldsymbol{\theta}\right\rfloor_{\times}) exp(⌊δθ⌋×)R=Rexp(⌊RTδθ⌋×)
- R e x p ( ⌊ δ θ ⌋ × ) = e x p ( ⌊ R δ θ ⌋ × ) R \mathbf{R}exp(\left\lfloor\delta\boldsymbol{\theta}\right\rfloor_{\times})=exp(\left\lfloor\mathbf{R}\delta\boldsymbol{\theta}\right\rfloor_{\times})\mathbf{R} Rexp(⌊δθ⌋×)=exp(⌊Rδθ⌋×)R
- l o g ( e x p ( ⌊ θ ⌋ × ) e x p ( ⌊ δ θ ⌋ × ) ) = J r − 1 ( θ ) δ θ + θ log(exp(\left\lfloor\boldsymbol{\theta}\right\rfloor_{\times})exp(\left\lfloor\delta\boldsymbol{\theta}\right\rfloor_{\times}))=\mathbf{J}_{r}^{-1}(\boldsymbol{\theta})\delta\boldsymbol{\theta}+\boldsymbol{\theta} log(exp(⌊θ⌋×)exp(⌊δθ⌋×))=Jr−1(θ)δθ+θ
- l o g ( e x p ( ⌊ δ θ ⌋ × ) e x p ( ⌊ θ ⌋ × ) ) = J l − 1 ( θ ) δ θ + θ log(exp(\left\lfloor\delta\boldsymbol{\theta}\right\rfloor_{\times})exp(\left\lfloor\boldsymbol{\theta}\right\rfloor_{\times}))=\mathbf{J}_{l}^{-1}(\boldsymbol{\theta})\delta\boldsymbol{\theta}+\boldsymbol{\theta} log(exp(⌊δθ⌋×)exp(⌊θ⌋×))=Jl−1(θ)δθ+θ
- R 1 ⊖ R 2 = l o g ( R 2 − 1 R 1 ) \mathbf{R}_{1}\ominus\mathbf{R}_{2}=log(\mathbf{R}_{2}^{-1}\mathbf{R}_{1}) R1⊖R2=log(R2−1R1)
- R e x p ( ⌊ δ θ ⌋ × ) R − 1 = e x p ( R ⌊ δ θ ⌋ × R − 1 ) \mathbf{R}exp(\left\lfloor\delta\boldsymbol{\theta}\right\rfloor_{\times})\mathbf{R}^{-1}=exp(\mathbf{R}\left\lfloor\delta\boldsymbol{\theta}\right\rfloor_{\times}\mathbf{R}^{-1}) Rexp(⌊δθ⌋×)R−1=exp(R⌊δθ⌋×R−1)
- R ⌊ δ θ ⌋ × R − 1 = ⌊ R δ θ ⌋ × \mathbf{R}\left\lfloor\delta\boldsymbol{\theta}\right\rfloor_{\times}\mathbf{R}^{-1} = \left\lfloor\mathbf{R}\delta\boldsymbol{\theta}\right\rfloor_{\times} R⌊δθ⌋×R−1=⌊Rδθ⌋×
R − 1 R^{-1} R−1求导(右乘)
∂ R − 1 ∂ δ θ = ( R exp ( ⌊ δ θ ⌋ × ) ) − 1 ⊖ R − 1 δ θ = ( 6 ) l o g ( R e x p ( ⌊ − δ θ ⌋ × ) R − 1 ) δ θ = ( 7 ) l o g ( e x p ( R ⌊ − δ θ ⌋ × R − 1 ) ) δ θ = l o g ( e x p ( ⌊ − R δ θ ⌋ × ) ) δ θ = − R \begin{aligned} \frac{\partial \mathbf{R}^{-1}}{\partial \delta\boldsymbol{\theta}} &= \frac{\mathbf{(R\exp(\left\lfloor\delta\boldsymbol{\theta}\right\rfloor_{\times}))^{-1}\ominus\mathbf{R}^{-1}}}{\delta\boldsymbol{\theta}} \xlongequal{(6)}\frac{log(\mathbf{R}exp(\left\lfloor-\delta\boldsymbol{\theta}\right\rfloor_{\times})\mathbf{R}^{-1})}{\delta\boldsymbol{\theta}}\\ &\xlongequal{(7)}\frac{log(exp(\mathbf{R}\left\lfloor-\delta\boldsymbol{\theta}\right\rfloor_{\times}\mathbf{R}^{-1}))}{\delta\boldsymbol{\theta}}= \frac{log(exp(\left\lfloor-\mathbf{R}\delta\boldsymbol{\theta}\right\rfloor_{\times}))}{\delta\boldsymbol{\theta}}\\ &=-\mathbf{R} \end{aligned} ∂δθ∂R−1=δθ(Rexp(⌊δθ⌋×))−1⊖R−1(6)δθlog(Rexp(⌊−δθ⌋×)R−1)(7)δθlog(exp(R⌊−δθ⌋×R−1))=δθlog(exp(⌊−Rδθ⌋×))=−R
R p \mathbf{Rp} Rp求导(右乘)
∂ ( R p ) ∂ δ θ = R e x p ( ⌊ δ θ ⌋ × ) p − R p δ θ = ( 1 ) R ⌊ δ θ ⌋ × p δ θ = − R ⌊ p ⌋ × \begin{aligned} \frac{\partial \mathbf{(Rp)}}{\partial \delta\boldsymbol{\theta}} &= \frac{\mathbf{R}exp(\left\lfloor\delta\boldsymbol{\theta}\right\rfloor_{\times})\mathbf{p}-\mathbf{Rp}}{ \delta\boldsymbol{\theta}}\\ &\xlongequal{(1)}\frac{\mathbf{R}\left\lfloor\delta\boldsymbol{\theta}\right\rfloor_{\times}\mathbf{p}}{ \delta\boldsymbol{\theta}}=-\mathbf{R}\left\lfloor\mathbf{p}\right\rfloor_{\times} \end{aligned} ∂δθ∂(Rp)=δθRexp(⌊δθ⌋×)p−Rp(1)δθR⌊δθ⌋×p=−R⌊p⌋×
R Q \mathbf{RQ} RQ求导(右乘)
∂ ( R Q ) ∂ δ θ = R e x p ( ⌊ δ θ ⌋ × ) Q ⊖ R Q δ θ = ( 6 ) l o g ( Q T e x p ( ⌊ δ θ ⌋ × ) Q ) δ θ = ( 7 ) l o g ( e x p ( Q T ⌊ δ θ ⌋ × Q ) ) δ θ = l o g ( e x p ( ⌊ Q T δ θ ⌋ × ) ) δ θ = Q T \begin{aligned} \frac{\partial \mathbf{(RQ)}}{\partial \delta\boldsymbol{\theta}} &= \frac{\mathbf{R}exp(\left\lfloor\delta\boldsymbol{\theta}\right\rfloor_{\times})\mathbf{Q}\ominus\mathbf{RQ}}{\delta\boldsymbol{\theta}}\\ &\xlongequal{(6)}\frac{log(\mathbf{Q}^{T}exp(\left\lfloor\delta\boldsymbol{\theta}\right\rfloor_{\times})\mathbf{Q})}{\delta\boldsymbol{\theta}}\\ &\xlongequal{(7)}\frac{log(exp(\mathbf{Q}^{T}\left\lfloor\delta\boldsymbol{\theta}\right\rfloor_{\times}\mathbf{Q}))}{\delta\boldsymbol{\theta}}\\ &=\frac{log(exp(\left\lfloor\mathbf{Q}^{T}\delta\boldsymbol{\theta}\right\rfloor_{\times}))}{\delta\boldsymbol{\theta}}=\mathbf{Q}^{T} \end{aligned} ∂δθ∂(RQ)=δθRexp(⌊δθ⌋×)Q⊖RQ(6)δθlog(QTexp(⌊δθ⌋×)Q)(7)δθlog(exp(QT⌊δθ⌋×Q))=δθlog(exp(⌊QTδθ⌋×))=QT
l o g ( R ) log(\mathbf{R}) log(R)求导(右乘)
∂ l o g ( R ) ∂ δ θ = l o g ( R e x p ( ⌊ δ θ ⌋ × ) ) − l o g ( R ) δ θ = ( 4 ) J r − 1 ( l o g R ) δ θ + l o g R − l o g R δ θ = J r − 1 ( l o g R ) \begin{aligned} \frac{\partial log(\mathbf{R})}{\partial \delta\boldsymbol{\theta}} &= \frac{log(\mathbf{R}exp(\left\lfloor\delta\boldsymbol{\theta}\right\rfloor_{\times}))-log(\mathbf{R})}{\delta\boldsymbol{\theta}}\\ &\xlongequal{(4)} \frac{\mathbf{J}_{r}^{-1}(log\mathbf{R})\delta\boldsymbol{\theta} + log\mathbf{R} - log\mathbf{R}}{\delta\boldsymbol{\theta}}\\ &=\mathbf{J}_{r}^{-1}(log\mathbf{R}) \end{aligned} ∂δθ∂log(R)=δθlog(Rexp(⌊δθ⌋×))−log(R)(4)δθJr−1(logR)δθ+logR−logR=Jr−1(logR)
l o g ( R Q ) log(\mathbf{RQ}) log(RQ)求导(右乘)
∂ l o g ( R Q ) ∂ δ θ = l o g ( R e x p ( ⌊ δ θ ⌋ × ) Q ) − l o g ( R Q ) δ θ = ( 2 ) l o g ( R Q e x p ( ⌊ Q T δ θ ⌋ × ) ) − l o g ( R Q ) δ θ = ( 4 ) J r − 1 ( l o g ( R Q ) ) Q T δ θ + l o g ( R Q ) − l o g ( R Q ) δ θ = J r − 1 ( l o g ( R Q ) ) Q T \begin{aligned} \frac{\partial log(\mathbf{RQ})}{\partial\delta\boldsymbol{\theta}} &= \frac{log(\mathbf{R}exp(\left\lfloor\delta\boldsymbol{\theta}\right\rfloor_{\times})\mathbf{Q})-log(\mathbf{RQ})}{\delta\boldsymbol{\theta}}\\ &\xlongequal{(2)}\frac{log(\mathbf{R}\mathbf{Q}exp(\left\lfloor\mathbf{Q}^{T}\delta\boldsymbol{\theta}\right\rfloor_{\times}))-log(\mathbf{RQ})}{\delta\boldsymbol{\theta}}\\ &\xlongequal{(4)}\frac{\mathbf{J}_{r}^{-1}(log(\mathbf{RQ}))\mathbf{Q}^{T}\delta\boldsymbol{\theta} + log(\mathbf{RQ})-log(\mathbf{RQ})}{\delta\boldsymbol{\theta}}\\ &=\mathbf{J}_{r}^{-1}(log(\mathbf{RQ}))\mathbf{Q}^{T} \end{aligned} ∂δθ∂log(RQ)=δθlog(Rexp(⌊δθ⌋×)Q)−log(RQ)(2)δθlog(RQexp(⌊QTδθ⌋×))−log(RQ)(4)δθJr−1(log(RQ))QTδθ+log(RQ)−log(RQ)=Jr−1(log(RQ))QT
左右雅克比矩阵
关于左乘
对于左乘,只用把扰动量 e x p ( ⌊ δ θ ⌋ × ) exp(\left\lfloor\delta\boldsymbol{\theta}\right\rfloor_{\times}) exp(⌊δθ⌋×) 放在旋转矩阵 R \mathbf{R} R 的左边进行推导即可