文章目录
1 指数和对数映射
对李代数拆分,
ϕ
=
θ
a
\ \phi= \theta a
ϕ=θa,模长
θ
\ \theta
θ,方向为单位向量
a
,
∣
∣
a
∣
∣
=
1
\ a, \vert\vert{a}\vert\vert=1
a,∣∣a∣∣=1,有以下性质:
a
∧
a
∧
=
a
a
T
−
I
a
∧
a
∧
a
∧
=
a
∧
(
a
a
T
−
I
)
=
−
a
∧
a^{\land}a^{\land} = aa^T-I \\ a^{\land}a^{\land}a^{\land}=a^{\land}(aa^T-I)=-a^{\land}
a∧a∧=aaT−Ia∧a∧a∧=a∧(aaT−I)=−a∧
对于
s
o
(
3
)
\ so(3)
so(3)的任意向量
ϕ
\ \phi
ϕ,可以定义指数映射,以及对应的泰勒展开:
c
o
s
x
=
1
−
x
2
2
!
+
x
4
4
!
−
x
6
6
!
+
⋯
s
i
n
x
=
x
−
x
3
3
!
+
x
5
5
!
−
x
7
7
!
+
⋯
R
=
e
ϕ
∧
=
∑
n
=
0
1
n
!
(
ϕ
∧
)
n
=
∑
n
=
0
1
n
!
(
θ
a
∧
)
n
=
I
+
θ
a
∧
+
1
2
!
θ
2
a
∧
a
∧
+
1
3
!
θ
3
a
∧
a
∧
a
∧
+
1
4
!
θ
4
a
∧
a
∧
a
∧
a
∧
+
⋯
=
a
a
T
−
a
∧
a
∧
+
θ
a
∧
+
1
2
!
θ
2
a
∧
a
∧
−
1
3
!
θ
3
a
∧
−
1
4
!
θ
4
a
∧
a
∧
+
⋯
=
a
a
T
+
(
θ
−
1
3
!
θ
3
+
1
5
!
θ
5
−
⋯
)
a
∧
−
(
1
−
1
2
!
θ
2
+
1
4
!
θ
4
−
⋯
)
a
∧
a
∧
=
a
∧
a
∧
+
I
+
s
i
n
θ
a
∧
−
c
o
s
θ
a
∧
a
∧
=
(
1
−
c
o
s
θ
)
a
∧
a
∧
+
I
+
s
i
n
θ
a
∧
=
c
o
s
θ
I
+
(
1
−
c
o
s
θ
)
a
a
T
+
s
i
n
θ
a
∧
cosx = 1-\frac{x^2}{2!}+\frac{x^4}{4!}-\frac{x^6}{6!}+ \cdots \\ sinx = x-\frac{x^3}{3!}+\frac{x^5}{5!}-\frac{x^7}{7!}+ \cdots \\ R = e^{\phi^{\land}}= \sum_{n=0}{\frac{1}{n!}(\phi^{\land})^n} = \sum_{n=0}{\frac{1}{n!}(\theta a^{\land})^n} \\ = I + \theta a^{\land} + \frac{1}{2!}\theta^2a^{\land}a^{\land} + \frac{1}{3!}\theta^3a^{\land}a^{\land}a^{\land} + \frac{1}{4!}\theta^4a^{\land}a^{\land}a^{\land}a^{\land} + \cdots \\ = aa^T - a^{\land}a^{\land} + \theta a^{\land} + \frac{1}{2!}\theta^2a^{\land}a^{\land} -\frac{1}{3!}\theta^3a^{\land} - \frac{1}{4!}\theta^4a^{\land}a^{\land} + \cdots \\ = aa^T + (\theta - \frac{1}{3!}\theta^3 + \frac{1}{5!}\theta^5 - \cdots)a^{\land} - (1-\frac{1}{2!}\theta^2+\frac{1}{4!}\theta^4-\cdots)a^{\land}a^{\land} \\ = a^{\land}a^{\land} + I + sin\theta a^{\land} - cos\theta a^{\land}a^{\land} \\ = (1-cos\theta)a^{\land}a^{\land} +I +sin\theta a^{\land} \\ = cos\theta I + (1-cos\theta)aa^T+sin\theta a^{\land}
cosx=1−2!x2+4!x4−6!x6+⋯sinx=x−3!x3+5!x5−7!x7+⋯R=eϕ∧=n=0∑n!1(ϕ∧)n=n=0∑n!1(θa∧)n=I+θa∧+2!1θ2a∧a∧+3!1θ3a∧a∧a∧+4!1θ4a∧a∧a∧a∧+⋯=aaT−a∧a∧+θa∧+2!1θ2a∧a∧−3!1θ3a∧−4!1θ4a∧a∧+⋯=aaT+(θ−3!1θ3+5!1θ5−⋯)a∧−(1−2!1θ2+4!1θ4−⋯)a∧a∧=a∧a∧+I+sinθa∧−cosθa∧a∧=(1−cosθ)a∧a∧+I+sinθa∧=cosθI+(1−cosθ)aaT+sinθa∧
可见
s
o
(
3
)
\ so(3)
so(3)就是旋转向量组成的空间,而指数映射就是罗德里格斯公式。而对数映射可以将李群转为李代数
ϕ
=
θ
a
=
l
n
(
R
)
∨
\ \phi=\theta a=ln(R)^{\lor}
ϕ=θa=ln(R)∨,但计算较为复杂,可使用以下公式进行计算角度,根据转轴不变求解方向向量:
R
=
c
o
s
θ
I
+
(
1
−
c
o
s
θ
)
a
a
T
+
s
i
n
θ
a
∧
t
r
(
R
)
=
c
o
s
θ
t
r
(
I
)
+
(
1
−
c
o
s
θ
)
t
r
(
a
a
T
)
+
s
i
n
θ
t
r
(
a
∧
)
=
3
c
o
s
θ
+
(
1
−
c
o
s
θ
)
=
1
+
2
c
o
s
θ
θ
=
a
r
c
c
o
s
t
r
(
R
)
−
1
2
R
a
=
a
R = cos\theta I + (1-cos\theta)aa^T+sin\theta a^{\land} \\ tr(R) = cos\theta tr(I) + (1-cos\theta)tr(aa^T)+sin\theta tr(a^{\land}) \\ = 3cos\theta+(1-cos\theta) = 1+2cos\theta\\ \theta = arccos \frac{tr(R)-1}{2} \\ Ra = a
R=cosθI+(1−cosθ)aaT+sinθa∧tr(R)=cosθtr(I)+(1−cosθ)tr(aaT)+sinθtr(a∧)=3cosθ+(1−cosθ)=1+2cosθθ=arccos2tr(R)−1Ra=a
对于
s
e
(
3
)
\ se(3)
se(3)进行指数映射,推导如下:
T
=
e
ϵ
∧
=
∑
n
=
0
1
n
!
(
ϵ
∧
)
n
=
I
+
∑
n
=
1
1
n
!
(
[
ϕ
∧
ρ
0
T
0
]
)
n
T = e^{\epsilon^{\land}} = \sum_{n=0}{\frac{1}{n!}(\epsilon^{\land})^n} \\ = I + \sum_{n=1}{\frac{1}{n!}(\begin{bmatrix} \phi^{\land} & \rho \\ 0^T & 0 \end{bmatrix})^n} \\
T=eϵ∧=n=0∑n!1(ϵ∧)n=I+n=1∑n!1([ϕ∧0Tρ0])n
对矩阵乘法结果进行归纳后得到:
[
θ
a
∧
ρ
0
T
0
]
[
θ
a
∧
ρ
0
T
0
]
=
[
(
θ
a
∧
)
2
θ
a
∧
ρ
0
T
0
]
\begin{bmatrix} \theta a^{\land} & \rho \\ 0^T & 0 \end{bmatrix} \begin{bmatrix} \theta a^{\land} & \rho \\ 0^T & 0 \end{bmatrix} = \begin{bmatrix} (\theta a^{\land})^2 & \theta a^{\land}\rho \\ 0^T & 0 \end{bmatrix} \\
[θa∧0Tρ0][θa∧0Tρ0]=[(θa∧)20Tθa∧ρ0]
最终指数映射结果如下:
T
=
I
+
∑
n
=
1
1
n
!
[
(
θ
a
∧
)
n
(
θ
a
∧
)
n
−
1
ρ
0
T
0
]
=
[
∑
n
=
0
1
n
!
(
ϕ
∧
)
n
∑
n
=
1
1
n
!
(
ϕ
∧
)
n
−
1
ρ
0
T
1
]
=
[
∑
1
n
!
(
ϕ
∧
)
n
∑
1
(
n
+
1
)
!
(
ϕ
∧
)
n
ρ
0
T
1
]
=
[
R
J
ρ
0
T
1
]
T = I + \sum_{n=1}{\frac{1}{n!}\begin{bmatrix} (\theta a^{\land})^n & (\theta a^{\land})^{n-1}\rho \\ 0^T & 0 \end{bmatrix}} \\ = \begin{bmatrix} \sum_{n=0}{\frac{1}{n!}(\phi^{\land})^n} & \sum_{n=1}{\frac{1}{n!}(\phi^{\land})^{n-1}}\rho \\ 0^T & 1 \end{bmatrix} \\ = \begin{bmatrix} \sum{\frac{1}{n!}(\phi^{\land})^n} & \sum{\frac{1}{(n+1)!}(\phi^{\land})^n}\rho \\ 0^T & 1 \end{bmatrix} = \begin{bmatrix} R & J\rho \\ 0^T & 1 \end{bmatrix}
T=I+n=1∑n!1[(θa∧)n0T(θa∧)n−1ρ0]=[∑n=0n!1(ϕ∧)n0T∑n=1n!1(ϕ∧)n−1ρ1]=[∑n!1(ϕ∧)n0T∑(n+1)!1(ϕ∧)nρ1]=[R0TJρ1]
和
s
o
(
3
)
\ so(3)
so(3)一样将R
转为
ϕ
=
θ
a
\ \phi=\theta a
ϕ=θa的推导与so(3)
映射一致,
J
=
∑
n
=
0
1
(
n
+
1
)
!
(
ϕ
∧
)
n
=
∑
n
=
0
1
(
n
+
1
)
!
(
θ
a
∧
)
n
=
I
+
1
2
!
θ
a
∧
+
1
3
!
θ
2
a
∧
a
∧
+
1
4
!
θ
3
a
∧
a
∧
a
∧
+
1
5
!
θ
4
a
∧
a
∧
a
∧
a
∧
+
⋯
=
1
θ
(
1
2
!
θ
2
−
1
4
!
θ
4
+
⋯
)
a
∧
+
1
θ
(
1
3
!
θ
3
−
1
5
!
θ
5
+
⋯
)
a
∧
a
∧
+
I
=
1
θ
(
1
−
c
o
s
θ
)
a
∧
+
1
θ
(
θ
−
s
i
n
θ
)
a
∧
a
∧
+
I
=
1
−
c
o
s
θ
θ
a
∧
+
1
θ
(
θ
−
s
i
n
θ
)
(
a
a
T
−
I
)
+
I
=
1
−
c
o
s
θ
θ
a
∧
+
(
1
−
s
i
n
θ
θ
)
(
a
a
T
−
I
)
+
I
=
1
−
c
o
s
θ
θ
a
∧
+
(
1
−
s
i
n
θ
θ
)
a
a
T
+
s
i
n
θ
θ
I
t
=
J
ρ
J = \sum_{n=0}{\frac{1}{(n+1)!}(\phi^{\land})^n} = \sum_{n=0}{\frac{1}{(n+1)!}(\theta a^{\land})^n} \\ = I + \frac{1}{2!}\theta a^{\land} + \frac{1}{3!}\theta^2a^{\land}a^{\land} + \frac{1}{4!}\theta^3a^{\land}a^{\land}a^{\land} + \frac{1}{5!}\theta^4a^{\land}a^{\land}a^{\land}a^{\land} + \cdots \\ = \frac{1}{\theta}(\frac{1}{2!}\theta^2-\frac{1}{4!}\theta^4+\cdots)a^{\land}+\frac{1}{\theta}(\frac{1}{3!}\theta^3-\frac{1}{5!}\theta^5+\cdots)a^{\land}a^{\land}+I \\ = \frac{1}{\theta}(1-cos\theta)a^{\land}+\frac{1}{\theta}(\theta-sin\theta)a^{\land}a^{\land}+I \\ = \frac{1-cos\theta}{\theta}a^{\land}+\frac{1}{\theta}(\theta-sin\theta)(aa^T - I)+I \\ = \frac{1-cos\theta}{\theta}a^{\land} + (1-\frac{sin\theta}{\theta})(aa^T - I) + I \\ = \frac{1-cos\theta}{\theta}a^{\land} + (1-\frac{sin\theta}{\theta})aa^T + \frac{sin\theta}{\theta}I \\ t = J\rho
J=n=0∑(n+1)!1(ϕ∧)n=n=0∑(n+1)!1(θa∧)n=I+2!1θa∧+3!1θ2a∧a∧+4!1θ3a∧a∧a∧+5!1θ4a∧a∧a∧a∧+⋯=θ1(2!1θ2−4!1θ4+⋯)a∧+θ1(3!1θ3−5!1θ5+⋯)a∧a∧+I=θ1(1−cosθ)a∧+θ1(θ−sinθ)a∧a∧+I=θ1−cosθa∧+θ1(θ−sinθ)(aaT−I)+I=θ1−cosθa∧+(1−θsinθ)(aaT−I)+I=θ1−cosθa∧+(1−θsinθ)aaT+θsinθIt=Jρ
同样对数映射较为复杂,和
S
O
(
3
)
\ SO(3)
SO(3)一样的方式根据R
求对应的旋转向量
ϕ
=
θ
a
\ \phi=\theta a
ϕ=θa,同时根据
t
=
J
ρ
\ t=J\rho
t=Jρ求线性方程得到
ρ
\ \rho
ρ。