视觉SLAM十四讲|【一】三维空间刚体运动
视觉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?]=[R0T?t1?]?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=[R0T?t1?]∈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???a3?0a1??a2??a1?0?
?
其中
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?+q1?i+q2?j+q3?k
物理含义
假设某个旋转是围绕单位向量
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θ?,nx?sin2θ?,ny?sin2θ?,nz?sin2θ?]
反之，亦可以从单位四元数中计算出对应的旋转轴与夹角
θ
=
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}
qa?qb??=sa?sb??xa?xb??ya?yb??za?zb?+(sa?xb?+xa?sb?+ya?zb??za?yb?)i+(sa?yb??xa?zb?+ya?sb?+za?xb?)j+(sa?zb?+xa?yb??ya?xb?+za?sb?)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]
qa?qb?=[sa?sb??vaT?vb?,sa?vb?+sb?va?+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||
∣∣qa?qb?∣∣=∣∣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}
(qa?qb?)?1=qb?1?qa?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?=sa?sb?+xa?xb?+ya?yb?+za?zb?
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??2q32?2q1?q2?+2q0?q3?2q1?q3??2q0?q2??2q1?q2??2q0?q3?1?2q12??2q32?2q2?q3?+2q0?q1??2q1?q3?+2q0?q2?2q2?q3??2q0?q1?1?2q12??2q22?? ?
本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。 如若内容造成侵权/违法违规/事实不符，请联系我的编程经验分享网邮箱：veading@qq.com进行投诉反馈，一经查实，立即删除！