视觉SLAM十四讲|【一】三维空间刚体运动
旋转矩阵
S
O
(
n
)
=
{
R
∈
R
n
×
n
∣
R
R
T
=
I
,
d
e
t
(
R
)
=
1
}
SO(n)=\{ R \in \mathbb{R^{n \times n}} | RR^T=I,det(R)=1 \}
SO(n)={R∈Rn×n∣RRT=I,det(R)=1}
SO(n)为特殊正交群
设
a
a
a为一点坐标
(
x
,
y
,
z
)
(x,y,z)
(x,y,z)。
旋转与平移的结合
旋转与平移的结合可以描述为如下形式
a
′
=
R
T
a
+
t
a'=R^Ta+t
a′=RTa+t
性质
R
T
=
R
−
1
R^T = R^{-1}
RT=R−1
R
R
T
=
I
,
d
e
t
(
R
)
=
1
RR^T=I,det(R)=1
RRT=I,det(R)=1
旋转矩阵与变换矩阵
[
a
′
1
]
=
[
R
t
0
T
1
]
≜
T
[
a
′
1
]
\begin{bmatrix} a' \\ 1 \end{bmatrix} = \begin{bmatrix} R & t \\ 0^T & 1 \end{bmatrix} \triangleq T \begin{bmatrix} a' \\ 1 \end{bmatrix}
[a′1]=[R0Tt1]≜T[a′1]
关于变换矩阵，其具有特殊的结构：左上角位旋转矩阵，右侧为平移向量，左下角位0向量，右下角位1，该矩阵又称之为特殊欧式群
S
E
(
3
)
=
{
T
=
[
R
t
0
T
1
]
∈
R
4
×
4
∣
R
∈
S
O
(
3
)
,
t
∈
R
3
}
SE(3)={ \{ T=\begin{bmatrix} R & t \\ 0^T & 1 \end{bmatrix} \in \mathbb{R^{4 \times 4 } } | R \in SO(3), t \in \mathbb{R^3} \}}
SE(3)={T=[R0Tt1]∈R4×4∣R∈SO(3),t∈R3}
性质
求解该矩阵的逆表示一个反向的变换
T
−
1
=
[
R
T
−
R
T
t
0
T
1
]
T^{-1} = \begin{bmatrix} R^T & -R^Tt \\ 0^T & 1 \end{bmatrix}
T−1=[RT0T−RTt1]
旋转向量与欧拉角
旋转矩阵的缺点
- S O ( 3 ) SO(3) SO(3)用9个量表示3个自由度上的变换， S E ( 3 ) SE(3) SE(3)用16个量表示了6个自由度上的变换，存在冗余。
- 旋转矩阵自带约束：必须是个正交矩阵且行列式为1，约束会使得求解更加困难。
因此，使用旋转轴+旋转角来刻画旋转，该向量称之为旋转向量。
旋转向量到旋转矩阵
罗德里格斯公式
R
=
c
o
s
θ
+
(
1
−
c
o
s
θ
)
n
n
T
+
s
i
n
θ
n
∧
R = cos\theta + (1-cos\theta) n n^T + sin\theta n^{\land}
R=cosθ+(1−cosθ)nnT+sinθn∧
∧
\land
∧为向量到反对称的转换符（笔者也称之为矩阵化，反之解矩阵），其定位如下所示
a
∧
=
[
0
−
a
3
a
2
a
3
0
−
a
1
−
a
2
a
1
0
]
a^{\land} = \begin{bmatrix} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{bmatrix}
a∧=
0a3−a2−a30a1a2−a10
其中
n
n
n为旋转轴。
旋转矩阵到旋转向量
θ = a r c c o s ( t r ( R ) − 1 2 ) \theta = arccos(\frac{tr(R)-1}{2}) θ=arccos(2tr(R)−1)
旋转轴的性质
1、位于旋转轴上的向量在旋转后不发生改变，即
R
n
=
n
Rn=n
Rn=n
2、旋转轴
n
n
n是矩阵
R
R
R特征值
1
1
1对应的特征向量
四元数（Quaternion）
定义
q = q 0 + q 1 i + q 2 j + q 3 k q = q_0 + q_1 i + q_2 j + q_3 k q=q0+q1i+q2j+q3k
物理含义
假设某个旋转是围绕单位向量
n
=
[
n
x
,
n
y
,
n
z
]
T
n=[n_x, n_y, n_z]^T
n=[nx,ny,nz]T进行了角度为
θ
\theta
θ的旋转，这个旋转的四元数形式为
q
=
[
c
o
s
θ
2
,
n
x
s
i
n
θ
2
,
n
y
s
i
n
θ
2
,
n
z
s
i
n
θ
2
]
q = [cos\frac{\theta}{2}, n_x sin\frac{\theta}{2}, n_y sin\frac{\theta}{2}, n_z sin\frac{\theta}{2}]
q=[cos2θ,nxsin2θ,nysin2θ,nzsin2θ]
反之，亦可以从单位四元数中计算出对应的旋转轴与夹角
θ
=
2
a
r
c
c
o
s
(
q
0
)
\theta = 2arccos(q_0)
θ=2arccos(q0)
[
n
x
,
n
y
,
n
z
]
T
=
[
q
1
,
q
2
,
q
3
]
T
/
s
i
n
(
θ
/
2
)
[n_x, n_y, n_z]^T = [q_1, q_2, q_3]^T / sin(\theta/2)
[nx,ny,nz]T=[q1,q2,q3]T/sin(θ/2)
运算
四元数的定义可以写为如下形式
q
=
s
+
x
i
+
y
j
+
z
k
=
[
s
,
v
]
q = s + x i + y j + z k = [s, v]
q=s+xi+yj+zk=[s,v]
v
=
[
x
i
,
y
j
,
z
k
]
v = [xi, yj, zk]
v=[xi,yj,zk]
加法
q a ± q b = [ s a ± s b , v a ± v b ] q_a \pm q_b = [s_a \pm s_b, v_a \pm v_b] qa±qb=[sa±sb,va±vb]
乘法
q
a
q
b
=
s
a
s
b
−
x
a
x
b
−
y
a
y
b
−
z
a
z
b
+
(
s
a
x
b
+
x
a
s
b
+
y
a
z
b
−
z
a
y
b
)
i
+
(
s
a
y
b
−
x
a
z
b
+
y
a
s
b
+
z
a
x
b
)
j
+
(
s
a
z
b
+
x
a
y
b
−
y
a
x
b
+
z
a
s
b
)
k
\begin{split} q_a q_b &= s_a s_b - x_a x_b - y_a y_b - z_a z_b \\ & + (s_a x_b + x_a s_b + y_a z_b -z_ay_b)i \\ & + (s_ay_b - x_az_b + y_as_b + z_ax_b)j \\ & + (s_az_b + x_ay_b - y_ax_b + z_as_b)k \end{split}
qaqb=sasb−xaxb−yayb−zazb+(saxb+xasb+yazb−zayb)i+(sayb−xazb+yasb+zaxb)j+(sazb+xayb−yaxb+zasb)k
或者
q
a
q
b
=
[
s
a
s
b
−
v
a
T
v
b
,
s
a
v
b
+
s
b
v
a
+
v
a
×
v
b
]
q_a q_b = [s_as_b-v_a^Tv_b, s_av_b + s_bv_a + v_a \times v_b]
qaqb=[sasb−vaTvb,savb+sbva+va×vb]
矩阵外积的性质如下所是
模长
∣
∣
q
a
∣
∣
=
(
s
a
2
+
x
a
2
+
y
a
2
+
z
a
2
)
||q_a||= \sqrt{(s_a^2+x_a^2+y_a^2+z_a^2)}
∣∣qa∣∣=(sa2+xa2+ya2+za2)
∣
∣
q
a
q
b
∣
∣
=
∣
∣
q
a
∣
∣
∣
∣
q
b
∣
∣
||q_a q_b|| = ||q_a|| ||q_b||
∣∣qaqb∣∣=∣∣qa∣∣∣∣qb∣∣
逆
q
−
1
=
q
∗
/
∣
∣
q
∣
∣
2
q^{-1} = q^* / ||q||^2
q−1=q∗/∣∣q∣∣2
q
q
−
1
=
q
−
1
q
=
1
qq^{-1}=q^{-1}q=1
qq−1=q−1q=1
(
q
a
q
b
)
−
1
=
q
b
−
1
q
a
−
1
(q_a q_b)^{-1} = q_b^{-1}q_a^{-1}
(qaqb)−1=qb−1qa−1
数乘与点乘
q
a
⋅
q
b
=
s
a
s
b
+
x
a
x
b
+
y
a
y
b
+
z
a
z
b
q_a \cdot q_b = s_a s_b + x_ax_b + y_ay_b + z_az_b
qa⋅qb=sasb+xaxb+yayb+zazb
k
q
=
[
k
s
,
k
v
]
kq = [ks, kv]
kq=[ks,kv]
四元数表示旋转
假设一空间三维点
P
=
[
x
,
y
,
z
]
∈
R
3
P=[x,y,z] \in \mathbb{R^3}
P=[x,y,z]∈R3，由轴角
n
,
θ
n,\theta
n,θ表示旋转
三维空间点用一个虚四元数表示为
p
=
[
0
,
x
,
y
,
z
]
p = [0,x,y,z]
p=[0,x,y,z]
用四元数表示旋转为
q
=
[
c
o
s
θ
2
,
n
s
i
n
θ
2
]
q=[cos\frac{\theta}{2}, nsin\frac{\theta}{2}]
q=[cos2θ,nsin2θ]
乘积可以表示为
p
′
=
q
p
q
−
1
p'=qpq^{-1}
p′=qpq−1
四元数转旋转矩阵
R = [ 1 − 2 q 2 2 − 2 q 3 2 2 q 1 q 2 − 2 q 0 q 3 2 q 1 q 3 + 2 q 0 q 2 2 q 1 q 2 + 2 q 0 q 3 1 − 2 q 1 2 − 2 q 3 2 2 q 2 q 3 − 2 q 0 q 1 2 q 1 q 3 − 2 q 0 q 2 2 q 2 q 3 + 2 q 0 q 1 1 − 2 q 1 2 − 2 q 2 2 ] R = \begin{bmatrix} 1-2q_2^2-2q_3^2 & 2q_1q_2-2q_0q_3 & 2q_1q_3+2q_0q_2 \\ 2q_1q_2+2q_0q_3 & 1-2q_1^2-2q_3^2 & 2q_2q_3-2q_0q_1 \\ 2q_1q_3-2q_0q_2 & 2q_2q_3+2q_0q_1 & 1-2q_1^2-2q_2^2 \end{bmatrix} R= 1−2q22−2q322q1q2+2q0q32q1q3−2q0q22q1q2−2q0q31−2q12−2q322q2q3+2q0q12q1q3+2q0q22q2q3−2q0q11−2q12−2q22
306
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
