视觉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(R1R2−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(R2exp(ϕ∧))−1)∨−ln(R1R2−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(R1exp(ϕ∧)−1R2−1)∨−ln(R1R2−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(R1exp(ϕ∧)−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(R1R2−1R2exp(ϕ∧)−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(R1R2−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(R1R2−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(R1R2−1exp(((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(R1R2−1exp(−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(R1R2−1exp(−R2ϕ)∧)∨−ln(R1R2−1)∨
此时,考虑把
R
1
R
2
−
1
exp
(
−
R
2
ϕ
∧
)
R_1R_2^{-1}\exp(-R_2\phi^{\wedge})
R1R2−1exp(−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(R1R2−1)∨+Jr−1(ln(R1R2−1)∨)(−R2ϕ)−ln(R1R2−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(R1R2−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(−ϕ)