三维重建基础知识:四元素、旋转矩阵与旋转向量
旋转矩阵
1、坐标系绕X轴旋转
ϕ
\phi
ϕ角(Pitch 俯仰角)
R
x
=
[
1
0
0
0
c
o
s
ϕ
−
s
i
n
ϕ
0
s
i
n
ϕ
c
o
s
ϕ
]
R_{x}=\begin{gathered} \begin{bmatrix} 1 & 0&0 \\ 0 & cos\phi&-sin\phi\\0&sin\phi&cos\phi \end{bmatrix} \end{gathered}
Rx=⎣⎡1000cosϕsinϕ0−sinϕcosϕ⎦⎤
2、坐标系绕Y轴旋转
θ
\theta
θ角(Yaw 偏航角)
R
y
=
[
c
o
s
θ
0
s
i
n
θ
0
1
0
−
s
i
n
θ
0
c
o
s
θ
]
R_{y}=\begin{gathered} \begin{bmatrix} cos\theta & 0&sin\theta \\ 0 & 1&0\\-sin\theta&0&cos\theta \end{bmatrix} \end{gathered}
Ry=⎣⎡cosθ0−sinθ010sinθ0cosθ⎦⎤
3、坐标系绕Z轴旋转
ψ
\psi
ψ角(Roll 翻滚角)
R
z
=
[
c
o
s
ψ
−
s
i
n
ψ
0
s
i
n
ψ
c
o
s
ψ
0
0
0
1
]
R_{z}=\begin{gathered} \begin{bmatrix} cos\psi & -sin\psi&0 \\ sin\psi & cos\psi&0\\0&0&1\end{bmatrix} \end{gathered}
Rz=⎣⎡cosψsinψ0−sinψcosψ0001⎦⎤
整个坐标系的旋转矩阵是绕各个轴旋转的旋转矩阵的乘积。
R
=
R
x
∗
R
y
∗
R
z
R=R_x*R_y*R_z
R=Rx∗Ry∗Rz
旋转矩阵必须是正交的。
Rodrigue旋转向量
设旋转向量的单位向量为
r
=
(
r
x
,
r
y
,
r
z
)
r=(r_x,r_y,r_z)
r=(rx,ry,rz)(旋转轴),模为
θ
\theta
θ(旋转角),三维点
p
p
p在旋转向量
r
r
r的作用下变成
p
′
p'
p′:
p
′
=
c
o
s
θ
⋅
p
+
(
1
−
c
o
s
θ
)
(
p
⋅
r
)
⋅
r
+
s
i
n
θ
⋅
r
×
p
p'=cos\theta\cdot p+(1-cos\theta)(p\cdot r)\cdot r+sin\theta\cdot r\times p
p′=cosθ⋅p+(1−cosθ)(p⋅r)⋅r+sinθ⋅r×p
旋转向量转旋转矩阵:
R
=
I
+
(
1
−
c
o
s
θ
)
∗
P
2
+
s
i
n
θ
∗
P
R=I+(1-cos\theta)*P^2+sin\theta*P
R=I+(1−cosθ)∗P2+sinθ∗P
其中
P
=
[
0
−
r
z
r
y
r
z
0
−
r
x
−
r
y
r
x
0
]
P=\begin{gathered} \begin{bmatrix} 0 & -r_z&r_y \\ r_z & 0&-r_x\\-r_y&r_x&0\end{bmatrix} \end{gathered}
P=⎣⎡0rz−ry−rz0rxry−rx0⎦⎤
四元素
四元素是三维复数域,可表示为Q=[x,y,z,w]或Q=w+xi+yj+z*k,其中w是实部,[x,y,z]是虚部,[i,j,k]是虚轴,四元数是单位向量。描述了旋转轴
r
=
(
r
x
,
r
y
,
r
z
)
r=(r_x,r_y,r_z)
r=(rx,ry,rz)和角
θ
\theta
θ,对应的四元数为:
Q
=
[
c
o
s
(
θ
/
2
)
,
r
x
∗
s
i
n
(
θ
/
2
)
,
r
y
∗
s
i
n
(
θ
/
2
)
,
r
z
∗
s
i
n
(
θ
/
2
)
]
Q=[cos(\theta/2),r_x*sin(\theta/2),r_y*sin(\theta/2),r_z*sin(\theta/2)]
Q=[cos(θ/2),rx∗sin(θ/2),ry∗sin(θ/2),rz∗sin(θ/2)]
两个四元素叉乘的物理含义:先绕第一个四元素旋转,再绕第二个旋转。
Citation
https://blog.csdn.net/tchenjiant/article/details/51485745
https://blog.csdn.net/hongbin_xu/article/details/78929006
https://zhuanlan.zhihu.com/p/93563218