1、李代数公式基本性质:
1、扰动:
R
−
1
→
(
R
e
x
p
(
ϕ
∧
)
)
−
1
=
e
x
p
(
ϕ
∧
)
−
1
R
−
1
=
e
x
p
(
ϕ
∧
)
T
R
−
1
=
e
x
p
(
−
ϕ
∧
)
R
−
1
R^{-1} \to(Rexp(\phi^{\land}))^{-1}=exp(\phi^{\land})^{-1}R^{-1}=exp(\phi^{\land})^{T}R^{-1}=exp(-\phi^{\land})R^{-1}
R−1→(Rexp(ϕ∧))−1=exp(ϕ∧)−1R−1=exp(ϕ∧)TR−1=exp(−ϕ∧)R−1
上面用到了反对称矩阵的性质
2、叉乘交换:
a
⃗
×
b
⃗
=
−
b
⃗
×
a
⃗
\vec{a}\times\vec{b}=-\vec{b}\times\vec{a}
a×b=−b×a
3、右扰动对数近似:
l
n
(
R
e
x
p
(
ϕ
∧
)
)
∨
≈
l
n
(
R
)
∨
+
J
r
−
1
(
l
n
(
R
)
∨
)
ϕ
ln(Rexp(\phi^{\land}))^{\vee}\thickapprox ln(R)^{\vee}+J^{-1}_{r}(ln(R)^{\vee})\phi
ln(Rexp(ϕ∧))∨≈ln(R)∨+Jr−1(ln(R)∨)ϕ
其中
J
r
−
1
(
θ
w
)
=
θ
2
c
o
t
θ
2
I
+
(
1
−
θ
2
c
o
t
θ
2
)
w
w
T
+
θ
2
w
∧
J^{-1}_{r}(\theta w)=\frac{\theta}{2}cot\frac{\theta}{2}I+(1-\frac{\theta}{2}cot\frac{\theta}{2})ww^{T}+\frac{\theta}{2}w^{\land}
Jr−1(θw)=2θcot2θI+(1−2θcot2θ)wwT+2θw∧
4、左扰动对数近似:
l
n
(
e
x
p
(
ϕ
∧
)
R
)
∨
≈
J
l
−
1
(
l
n
(
R
)
∨
)
ϕ
+
l
n
(
R
)
∨
ln(exp(\phi^{\land})R)^{\vee}\thickapprox J^{-1}_{l}(ln(R)^{\vee})\phi + ln(R)^{\vee}
ln(exp(ϕ∧)R)∨≈Jl−1(ln(R)∨)ϕ+ln(R)∨
5、
R
p
∧
R
T
=
(
R
p
)
∧
Rp^{\land}R^{T}=(Rp)^{\land}
Rp∧RT=(Rp)∧:证明请参考这里
6、SO(3)伴随性质:
R
e
x
p
(
ϕ
∧
)
R
T
=
e
x
p
(
(
R
ϕ
)
∧
)
Rexp(\phi^{\land})R^{T}=exp((R\phi)^{\land})
Rexp(ϕ∧)RT=exp((Rϕ)∧)
7、SE(3)伴随性质:
T
e
x
p
(
ϕ
∧
)
T
T
=
e
x
p
(
(
A
d
(
T
)
ϕ
)
∧
)
Texp(\phi^{\land})T^{T}=exp((Ad(T)\phi)^{\land})
Texp(ϕ∧)TT=exp((Ad(T)ϕ)∧)
其中
d
(
T
)
=
[
R
t
∧
R
0
R
]
d(T)=\left[\begin{matrix} R&t^{\land}R\\0&R \end{matrix}\right]
d(T)=[R0t∧RR]
2、常见雅可比(以自变量R举例)
1、旋转点的左扰动雅可比:
∂
(
R
p
)
∂
R
=
lim
φ
→
0
e
x
p
(
φ
∧
)
R
p
−
R
p
∂
φ
=
lim
φ
→
0
(
I
+
φ
∧
)
R
p
−
R
p
∂
φ
=
lim
φ
→
0
φ
∧
R
p
∂
φ
=
−
(
R
p
)
∧
φ
∂
φ
=
−
(
R
p
)
∧
\begin{aligned} \frac{\partial(Rp)}{\partial R} &=\lim_{\varphi \to 0}\frac{exp(\varphi^{\land})Rp-Rp}{\partial\varphi}\\ &=\lim_{\varphi \to 0} \frac{(I+\varphi^{\land})Rp-Rp}{\partial\varphi}\\ &=\lim_{\varphi \to 0}\frac{\varphi^{\land}Rp}{\partial\varphi}\\ &=\frac{-(Rp)^{\land}\varphi}{\partial\varphi}\\ &=-(Rp)^{\land} \end{aligned}
∂R∂(Rp)=φ→0lim∂φexp(φ∧)Rp−Rp=φ→0lim∂φ(I+φ∧)Rp−Rp=φ→0lim∂φφ∧Rp=∂φ−(Rp)∧φ=−(Rp)∧
2、旋转点的右扰动雅可比:
∂
(
R
p
)
∂
R
=
lim
φ
→
0
R
e
x
p
(
φ
∧
)
p
−
R
p
∂
φ
=
lim
φ
→
0
R
(
I
+
φ
∧
)
p
−
R
p
∂
φ
=
lim
φ
→
0
R
φ
∧
p
∂
φ
=
−
R
p
∧
φ
∂
φ
=
−
R
p
∧
\begin{aligned} \frac{\partial(Rp)}{\partial R} &=\lim_{\varphi \to 0}\frac{Rexp(\varphi^{\land})p-Rp}{\partial\varphi}\\ &=\lim_{\varphi \to 0} \frac{R(I+\varphi^{\land})p-Rp}{\partial\varphi}\\ &=\lim_{\varphi \to 0}\frac{R\varphi^{\land}p}{\partial\varphi}\\ &=-\frac{Rp^{\land}\varphi}{\partial\varphi}\\ &=-Rp^{\land} \end{aligned}
∂R∂(Rp)=φ→0lim∂φRexp(φ∧)p−Rp=φ→0lim∂φR(I+φ∧)p−Rp=φ→0lim∂φRφ∧p=−∂φRp∧φ=−Rp∧
3、旋转连乘雅可比:
d
l
n
(
R
1
R
2
)
∨
d
R
2
=
lim
ϕ
→
0
l
n
(
R
1
R
2
e
x
p
(
ϕ
∧
)
)
∨
−
l
n
(
R
1
R
2
)
∨
ϕ
=
lim
ϕ
→
0
l
n
(
R
1
R
2
)
∨
+
J
r
−
1
ϕ
−
l
n
(
R
1
R
2
)
∨
ϕ
=
J
r
−
1
(
l
n
(
R
1
R
2
)
∨
)
\begin{aligned} \frac{dln(R_{1}R_{2})^{\vee}}{dR_{2}} &=\lim_{\phi \to 0}\frac{ln(R_{1}R_{2}exp(\phi^{\land}))^{\vee}-ln(R_{1}R_{2})^{\vee}}{\phi}\\ &=\lim_{\phi \to 0}\frac{ln(R_{1}R_{2})^{\vee}+J^{-1}_{r}\phi-ln(R_{1}R_{2})^{\vee}}{\phi}\\ &=J^{-1}_{r}(ln(R_{1}R_{2})^{\vee}) \end{aligned}
dR2dln(R1R2)∨=ϕ→0limϕln(R1R2exp(ϕ∧))∨−ln(R1R2)∨=ϕ→0limϕln(R1R2)∨+Jr−1ϕ−ln(R1R2)∨=Jr−1(ln(R1R2)∨)
其中用到了性质3
4、旋转连乘的雅可比:
d
l
n
(
R
1
R
2
)
∨
d
R
1
=
lim
ϕ
→
0
l
n
(
R
1
e
x
p
(
ϕ
∧
)
R
2
)
∨
−
l
n
(
R
1
R
2
)
∨
ϕ
=
lim
ϕ
→
0
l
n
(
R
1
R
2
R
2
T
e
x
p
(
ϕ
∧
)
R
2
)
∨
−
l
n
(
R
1
R
2
)
∨
ϕ
=
lim
ϕ
→
0
l
n
(
R
1
R
2
e
x
p
(
(
R
2
T
ϕ
)
∧
)
)
∨
−
l
n
(
R
1
R
2
)
∨
ϕ
=
lim
ϕ
→
0
l
n
(
R
1
R
2
)
∨
+
J
r
−
1
R
2
T
ϕ
−
l
n
(
R
1
R
2
)
∨
ϕ
=
J
r
−
1
(
l
n
(
R
1
R
2
)
∨
)
R
2
T
\begin{aligned} \frac{dln(R_{1}R_{2})^{\vee}}{dR_{1}} &=\lim_{\phi \to 0}\frac{ln(R_{1}exp(\phi^{\land})R_{2})^{\vee}-ln(R_{1}R_{2})^{\vee}}{\phi}\\ &=\lim_{\phi \to 0}\frac{ln(R_{1}R_{2}R^{T}_{2}exp(\phi^{\land})R_{2})^{\vee}-ln(R_{1}R_{2})^{\vee}}{\phi}\\ &=\lim_{\phi \to 0}\frac{ln(R_{1}R_{2}exp((R^{T}_{2}\phi)^{\land}))^{\vee}-ln(R_{1}R_{2})^{\vee}}{\phi}\\ &=\lim_{\phi \to 0}\frac{ln(R_{1}R_{2})^{\vee}+J^{-1}_{r}R^{T}_{2}\phi-ln(R_{1}R_{2})^{\vee}}{\phi}\\ &=J^{-1}_{r}(ln(R_{1}R_{2})^{\vee})R^{T}_{2} \end{aligned}
dR1dln(R1R2)∨=ϕ→0limϕln(R1exp(ϕ∧)R2)∨−ln(R1R2)∨=ϕ→0limϕln(R1R2R2Texp(ϕ∧)R2)∨−ln(R1R2)∨=ϕ→0limϕln(R1R2exp((R2Tϕ)∧))∨−ln(R1R2)∨=ϕ→0limϕln(R1R2)∨+Jr−1R2Tϕ−ln(R1R2)∨=Jr−1(ln(R1R2)∨)R2T
其中用到了性质3和性质6
5、旋转点逆的右扰动:
d
R
−
1
p
d
R
=
lim
φ
→
0
(
R
e
x
p
(
φ
∧
)
)
−
1
p
−
R
−
1
p
φ
=
lim
φ
→
0
e
x
p
(
−
φ
∧
)
R
−
1
p
−
R
−
1
p
φ
=
lim
φ
→
0
(
I
−
φ
∧
)
R
−
1
p
−
R
−
1
p
φ
=
lim
φ
→
0
−
φ
∧
R
−
1
p
φ
=
lim
φ
→
0
(
R
−
1
p
)
∧
φ
φ
=
(
R
−
1
p
)
∧
\begin{aligned} \frac{dR^{-1}p}{dR}&=\lim_{\varphi \to 0}\frac{(Rexp({\varphi^{\land}}))^{-1}p-R^{-1}p}{\varphi}\\ &=\lim_{\varphi \to 0}\frac{exp(-\varphi^{\land})R^{-1}p-R^{-1}p}{\varphi}\\ &=\lim_{\varphi \to 0}\frac{(I-\varphi^{\land})R^{-1}p-R^{-1}p}{\varphi}\\ &=\lim_{\varphi \to 0}\frac{-\varphi^{\land}R^{-1}p}{\varphi}\\ &=\lim_{\varphi \to 0}\frac{(R^{-1}p)^{\land}\varphi}{\varphi}\\ &=(R^{-1}p)^{\land} \end{aligned}
dRdR−1p=φ→0limφ(Rexp(φ∧))−1p−R−1p=φ→0limφexp(−φ∧)R−1p−R−1p=φ→0limφ(I−φ∧)R−1p−R−1p=φ→0limφ−φ∧R−1p=φ→0limφ(R−1p)∧φ=(R−1p)∧
6、连乘旋转点逆的右扰动
d
l
n
(
R
1
R
2
−
1
)
∨
d
R
2
=
lim
φ
→
0
l
n
(
R
1
(
R
2
e
x
p
(
φ
∧
)
)
−
1
)
∨
−
l
n
(
R
1
R
2
−
1
)
∨
φ
=
lim
φ
→
0
l
n
(
R
1
e
x
p
(
−
φ
∧
)
R
2
−
1
)
∨
−
l
n
(
R
1
R
2
−
1
)
∨
φ
=
lim
φ
→
0
l
n
(
R
1
R
2
−
1
R
2
e
x
p
(
−
φ
∧
)
R
2
−
1
)
∨
−
l
n
(
R
1
R
2
−
1
)
∨
φ
=
lim
φ
→
0
l
n
(
R
1
R
2
−
1
e
x
p
(
−
(
R
2
φ
)
∧
)
)
∨
−
l
n
(
R
1
R
2
−
1
)
∨
φ
=
lim
φ
→
0
l
n
(
R
1
R
2
−
1
)
∨
−
J
r
−
1
R
2
φ
−
l
n
(
R
1
R
2
−
1
)
∨
φ
=
lim
φ
→
0
−
J
r
−
1
R
2
φ
φ
=
−
J
r
−
1
R
2
\begin{aligned} \frac{dln(R_{1}R^{-1}_{2})^{\vee}}{dR^{2}} &=\lim_{\varphi \to 0}\frac{ln(R_{1}(R_{2}exp(\varphi^{\land}))^{-1})^{\vee}-ln(R_{1}R^{-1}_{2})^{\vee}}{\varphi}\\ &=\lim_{\varphi \to 0}\frac{ln(R_{1}exp(-\varphi^{\land})R^{-1}_{2})^{\vee}-ln(R_{1}R^{-1}_{2})^{\vee}}{\varphi}\\ &=\lim_{\varphi \to 0}\frac{ln(R_{1}R^{-1}_{2}R_{2}exp(-\varphi^{\land})R^{-1}_{2})^{\vee}-ln(R_{1}R^{-1}_{2})^{\vee}}{\varphi}\\ &=\lim_{\varphi \to 0}\frac{ln(R_{1}R^{-1}_{2}exp(-(R_{2}\varphi)^{\land}))^{\vee}-ln(R_{1}R^{-1}_{2})^{\vee}}{\varphi}\\ &=\lim_{\varphi \to 0}\frac{ln(R_{1}R^{-1}_{2})^{\vee}-J^{-1}_{r}R_{2}\varphi-ln(R_{1}R^{-1}_{2})^{\vee}}{\varphi}\\ &=\lim_{\varphi \to 0}\frac{-J^{-1}_{r}R_{2}\varphi}{\varphi}\\ &=-J^{-1}_{r}R_{2} \end{aligned}
dR2dln(R1R2−1)∨=φ→0limφln(R1(R2exp(φ∧))−1)∨−ln(R1R2−1)∨=φ→0limφln(R1exp(−φ∧)R2−1)∨−ln(R1R2−1)∨=φ→0limφln(R1R2−1R2exp(−φ∧)R2−1)∨−ln(R1R2−1)∨=φ→0limφln(R1R2−1exp(−(R2φ)∧))∨−ln(R1R2−1)∨=φ→0limφln(R1R2−1)∨−Jr−1R2φ−ln(R1R2−1)∨=φ→0limφ−Jr−1R2φ=−Jr−1R2
其中用到了性质3和性质6