视觉SLAM十四讲|【三】李代数旋转计算示例
视觉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
a∧b=?b∧a
可以化简上式为
=
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?(??)
本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。 如若内容造成侵权/违法违规/事实不符，请联系我的编程经验分享网邮箱：veading@qq.com进行投诉反馈，一经查实，立即删除！