约定：
李代数
ϕ
\phi
ϕ
的指数映射：
e
x
p
(
ϕ
∧
)
exp(\phi^\wedge)
e
x
p
(
ϕ
∧
)
旋转矩阵
R
R
R
的对数映射：
l
n
(
R
)
∨
ln(R)^\vee
l
n
(
R
)
∨
另一个，李代数
φ
\varphi
φ
是一个小量
公式1：BCH公式（线性近似公式）
e
x
p
(
ϕ
∧
)
e
x
p
(
φ
∧
)
=
e
x
p
[
(
ϕ
+
J
r
(
ϕ
)
−
1
φ
)
∧
]
exp(\phi^\wedge)exp(\varphi^\wedge)=exp[(\phi+J_r(\phi)^{-1}\varphi)^\wedge]
e
x
p
(
ϕ
∧
)
e
x
p
(
φ
∧
)
=
e
x
p
[
(
ϕ
+
J
r
(
ϕ
)
−
1
φ
)
∧
]
BCH公式给出了
两个李代数指数映射乘积的结果
。下面是一个等价的表达：
l
n
[
e
x
p
(
ϕ
∧
)
e
x
p
(
φ
∧
)
]
∨
=
ϕ
+
J
r
(
ϕ
)
−
1
φ
ln[exp(\phi^\wedge)exp(\varphi^\wedge)]^\vee=\phi+J_r(\phi)^{-1}\varphi
l
n
[
e
x
p
(
ϕ
∧
)
e
x
p
(
φ
∧
)
]
∨
=
ϕ
+
J
r
(
ϕ
)
−
1
φ
公式2：
S
O
(
3
)
SO(3)
S
O
(
3
)
上的伴随性质
R
e
x
p
(
p
∧
)
R
T
=
e
x
p
[
(
R
p
)
∧
]
Rexp(p^\wedge)R^T=exp[(Rp)^\wedge]
R
e
x
p
(
p
∧
)
R
T
=
e
x
p
[
(
R
p
)
∧
]
同样重要的公式变形！（相当于两边同时乘以
R
T
R^T
R
T
）：
e
x
p
(
p
∧
)
R
T
=
R
T
e
x
p
[
(
R
p
)
∧
]
exp(p^\wedge)R^T=R^Texp[(Rp)^\wedge]
e
x
p
(
p
∧
)
R
T
=
R
T
e
x
p
[
(
R
p
)
∧
]
使用如下几个推导巩固上述两个公式：
1.（右）扰动模型
d
(
R
p
)
d
φ
=
lim
φ
→
0
R
e
x
p
(
φ
∧
)
p
−
R
p
φ
≈
lim
φ
→
0
R
(
I
+
φ
∧
)
p
−
R
p
φ
=
lim
φ
→
0
R
φ
∧
p
φ
=
lim
φ
→
0
R
p
∧
φ
φ
=
(
R
p
)
∧
\begin{aligned} \frac{d(Rp)}{d\varphi} & = \lim\limits_{\varphi \to 0 } \frac{Rexp(\varphi^\wedge)p-Rp}{\varphi} \\ & \approx \lim\limits_{\varphi \to 0 } \frac{R(I+\varphi^\wedge)p-Rp}{\varphi} \\ & = \lim\limits_{\varphi \to 0 } \frac{R\varphi^\wedge p}{\varphi} \\ & = \lim\limits_{\varphi \to 0 } \frac{Rp^\wedge \varphi}{\varphi} \\ & = (Rp)^\wedge \end{aligned}
d
φ
d
(
R
p
)
=
φ
→
0
lim
φ
R
e
x
p
(
φ
∧
)
p
−
R
p
≈
φ
→
0
lim
φ
R
(
I
+
φ
∧
)
p
−
R
p
=
φ
→
0
lim
φ
R
φ
∧
p
=
φ
→
0
lim
φ
R
p
∧
φ
=
(
R
p
)
∧
d
(
R
−
1
p
)
d
φ
=
lim
φ
→
0
[
R
e
x
p
(
φ
∧
)
]
−
1
p
−
R
−
1
p
φ
=
lim
φ
→
0
e
x
p
(
φ
∧
)
−
1
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{d(R^{-1}p)}{d\varphi} & = \lim\limits_{\varphi \to 0 } \frac{[Rexp(\varphi^\wedge)]^{-1}p-R^{-1}p}{\varphi} \\ & = \lim\limits_{\varphi \to 0 } \frac{exp(\varphi^\wedge)^{-1}R^{-1}p-R^{-1}p}{\varphi} \\ & \approx \lim\limits_{\varphi \to 0 } \frac{(I-\varphi^\wedge)R^{-1}p-R^{-1}p}{\varphi} \\ & = \lim\limits_{\varphi \to 0 } \frac{-\varphi^\wedge R^{-1}p}{\varphi} \\ & = \lim\limits_{\varphi \to 0 } \frac{(R^{-1}p)^\wedge \varphi}{\varphi} \\ & = (R^{-1}p)^\wedge \end{aligned}
d
φ
d
(
R
−
1
p
)
=
φ
→
0
lim
φ
[
R
e
x
p
(
φ
∧
)
]
−
1
p
−
R
−
1
p
=
φ
→
0
lim
φ
e
x
p
(
φ
∧
)
−
1
R
−
1
p
−
R
−
1
p
≈
φ
→
0
lim
φ
(
I
−
φ
∧
)
R
−
1
p
−
R
−
1
p
=
φ
→
0
lim
φ
−
φ
∧
R
−
1
p
=
φ
→
0
lim
φ
(
R
−
1
p
)
∧
φ
=
(
R
−
1
p
)
∧
【注】：第一个等号，是对
R
R
R
的扰动