视觉SLAM十四讲|【三】李代数旋转计算示例

2024-01-08 21:49:04

视觉SLAM十四讲|【三】李代数旋转计算示例

示例1

δ ( R ? 1 p ) δ R \frac{\delta(R^{-1}p)}{\delta R } δRδ(R?1p)?
以右旋近似,可以得到如下
= lim ? ? → 0 ( ( R ( e x p ( ? ∧ ) ) ) ? 1 p ) ? R ? 1 p ? =\lim_{\phi \rightarrow 0} \frac{((R(exp(\phi^{\wedge})))^{-1}p)-R^{-1}p}{\phi} =?0lim??((R(exp(?)))?1p)?R?1p?
= lim ? ? → 0 e x p ( ? ∧ ) ? 1 R ? 1 p ? R ? 1 p ? =\lim_{\phi \rightarrow 0} \frac{exp(\phi^{\wedge})^{-1}R^{-1}p-R^{-1}p}{\phi} =?0lim??exp(?)?1R?1p?R?1p?
= lim ? ? → 0 ( I + ? ∧ ) ? 1 R ? 1 p ? R ? 1 p ? =\lim_{\phi \rightarrow 0} \frac{(I+\phi^{\wedge})^{-1}R^{-1}p-R^{-1}p}{\phi} =?0lim??(I+?)?1R?1p?R?1p?
= lim ? ? → 0 ( I ? ? ∧ ) R ? 1 p ? R ? 1 p ? =\lim_{\phi \rightarrow 0} \frac{(I-\phi^{\wedge})R^{-1}p-R^{-1}p}{\phi} =?0lim??(I??)R?1p?R?1p?
= lim ? ? → 0 ( ? ? ∧ ) R ? 1 p ? =\lim_{\phi \rightarrow 0} \frac{(-\phi^{\wedge})R^{-1}p}{\phi} =?0lim??(??)R?1p?
为了将分母中的 ? \phi ?消元掉,需要把 ? ∧ \phi^{\wedge} ?转换为 ? \phi ?,考虑到李代数伴随矩阵的性质如下
a ∧ b = ? b ∧ a a^{\land} b = -b^{\land}a ab=?ba
可以化简上式为
= lim ? ? → 0 ? ( R ? 1 p ) ∧ ( ? ? ) ? =\lim_{\phi \rightarrow 0} \frac{-(R^{-1}p)^{\wedge} (-\phi)}{\phi} =?0lim???(R?1p)(??)?
= ( R ? 1 p ) ∧ =(R^{-1}p)^{\wedge} =(R?1p)

示例2

δ l n ( R 1 R 2 ? 1 ) ∨ δ R 2 \frac{\delta ln(R_1R_2^{-1})^{\vee}}{\delta R_2} δR2?δln(R1?R2?1?)?
以右旋近似,可以得到如下
lim ? ? → 0 l n ( R 1 ( R 2 exp ? ( ? ∧ ) ) ? 1 ) ∨ ? l n ( R 1 R 2 ? 1 ) ∨ ? \lim_{\phi \rightarrow 0} \frac{ln(R_1(R_2\exp(\phi^{\wedge}))^{-1})^{\vee}-ln(R_1R_2^{-1})^{\vee}}{\phi} ?0lim??ln(R1?(R2?exp(?))?1)?ln(R1?R2?1?)?
= lim ? ? → 0 l n ( R 1 exp ? ( ? ∧ ) ? 1 R 2 ? 1 ) ∨ ? l n ( R 1 R 2 ? 1 ) ∨ ? = \lim_{\phi \rightarrow 0} \frac{ln(R_1\exp(\phi^{\wedge})^{-1}R_2^{-1})^{\vee}-ln(R_1R_2^{-1})^{\vee}}{\phi} =?0lim??ln(R1?exp(?)?1R2?1?)?ln(R1?R2?1?)?
为了化简式子,需要把 ? ∧ \phi^{\wedge} ?的矩阵化算子转移到最右边,已知SO(3)伴随性质:
R T e x p ( ? ∧ ) R = e x p ( ( R T ? ) ∧ ) R^Texp(\phi^{\land})R = exp((R^T\phi)^{\land}) RTexp(?)R=exp((RT?))
又有
R T = R ? 1 , ( R T ) ? 1 = R , R R T = I R^T=R^{-1},(R^T)^{-1}=R,RR^T=I RT=R?1,RT?1=R,RRT=I
所以对上式中 l n ( R 1 exp ? ( ? ∧ ) ? 1 R 2 ? 1 ) ∨ ln(R_1\exp(\phi^{\wedge})^{-1}R_2^{-1})^{\vee} ln(R1?exp(?)?1R2?1?),有
= l n ( R 1 R 2 ? 1 R 2 exp ? ( ? ∧ ) ? 1 R 2 ? 1 ) ∨ =ln(R_1R_2^{-1}R_2\exp(\phi^{\wedge})^{-1}R_2^{-1})^{\vee} =ln(R1?R2?1?R2?exp(?)?1R2?1?)
= l n ( R 1 R 2 ? 1 ( ( R 2 ) ? 1 ) T exp ? ( ? ∧ ) ? 1 R 2 ? 1 ) ∨ =ln(R_1R_2^{-1}((R_2)^{-1})^{T}\exp(\phi^{\wedge})^{-1}R_2^{-1})^{\vee} =ln(R1?R2?1?((R2?)?1)Texp(?)?1R2?1?)
= l n ( R 1 R 2 ? 1 ( ( R 2 ) ? 1 ) T exp ? ( ? ? ∧ ) R 2 ? 1 ) ∨ =ln(R_1R_2^{-1}((R_2)^{-1})^{T}\exp(-\phi^{\wedge})R_2^{-1})^{\vee} =ln(R1?R2?1?((R2?)?1)Texp(??)R2?1?)
= l n ( R 1 R 2 ? 1 exp ? ( ( ( R 2 ? 1 ) T ) ( ? ? ∧ ) ) ) ∨ =ln(R_1R_2^{-1}\exp(((R_2^{-1})^T)(-\phi^{\wedge})))^{\vee} =ln(R1?R2?1?exp(((R2?1?)T)(??)))
= l n ( R 1 R 2 ? 1 exp ? ( ? R 2 ? ) ∧ ) ∨ =ln(R_1R_2^{-1}\exp(-R_2\phi)^{\wedge})^{\vee} =ln(R1?R2?1?exp(?R2??))
因此,对于整个式子,有
= lim ? ? → 0 l n ( R 1 R 2 ? 1 exp ? ( ? R 2 ? ) ∧ ) ∨ ? l n ( R 1 R 2 ? 1 ) ∨ ? = \lim_{\phi \rightarrow 0} \frac{ln(R_1R_2^{-1}\exp(-R_2\phi)^{\wedge})^{\vee}-ln(R_1R_2^{-1})^{\vee}}{\phi} =?0lim??ln(R1?R2?1?exp(?R2??))?ln(R1?R2?1?)?
此时,考虑把 R 1 R 2 ? 1 exp ? ( ? R 2 ? ∧ ) R_1R_2^{-1}\exp(-R_2\phi^{\wedge}) R1?R2?1?exp(?R2??)中的前后分开,注意到李代数的性质如下
l n ( R e x p ( ? ∧ ) ) ∨ = l n ( R ) ∨ + J r ? 1 ? ln(Rexp(\phi^{\land}))^{\vee}=ln(R)^{\vee}+J_r^{-1}\phi ln(Rexp(?))=ln(R)+Jr?1??
因此有
= lim ? ? → 0 l n ( R 1 R 2 ? 1 ) ∨ + J r ? 1 ( l n ( R 1 R 2 ? 1 ) ∨ ) ( ? R 2 ? ) ? l n ( R 1 R 2 ? 1 ) ∨ ? = \lim_{\phi \rightarrow 0} \frac{ln(R_1R_2^{-1})^{\vee}+J_r^{-1}(ln(R_1R_2^{-1})^{\vee})(-R_2\phi)-ln(R_1R_2^{-1})^{\vee}}{\phi} =?0lim??ln(R1?R2?1?)+Jr?1?(ln(R1?R2?1?))(?R2??)?ln(R1?R2?1?)?
= ? J r ? 1 ( l n ( R 1 R 2 ? 1 ) ∨ ) R 2 =-J_r^{-1}(ln(R_1R_2^{-1})^{\vee})R_2 =?Jr?1?(ln(R1?R2?1?))R2?
其中

J l = s i n θ θ I + ( 1 ? s i n θ θ ) a a T + 1 ? c o s θ θ a ∧ J_l = \frac{sin\theta}{\theta}I + (1- \frac{sin\theta}{\theta})aa^T + \frac{1-cos\theta}{\theta}a^{\land} Jl?=θsinθ?I+(1?θsinθ?)aaT+θ1?cosθ?a

J l ? 1 = θ 2 c o t θ 2 I + ( 1 ? θ 2 c o t θ 2 ) a a T ? θ 2 a ∧ J_l ^{-1} = \frac{\theta}{2}cot \frac{\theta}{2}I + (1- \frac{\theta}{2}cot \frac{\theta}{2})aa^T - \frac{\theta}{2}a^{\land} Jl?1?=2θ?cot2θ?I+(1?2θ?cot2θ?)aaT?2θ?a
J r ( ? ) = J l ( ? ? ) J_r(\phi) = J_l(-\phi) Jr?(?)=Jl?(??)

文章来源:https://blog.csdn.net/qq_43443531/article/details/135451028
本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。