1 二维空间刚体变换 (3个自由度)
二维空间的李群李代数参考Lie Groups for 2D and 3D Transformations。
1.1 李群 S E ( 2 ) \mathrm{SE}(2) SE(2)
二维刚体变换公式为
T
,
T
∈
S
E
(
2
)
T, T \in \mathrm{SE}(2)
T,T∈SE(2), 如下所示:
R
=
[
cos
θ
−
sin
θ
sin
θ
cos
θ
]
∈
SO
(
2
)
,
t
=
[
t
x
t
y
]
∈
R
2
T
=
(
R
t
0
1
)
=
[
cos
θ
−
sin
θ
t
x
sin
θ
cos
θ
t
y
0
0
1
]
∈
SE
(
2
)
\begin{aligned} & \mathbf{R}=\left[\begin{array}{cc} \cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right] \in \operatorname{SO}(2), \mathbf{t}=\left[\begin{array}{l} t_x \\ t_y \end{array}\right] \in \mathbb{R}^2 \\ & T=\left(\begin{array}{ll} \mathbf{R} & \mathbf{t} \\ \mathbf{0} & 1 \end{array}\right)=\left[\begin{array}{ccc} \cos \theta & -\sin \theta & t_x \\ \sin \theta & \cos \theta & t_y \\ 0 & 0 & 1 \end{array}\right] \in \operatorname{SE}(2) \end{aligned}
R=[cosθsinθ−sinθcosθ]∈SO(2),t=[txty]∈R2T=(R0t1)=
cosθsinθ0−sinθcosθ0txty1
∈SE(2)
1.2 李代数 s e ( 2 ) {{s e}(2)} se(2)
与
S
E
(
2
)
\mathrm{SE}(2)
SE(2)对应的李代数
ϕ
∧
∈
s
e
(
2
)
\boldsymbol{\phi}^{\wedge} \in \mathrm{se}(2)
ϕ∧∈se(2), 如下
G
1
=
(
0
0
1
0
0
0
0
0
0
)
,
G
2
=
(
0
0
0
0
0
1
0
0
0
)
,
G
3
=
(
0
−
1
0
1
0
0
0
0
0
)
ϕ
=
(
u
1
u
2
θ
)
T
∈
R
3
ϕ
∧
=
u
1
G
1
+
u
2
G
2
+
θ
G
3
=
[
0
−
θ
u
1
θ
0
u
2
0
0
0
]
∈
se (2)
\begin{aligned} & G_1=\left(\begin{array}{lll} 0 & 0 & 1 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \end{array}\right), G_2=\left(\begin{array}{ccc} 0 & 0 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & 0 \end{array}\right), G_3=\left(\begin{array}{ccc} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 0 \end{array}\right) \\ & \boldsymbol{\phi}=\left(\begin{array}{lll} {u}_1 & {u}_2 & \theta \end{array}\right)^T \quad \in \mathbb{R}^3 \\ & \boldsymbol{\phi}^{\wedge}={u}_1 G_1+{u}_2 G_2+\theta G_3 \quad=\left[\begin{array}{ccc} 0 & -\theta & \mathrm{u}_1 \\ \theta & 0 & \mathrm{u}_2 \\ 0 & 0 & 0 \end{array}\right] \in \text { se (2) } & \end{aligned}
G1=
000000100
,G2=
000000010
,G3=
010−100000
ϕ=(u1u2θ)T∈R3ϕ∧=u1G1+u2G2+θG3=
0θ0−θ00u1u20
∈ se (2) 上式中的
ϕ
\boldsymbol{\phi}
ϕ 即为我们理解的旋转向量, 或者称为角轴。注意
ϕ
∧
\boldsymbol{\phi}^{\wedge}
ϕ∧ 是反对称阵, 但是和
s
o
(
3
)
\mathbf{so}(3)
so(3) 中的反对称阵写法不同。在视觉 SLAM 十四讲中, 主要介绍的是三维空间。
1.3 指数映射(将李代数 se ( 2 ) \operatorname{se}(2) se(2) 转换为李群 S E ( 2 ) \mathrm{SE}(2) SE(2))
已知李代数
se
(
2
)
\operatorname{se}(2)
se(2) 如下:
ϕ
=
[
u
1
u
1
θ
]
∈
R
3
ϕ
∧
=
[
0
−
θ
u
1
θ
0
u
2
0
0
0
]
∈
se
(
2
)
\begin{gathered} \boldsymbol{\phi}=\left[\begin{array}{c} \mathrm{u}_1 \\ \mathrm{u}_1 \\ \theta \end{array}\right] \in \mathbb{R}^3 \\ \boldsymbol{\phi}^{\wedge}=\left[\begin{array}{ccc} 0 & -\theta & \mathrm{u}_1 \\ \theta & 0 & \mathrm{u}_2 \\ 0 & 0 & 0 \end{array}\right] \in \operatorname{se}(2) \end{gathered}
ϕ=
u1u1θ
∈R3ϕ∧=
0θ0−θ00u1u20
∈se(2)那么对应的李群
S
E
(
2
)
\mathrm{SE}(2)
SE(2) 为:
T
=
exp
(
ϕ
∧
)
=
(
exp
(
θ
∧
)
V
u
0
0
)
…
…
…
…
(1)
\mathrm{T}=\exp \left(\boldsymbol{\phi}^{\wedge}\right)=\left(\begin{array}{cc} \exp \left(\theta^{\wedge}\right) & \mathbf{V u} \\ \mathbf{0} & 0 \end{array}\right) \ldots \ldots \ldots \ldots \text { (1) }
T=exp(ϕ∧)=(exp(θ∧)0Vu0)………… (1) 其中
exp
(
θ
∧
)
=
[
cos
θ
−
sin
θ
sin
θ
cos
θ
]
,
V
=
1
θ
⋅
(
sin
θ
−
(
1
−
cos
θ
)
1
−
cos
θ
sin
θ
)
\exp \left(\theta^{\wedge}\right)=\left[\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta\end{array}\right], \boldsymbol{V}=\frac{1}{\theta} \cdot\left(\begin{array}{cc}\sin \theta & -(1-\cos \theta) \\ 1-\cos \theta & \sin \theta\end{array}\right)
exp(θ∧)=[cosθsinθ−sinθcosθ],V=θ1⋅(sinθ1−cosθ−(1−cosθ)sinθ),
u
=
[
u
1
u
2
]
\mathbf{u}=\left[\begin{array}{l}\mathrm{u}_1 \\ \mathrm{u}_2\end{array}\right]
u=[u1u2] 。那么
T
=
exp
(
ϕ
∧
)
=
[
cos
θ
−
sin
θ
sin
θ
θ
u
1
−
1
−
cos
θ
θ
u
2
sin
θ
cos
θ
1
−
cos
θ
θ
u
1
+
sin
δ
θ
δ
θ
u
2
0
0
1
]
…
…
…
…
(2)
\mathrm{T}=\exp \left(\boldsymbol{\phi}^{\wedge}\right)=\left[\begin{array}{ccc} \cos \theta & -\sin \theta & \frac{\sin \theta}{\theta} \mathrm{u}_1-\frac{1-\cos \theta}{\theta} \mathrm{u}_2 \\ \sin \theta & \cos \theta & \frac{1-\cos \theta}{\theta} \mathrm{u}_1+\frac{\sin \delta_\theta}{\delta_\theta} \mathrm{u}_2 \\ 0 & 0 & 1 \end{array}\right] \ldots \ldots \ldots \ldots \text { (2) }
T=exp(ϕ∧)=
cosθsinθ0−sinθcosθ0θsinθu1−θ1−cosθu2θ1−cosθu1+δθsinδθu21
………… (2) 从公式 (2) 可以知道, 李代数的
ϕ
\boldsymbol{\phi}
ϕ 这个向量中的三个量
u
1
\mathrm{u}_1
u1 、
u
2
、
θ
\mathrm{u}_2 、 \theta
u2、θ, 刚好能对应到
T
\mathbf{T}
T 中的
θ
\theta
θ, 因此李代数中的
θ
\theta
θ 确实真实反映了旋转,可是
u
1
、
u
2
\mathrm{u}_1 、 \mathrm{u}_2
u1、u2 只是部分反映了平移, 需要乘以
V
\boldsymbol{V}
V, 才是变换成
T
\mathbf{T}
T 后真正的平移量。
2 三维空间刚体变换(6个自由度)
三维空间中旋转矩阵的写法, 我发现大家习惯这样写这个旋转矩阵的公式, 如下:
R
=
[
cos
γ
cos
β
cos
γ
sin
β
sin
α
−
sin
γ
cos
α
cos
γ
sin
β
cos
α
+
sin
γ
sin
α
sin
γ
cos
β
sin
γ
sin
β
sin
α
+
cos
γ
cos
α
sin
γ
sin
β
cos
α
−
cos
γ
sin
α
−
sin
β
cos
β
sin
α
cos
β
sin
α
]
3
×
3
∈
SO
(
3
)
\mathbf{R}=\left[\begin{array}{ccc} \cos \gamma \cos \beta & \cos \gamma \sin \beta \sin \alpha-\sin \gamma \cos \alpha & \cos \gamma \sin \beta \cos \alpha+\sin \gamma \sin \alpha \\ \sin \gamma \cos \beta & \sin \gamma \sin \beta \sin \alpha+\cos \gamma \cos \alpha & \sin \gamma \sin \beta \cos \alpha-\cos \gamma \sin \alpha \\ -\sin \beta & \cos \beta \sin \alpha & \cos \beta \sin \alpha \end{array}\right]_{3 \times 3} \in \operatorname{SO}(3)
R=
cosγcosβsinγcosβ−sinβcosγsinβsinα−sinγcosαsinγsinβsinα+cosγcosαcosβsinαcosγsinβcosα+sinγsinαsinγsinβcosα−cosγsinαcosβsinα
3×3∈SO(3)因为之前上过牛小骥（武汉大学）的惯导课，感兴趣可以去b站看一下，同时对于一个载体的姿态比较敏感。遂将上面的旋转矩阵和上课时用的旋转矩阵进行了对比, 截取了 PPT 中的内容, 其中旋转矩阵公式如下。下面的旋转矩阵的含义是将 b 系中的点转换到 n 系下。但是旋转的顺序是从 n 系旋转到 b 系得到的旋转矩阵。
C
b
n
=
(
c
θ
c
ψ
−
c
ϕ
s
ψ
+
s
ϕ
s
θ
c
ψ
s
ϕ
s
ψ
+
c
ϕ
s
θ
c
ψ
c
θ
s
ψ
c
ϕ
c
ψ
+
s
ϕ
s
θ
s
ψ
−
s
ϕ
c
ψ
+
c
ϕ
s
θ
s
ψ
−
s
θ
s
ϕ
c
θ
c
ϕ
c
θ
)
\mathbf{C}_b^n=\left(\begin{array}{ccc} c \theta c \psi & -c \phi s \psi+s \phi s \theta c \psi & s \phi s \psi+c \phi s \theta c \psi \\ c \theta s \psi & c \phi c \psi+s \phi s \theta s \psi & -s \phi c \psi+c \phi s \theta s \psi \\ -s \theta & s \phi c \theta & c \phi c \theta \end{array}\right)
Cbn=
cθcψcθsψ−sθ−cϕsψ+sϕsθcψcϕcψ+sϕsθsψsϕcθsϕsψ+cϕsθcψ−sϕcψ+cϕsθsψcϕcθ
说明一下: 公式中的
ψ
\psi
ψ 为航向角(绕 z 轴旋转),
θ
\theta
θ 为俯仰角(绕 y 轴旋转),
ϕ
\phi
ϕ 为横滚角( 绕 x 轴旋转)。旋转顺序分别是 ZYX。
与大家常用的公式进行了对比, 上面公式中的
α
\alpha
α 对应的是
ϕ
\phi
ϕ (横滚角),
β
\beta
β 对应的是
θ
\theta
θ (俯仰角),
γ
\gamma
γ 对应的是
ψ
\psi
ψ (航向角)。
2.1 李群 S E ( 3 ) \mathrm{SE}(3) SE(3)
三维刚体变换公式为 T , T ∈ S E ( 3 ) T, T \in \mathrm{SE}(3) T,T∈SE(3), 如下所示: R = [ cos γ cos β cos γ sin β sin α − sin γ cos α cos γ sin β cos α + sin γ sin α sin γ cos β sin γ sin β sin α + cos γ cos α sin γ sin β cos α − cos γ sin α − sin β cos β sin α cos β sin α ] ∈ SO ( 3 ) , t = [ t x t y t z ] ∈ R 3 T = ( R t 0 1 ) = [ cos γ cos β cos γ sin β sin α − sin γ cos α cos γ sin β cos α + sin γ sin α t x sin γ cos β sin γ sin β sin α + cos γ cos α sin γ sin β cos α − cos γ sin α t y − sin β cos β sin α cos β sin α t z 0 0 0 1 ] ∈ SE ( 3 ) \begin{gathered} \mathbf{R}=\left[\begin{array}{ccc} \cos \gamma \cos \beta & \cos \gamma \sin \beta \sin \alpha-\sin \gamma \cos \alpha & \cos \gamma \sin \beta \cos \alpha+\sin \gamma \sin \alpha \\ \sin \gamma \cos \beta & \sin \gamma \sin \beta \sin \alpha+\cos \gamma \cos \alpha & \sin \gamma \sin \beta \cos \alpha-\cos \gamma \sin \alpha \\ -\sin \beta & \cos \beta \sin \alpha & \cos \beta \sin \alpha \end{array}\right] \in \operatorname{SO}(3), \\ \mathbf{t}=\left[\begin{array}{c} t_x \\ t_y \\ t_z \end{array}\right] \in \mathbb{R}^3 \\ \operatorname{T}=\left(\begin{array}{ll} \mathbf{R} & \mathbf{t} \\ \mathbf{0} & 1 \end{array}\right) \\ =\left[\begin{array}{clll} \cos \gamma \cos \beta & \cos \gamma \sin \beta \sin \alpha-\sin \gamma \cos \alpha & \cos \gamma \sin \beta \cos \alpha+\sin \gamma \sin \alpha & t_x \\ \sin \gamma \cos \beta & \sin \gamma \sin \beta \sin \alpha+\cos \gamma \cos \alpha & \sin \gamma \sin \beta \cos \alpha-\cos \gamma \sin \alpha & t_y \\ -\sin \beta & \cos \beta \sin \alpha & \cos \beta \sin \alpha & t_z \\ 0 & 0 & 0 & 1 \end{array}\right] \in \operatorname{SE}(3) \end{gathered} R= cosγcosβsinγcosβ−sinβcosγsinβsinα−sinγcosαsinγsinβsinα+cosγcosαcosβsinαcosγsinβcosα+sinγsinαsinγsinβcosα−cosγsinαcosβsinα ∈SO(3),t= txtytz ∈R3T=(R0t1)= cosγcosβsinγcosβ−sinβ0cosγsinβsinα−sinγcosαsinγsinβsinα+cosγcosαcosβsinα0cosγsinβcosα+sinγsinαsinγsinβcosα−cosγsinαcosβsinα0txtytz1 ∈SE(3)
2.2 李代数 s e ( 3 ) {{s e}(3)} se(3)
与之对应的李代数
ξ
=
[
u
ϕ
]
=
[
u
1
u
2
u
3
ϕ
1
ϕ
2
ϕ
3
]
∈
R
6
\boldsymbol{\xi}=\left[\begin{array}{l}\boldsymbol{u} \\ \boldsymbol{\phi}\end{array}\right]=\left[\begin{array}{l}u_1 \\ u_2 \\ u_3 \\ \phi_1 \\ \phi_2 \\ \phi_3\end{array}\right] \in \mathbb{R}^6
ξ=[uϕ]=
u1u2u3ϕ1ϕ2ϕ3
∈R6
G
1
=
(
0
0
0
1
0
0
0
0
0
0
0
0
0
0
0
0
)
,
G
2
=
(
0
0
0
0
0
0
0
1
0
0
0
0
0
0
0
0
)
,
G
3
=
(
0
0
0
0
0
0
0
0
0
0
0
1
0
0
0
0
)
G_1=\left(\begin{array}{llll} 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \end{array}\right), \quad G_2=\left(\begin{array}{llll} 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \end{array}\right), G_3=\left(\begin{array}{llll} 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 \end{array}\right)
G1=
0000000000001000
,G2=
0000000000000100
,G3=
0000000000000010
G
4
=
(
0
0
0
0
0
0
−
1
0
0
1
0
0
0
0
0
0
)
,
G
5
=
(
0
0
1
0
0
0
0
0
−
1
0
0
0
0
0
0
0
)
,
G
6
=
(
0
−
1
0
0
1
0
0
0
0
0
0
0
0
0
0
0
)
G_4=\left(\begin{array}{cccc} 0 & 0 & 0 & 0 \\ 0 & 0 & -1 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 \end{array}\right), G_5=\left(\begin{array}{cccc} 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 \\ -1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \end{array}\right), G_6=\left(\begin{array}{cccc} 0 & -1 & 0 & 0 \\ 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \end{array}\right)
G4=
000000100−1000000
,G5=
00−10000010000000
,G6=
0100−100000000000
ξ
∧
=
u
1
G
1
+
u
2
G
2
+
u
3
G
3
+
ϕ
1
G
4
+
ϕ
2
G
5
+
ϕ
3
G
6
=
[
0
−
ϕ
3
ϕ
2
u
1
ϕ
3
0
−
ϕ
1
u
2
−
ϕ
2
ϕ
1
0
u
3
0
0
0
0
]
\begin{aligned} \xi^{\wedge}=u_1 G_1 & +u_2 G_2+u_3 G_3+\phi_1 G_4+\phi_2 G_5+\phi_3 G_6 \\ & =\left[\begin{array}{cccc} 0 & -\phi_3 & \phi_2 & u_1 \\ \phi_3 & 0 & -\phi_1 & u_2 \\ -\phi_2 & \phi_1 & 0 & u_3 \\ 0 & 0 & 0 & 0 \end{array}\right] \end{aligned}
ξ∧=u1G1+u2G2+u3G3+ϕ1G4+ϕ2G5+ϕ3G6=
0ϕ3−ϕ20−ϕ30ϕ10ϕ2−ϕ100u1u2u30
换言之，
ξ
∧
=
[
ϕ
∧
u
0
T
0
]
\boldsymbol{\xi}^{\wedge}=\left[\begin{array}{ll}\boldsymbol{\phi}^{\wedge} & \boldsymbol{u} \\ 0^T & 0\end{array}\right]
ξ∧=[ϕ∧0Tu0] ，这个是视觉 SLAM 十四讲中喜欢用的写法，上面的展开是从Lie Groups for 2D and 3D Transformations中摘抄的。
上式中的
ξ
\boldsymbol{\xi}
ξ 即为我们理解的旋转向量, 或者称为角轴。注意
ξ
∧
\boldsymbol{\xi}^{\wedge}
ξ∧ 是反对称阵。在视觉 SLAM 十四讲中，主要介绍的是三维空间，李群 SE (3)和李代数 se(3)是——对应（旋转角度在
±
π
\pm \pi
±π 之间，李群和李代数元素是——对应的）的，
S
E
(
3
)
\mathrm{SE}(3)
SE(3) 为
4
×
4
4 \times 4
4×4 的旋转矩阵,
s
e
(
3
)
\mathrm{se}(3)
se(3) 为
6
×
1
6 \times 1
6×1 的旋转向量。李群
S
O
(
3
)
\mathrm{SO}(3)
SO(3) 和李代数
s
o
(
3
)
\mathrm{so}(3)
so(3) 是一一对应的,
S
O
(
3
)
\mathrm{SO}(3)
SO(3)为
3
×
3
3 \times 3
3×3 的旋转矩阵，
s
o
(
3
)
\mathrm{so}(3)
so(3) 为
3
×
1
3 \times 1
3×1 的旋转向量。
2.3 指数映射（将李代数 s e ( 3 ) \mathrm{se}(3) se(3) 转换为李群SE(3)）
已知李代数
se
(
3
)
\operatorname{se}(3)
se(3) 如下:
ξ
=
[
u
ϕ
]
=
[
u
1
u
2
u
3
ϕ
1
ϕ
2
ϕ
3
]
∈
R
6
ξ
∧
=
[
0
−
ϕ
3
ϕ
2
u
1
ϕ
3
0
−
ϕ
1
u
2
−
ϕ
2
ϕ
1
0
u
3
0
0
0
0
]
\begin{gathered} \xi=\left[\begin{array}{l} \boldsymbol{u} \\ \boldsymbol{\phi} \end{array}\right]=\left[\begin{array}{l} u_1 \\ u_2 \\ u_3 \\ \phi_1 \\ \phi_2 \\ \phi_3 \end{array}\right] \in \mathbb{R}^6 \\ \xi^{\wedge}=\left[\begin{array}{cccc} 0 & -\phi_3 & \phi_2 & u_1 \\ \phi_3 & 0 & -\phi_1 & u_2 \\ -\phi_2 & \phi_1 & 0 & u_3 \\ 0 & 0 & 0 & 0 \end{array}\right] \end{gathered}
ξ=[uϕ]=
u1u2u3ϕ1ϕ2ϕ3
∈R6ξ∧=
0ϕ3−ϕ20−ϕ30ϕ10ϕ2−ϕ100u1u2u30
那么对应的李群 SE(3)为:
T
=
[
exp
(
ϕ
∧
)
J
u
0
T
1
]
=
[
R
J
u
0
T
1
]
\mathrm{T}=\left[\begin{array}{cc} \exp \left(\boldsymbol{\phi}^{\wedge}\right) & \boldsymbol{J} u \\ 0^T & 1 \end{array}\right]=\left[\begin{array}{cc} \mathbf{R} & \boldsymbol{J} \boldsymbol{u} \\ 0^T & 1 \end{array}\right]
T=[exp(ϕ∧)0TJu1]=[R0TJu1] 其中
exp
(
ϕ
∧
)
=
\exp \left(\boldsymbol{\phi}^{\wedge}\right)=
exp(ϕ∧)=
[
cos
γ
cos
β
cos
γ
sin
β
sin
α
−
sin
γ
cos
α
cos
γ
sin
β
cos
α
+
sin
γ
sin
α
sin
γ
cos
β
sin
γ
sin
β
sin
α
+
cos
γ
cos
α
sin
γ
sin
β
cos
α
−
cos
γ
sin
α
−
sin
β
cos
β
sin
α
cos
β
sin
α
]
\left[\begin{array}{ccc} \cos \gamma \cos \beta & \cos \gamma \sin \beta \sin \alpha-\sin \gamma \cos \alpha & \cos \gamma \sin \beta \cos \alpha+\sin \gamma \sin \alpha \\ \sin \gamma \cos \beta & \sin \gamma \sin \beta \sin \alpha+\cos \gamma \cos \alpha & \sin \gamma \sin \beta \cos \alpha-\cos \gamma \sin \alpha \\ -\sin \beta & \cos \beta \sin \alpha & \cos \beta \sin \alpha \end{array}\right]
cosγcosβsinγcosβ−sinβcosγsinβsinα−sinγcosαsinγsinβsinα+cosγcosαcosβsinαcosγsinβcosα+sinγsinαsinγsinβcosα−cosγsinαcosβsinα
不过这里面的
α
,
β
\alpha, \beta
α,β 和
γ
\gamma
γ 与
ϕ
1
,
ϕ
2
,
ϕ
3
\phi_1, \phi_2, \phi_3
ϕ1,ϕ2,ϕ3 的具体对应关系我还没有搞清楚。
J
\boldsymbol{J}
J 文档Lie Groups for 2D and 3D Transformations中的写法与视觉 SLAM 十四讲中1不一样，随后我进行了推倒，也就是将文档中的
J
\boldsymbol{J}
J 进行了推倒，看看它是否和视觉 SLAM 十四讲中一致。此时我们提前预备两个知识点:
- 已知三维向量 ϕ 3 × 1 = θ a , a \boldsymbol{\phi}_{3 \times 1}=\theta \boldsymbol{a}, \boldsymbol{a} ϕ3×1=θa,a 是一个长度为 1 \mathbf{1} 1 的方向向量, θ \theta θ 是 ϕ \boldsymbol{\phi} ϕ 的模长。那么, ϕ ∧ = θ a ∧ \boldsymbol{\phi}^{\wedge}=\theta \boldsymbol{a}^{\wedge} ϕ∧=θa∧
- 单位向量
a
\boldsymbol{a}
a 有一个性质:
a
∧
a
∧
=
a
a
T
−
I
\boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}=\boldsymbol{a} \boldsymbol{a}^T-\boldsymbol{I}
a∧a∧=aaT−I 这里面符号统一一下：
a
∧
\boldsymbol{a}^{\wedge}
a∧ 和
a
×
\boldsymbol{a}_{\times}
a×是一个意思，都是求三维向量的反对称阵。另外, 文档Lie Groups for 2D and 3D Transformations给的求
J
\boldsymbol{J}
J 的公式如下:
θ
=
ϕ
T
ϕ
A
=
sin
θ
θ
B
=
1
−
cos
θ
θ
2
C
=
1
−
A
θ
2
R
=
I
+
A
ϕ
∧
+
B
ϕ
∧
ϕ
∧
J
=
I
+
B
ϕ
∧
+
C
ϕ
∧
ϕ
∧
\begin{aligned} \theta & =\sqrt{\boldsymbol{\phi}^T \boldsymbol{\phi}} \\ A & =\frac{\sin \theta}{\theta} \\ B & =\frac{1-\cos \theta}{\theta^2} \\ C & =\frac{1-A}{\theta^2} \\ \mathbf{R} & =\mathbf{I}+A \boldsymbol{\phi}^{\wedge}+B \boldsymbol{\phi}^{\wedge} \boldsymbol{\phi}^{\wedge} \\ \boldsymbol{J} & =\mathbf{I}+B \boldsymbol{\phi}^{\wedge}+C \boldsymbol{\phi}^{\wedge} \boldsymbol{\phi}^{\wedge} \end{aligned}
θABCRJ=ϕTϕ=θsinθ=θ21−cosθ=θ21−A=I+Aϕ∧+Bϕ∧ϕ∧=I+Bϕ∧+Cϕ∧ϕ∧ 遂对文档中的
J
\boldsymbol{J}
J 进行了推倒, 看看它是否能推导到和十四讲中一致，过程如下:
J
=
I
+
B
ϕ
∧
+
C
ϕ
∧
ϕ
∧
=
I
+
1
−
cos
θ
θ
2
θ
a
∧
+
1
−
sin
θ
θ
θ
2
(
θ
2
a
∧
a
∧
)
=
I
+
1
−
cos
θ
θ
a
∧
+
(
1
−
sin
θ
θ
)
(
a
a
T
−
I
)
=
I
+
1
−
cos
θ
θ
a
∧
+
a
a
T
−
I
−
sin
θ
θ
a
a
T
+
sin
θ
θ
I
=
sin
θ
θ
I
+
(
1
−
sin
θ
θ
)
a
a
T
+
1
−
cos
θ
θ
a
∧
\begin{aligned} \boldsymbol{J}=\boldsymbol{I}+B \boldsymbol{\phi}^{\wedge} & +C \boldsymbol{\phi}^{\wedge} \boldsymbol{\phi}^{\wedge}=\boldsymbol{I}+\frac{1-\cos \theta}{\theta^2} \theta \boldsymbol{a}^{\wedge}+\frac{1-\frac{\sin \theta}{\theta}}{\theta^2}\left(\theta^2 \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}\right) \\ & =\boldsymbol{I}+\frac{1-\cos \theta}{\theta} \boldsymbol{a}^{\wedge}+\left(1-\frac{\sin \theta}{\theta}\right)\left(\boldsymbol{a} \boldsymbol{a}^T-\boldsymbol{I}\right) \\ = & \boldsymbol{I}+\frac{1-\cos \theta}{\theta} \boldsymbol{a}^{\wedge}+\boldsymbol{a} \boldsymbol{a}^T-\boldsymbol{I}-\frac{\sin \theta}{\theta} \boldsymbol{a} \boldsymbol{a}^T+\frac{\sin \theta}{\theta} \boldsymbol{I} \\ = & \frac{\sin \theta}{\theta} \boldsymbol{I}+\left(1-\frac{\sin \theta}{\theta}\right) \boldsymbol{a} \boldsymbol{a}^T+\frac{1-\cos \theta}{\theta} \boldsymbol{a}^{\wedge} \end{aligned}
J=I+Bϕ∧==+Cϕ∧ϕ∧=I+θ21−cosθθa∧+θ21−θsinθ(θ2a∧a∧)=I+θ1−cosθa∧+(1−θsinθ)(aaT−I)I+θ1−cosθa∧+aaT−I−θsinθaaT+θsinθIθsinθI+(1−θsinθ)aaT+θ1−cosθa∧ 至此, 我们求出来的
J
\boldsymbol{J}
J和视觉 SLAM 十四讲中的公式一致了, 也解决了我心中的疑惑。
从公式 T = [ exp ( ϕ ∧ ) J u 0 T 1 ] = [ R J u 0 T 1 ] \mathrm{T}=\left[\begin{array}{cc} \exp \left(\boldsymbol{\phi}^{\wedge}\right) & \boldsymbol{J} \boldsymbol{u} \\ 0^T & 1 \end{array}\right]=\left[\begin{array}{cc} \mathbf{R} & \boldsymbol{J} \boldsymbol{u} \\ 0^T & 1 \end{array}\right] T=[exp(ϕ∧)0TJu1]=[R0TJu1] 可知, 李代数 se ( 3 ) \operatorname{se}(3) se(3)中由两个三维向量( u \boldsymbol{u} u和 ϕ \boldsymbol{\phi} ϕ)构成， ϕ \boldsymbol{\phi} ϕ这个量刚好能对应到 T \mathbf{T} T 中的 R \mathbf{R} R, 因此李代数中的 ϕ \boldsymbol{\phi} ϕ确实真实反映了旋转, 可是 u \boldsymbol{u} u只是部分反映了平移, 需要乘以 J \boldsymbol{J} J , 才是变换成 T \mathbf{T} T 后真正的平移量。这一点与二维空间中的se(2)转换到 SE(2) 类似。
高翔, 张涛, 刘毅, 等. 视觉SLAM十四讲——从理论到实践[M]. 北京: 电子工业出版社, 2017: 17-22. ↩︎
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
