总是碰到很多数学问题,有些东西碰到都得查一遍,干脆整理到一块凑成一篇文章
1. 点乘叉乘
①点乘(内积):
a
⋅
b
=
∣
a
∣
∣
b
∣
c
o
s
θ
=
a
T
b
=
∑
i
=
0
N
a
i
∗
b
i
a\cdot b = |a||b|cos\theta=a^Tb=\sum_{i=0}^Na_i*b_i
a⋅b=∣a∣∣b∣cosθ=aTb=∑i=0Nai∗bi
表示一个向量到另一个向量的投影乘上被投影向量的模,是个数值;
其±由
c
o
s
θ
cos\theta
cosθ决定,表示了两个向量是否同向;
②叉乘(外积):
a
×
b
=
[
i
j
k
a
1
a
2
a
3
b
1
b
2
b
3
]
=
[
a
2
b
3
−
a
3
b
2
a
3
b
1
−
a
1
b
3
a
1
b
2
−
a
2
b
1
]
=
[
0
a
3
−
a
2
−
a
3
0
a
1
a
2
−
a
1
b
3
]
b
=
a
ˆ
b
a\times b=\begin{bmatrix} i & j & k \\ a_1 & a_2 & a_3 \\ b_1 & b_2 & b_3 \end{bmatrix}=\begin{bmatrix} a_2b_3-a_3b_2 \\ a_3b_1-a_1b_3 \\ a_1b_2-a_2b_1 \end{bmatrix} = \begin{bmatrix} 0 & a_3 & -a_2 \\ -a_3 & 0 & a_1 \\ a_2 & -a_1 & b_3 \end{bmatrix}b=a\^{\ }b
a×b=⎣
⎡ia1b1ja2b2ka3b3⎦
⎤=⎣
⎡a2b3−a3b2a3b1−a1b3a1b2−a2b1⎦
⎤=⎣
⎡0−a3a2a30−a1−a2a1b3⎦
⎤b=a ˆb
叉积只在3维下有效,其结果为一个向量,方向符合a到b的右手定则表示,叉积的这种表示方向的性质还常常被用于图形学中(如计算点是否在图形内);
此处的
a
ˆ
a\^{\ }
a ˆ表示a构成的反对称矩阵,这在SLAM中非常常用,是叉乘的另一种表现形式;
2. 坐标系的欧式变换
在SLAM中,常说在那个点在哪个坐标系下的表示,又常坐标系之间的位姿变换;
对于一个固定点p,其在W和C两个坐标系下的意义应该是相同的,有等式:
[ e 1 e 2 e 3 ] [ a 1 a 2 a 3 ] = [ e 1 ′ e 2 ′ e 3 ′ ] [ a 1 ′ a 2 ′ a 3 ′ ] \begin{bmatrix} e_1 & e_2 & e_3\end{bmatrix}\begin{bmatrix} a_1 \\ a_2 \\ a_3\end{bmatrix} = \begin{bmatrix} e_1' & e_2' & e_3'\end{bmatrix}\begin{bmatrix} a_1' \\ a_2' \\ a_3'\end{bmatrix} [e1e2e3]⎣ ⎡a1a2a3⎦ ⎤=[e1′e2′e3′]⎣ ⎡a1′a2′a3′⎦ ⎤这里的 [ e 1 e 2 e 3 ] \begin{bmatrix} e_1 & e_2 & e_3\end{bmatrix} [e1e2e3]和 [ e 1 ′ e 2 ′ e 3 ′ ] \begin{bmatrix} e_1' & e_2' & e_3'\end{bmatrix} [e1′e2′e3′]表示两个坐标系下的基,而 [ a 1 a 2 a 3 ] \begin{bmatrix} a_1 \\ a_2 \\ a_3\end{bmatrix} ⎣ ⎡a1a2a3⎦ ⎤和 [ a 1 ′ a 2 ′ a 3 ′ ] \begin{bmatrix} a_1' \\ a_2' \\ a_3'\end{bmatrix} ⎣ ⎡a1′a2′a3′⎦ ⎤则表示p点在这两组基下的坐标;
此处两边同时乘上W坐标系基的逆有:
[ a 1 a 2 a 3 ] = [ e 1 T e 1 ′ e 1 T e 2 ′ e 1 T e 3 ′ e 2 T e 1 ′ e 2 T e 2 ′ e 2 T e 3 ′ e 3 T e 1 ′ e 3 T e 2 ′ e 3 T e 3 ′ ] [ a 1 ′ a 2 ′ a 3 ′ ] = R a ′ \begin{bmatrix} a_1 \\ a_2 \\ a_3\end{bmatrix} = \begin{bmatrix} e_1^Te_1' & e_1^Te_2' & e_1^Te_3' \\ e_2^Te_1' & e_2^Te_2' & e_2^Te_3' \\ e_3^Te_1' & e_3^Te_2' & e_3^Te_3'\end{bmatrix}\begin{bmatrix} a_1' \\ a_2' \\ a_3'\end{bmatrix}=Ra' \\ ⎣ ⎡a1a2a3⎦ ⎤=⎣ ⎡e1Te1′e2Te1′e3Te1′e1Te2′e2Te2′e3Te2′e1Te3′e2Te3′e3Te3′⎦ ⎤⎣ ⎡a1′a2′a3′⎦ ⎤=Ra′ [ e 1 ′ e 2 ′ e 3 ′ ] = [ e 1 e 2 e 3 ] [ e 1 T e 1 ′ e 1 T e 2 ′ e 1 T e 3 ′ e 2 T e 1 ′ e 2 T e 2 ′ e 2 T e 3 ′ e 3 T e 1 ′ e 3 T e 2 ′ e 3 T e 3 ′ ] \begin{bmatrix} e_1' & e_2' & e_3'\end{bmatrix} = \begin{bmatrix} e_1 & e_2 & e_3\end{bmatrix}\begin{bmatrix} e_1^Te_1' & e_1^Te_2' & e_1^Te_3' \\ e_2^Te_1' & e_2^Te_2' & e_2^Te_3' \\ e_3^Te_1' & e_3^Te_2' & e_3^Te_3'\end{bmatrix} [e1′e2′e3′]=[e1e2e3]⎣ ⎡e1Te1′e2Te1′e3Te1′e1Te2′e2Te2′e3Te2′e1Te3′e2Te3′e3Te3′⎦ ⎤这里的R就是常说的旋转矩阵,重点来了!R具有的两重意思:
①表示坐标 a ′ a' a′到 a a a的变换,也就是P点在C坐标系转换到W坐标系,在代码中这个矩阵往往以 R W C R_{WC} RWC这个名称出现,可以理解为C系下坐标到W系坐标下变换的旋转矩阵;
②表示基 e e e到 e ′ e' e′的变换,和坐标刚好相反,这其实是初学矩阵论是就碰到的一个有趣的点,可以简单的理解为C上坐标到W上坐标的变换和C坐标到W坐标的变换刚好是反向的;
定义n维旋转矩阵R组成的特殊正交群SO(n)如下:
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}将3维下旋转矩阵和平移组合之后的变换矩阵T组成的特殊欧式群SE(3)定义如下:
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]
3. 各个旋转表示之间的变换
①角轴到旋转矩阵,罗德里格斯公式:
R
=
c
o
s
θ
I
+
(
1
−
c
o
s
θ
)
n
n
T
+
s
i
n
θ
n
ˆ
R = cos\theta I + (1-cos\theta)nn^T + sin\theta n\ \^{}
R=cosθI+(1−cosθ)nnT+sinθn ˆ②旋转矩阵到角轴:
t
r
(
R
)
=
c
o
s
θ
t
r
(
I
)
+
(
1
−
c
o
s
θ
)
t
r
(
n
n
T
)
+
s
i
n
θ
t
r
(
n
ˆ
)
=
3
c
o
s
θ
+
(
1
−
c
o
s
θ
)
=
1
+
2
∗
c
o
s
θ
\begin{aligned} & tr(R) = cos\theta tr(I) + (1-cos\theta)tr(nn^T) + sin\theta tr(n\ \^{}\ ) \\ & =3cos\theta + (1-cos\theta) \\&=1+2*cos\theta\\ \end{aligned} \\
tr(R)=cosθtr(I)+(1−cosθ)tr(nnT)+sinθtr(n ˆ )=3cosθ+(1−cosθ)=1+2∗cosθ那么角轴的的角度可以通过旋转矩阵的迹,方向可以通过旋转矩阵特征值为1的特征向量求取(因为轴绕着轴旋转==不旋转)
θ
=
a
r
c
c
o
s
t
r
(
R
)
−
1
2
R
n
=
n
\theta = arccos\frac{tr(R)-1}{2} \\ Rn = n
θ=arccos2tr(R)−1Rn=n③角轴转四元数
绕单位向量
[
n
x
,
n
y
,
n
z
]
[n_x, n_y, n_z]
[nx,ny,nz]旋转角度
θ
\theta
θ:
q
=
[
c
o
s
θ
2
,
n
x
s
i
n
θ
2
,
n
y
s
i
n
θ
2
,
n
z
θ
2
]
q = [cos\frac{\theta}{2}, n_xsin\frac{\theta}{2}, n_ysin\frac{\theta}{2}, n_z\frac{\theta}{2}]
q=[cos2θ,nxsin2θ,nysin2θ,nz2θ]④四元数到角轴
θ
=
2
∗
a
r
c
c
o
s
(
q
0
)
\theta = 2*arccos(q_0) \\
θ=2∗arccos(q0)
[
n
x
,
n
y
,
n
z
]
=
[
q
1
,
q
2
,
q
3
]
/
s
i
n
θ
2
[n_x, n_y, n_z] = [q_1, q_2, q_3]/sin\frac{\theta}{2}
[nx,ny,nz]=[q1,q2,q3]/sin2θ⑤旋转矩阵到四元数
旋转矩阵
R
=
m
i
j
,
i
∈
[
1
,
3
]
,
j
∈
[
1
,
3
]
R={m_{ij}}, i\in[1, 3], j\in[1,3]
R=mij,i∈[1,3],j∈[1,3]
q
0
=
t
r
(
R
)
+
1
2
,
q
1
=
m
23
−
m
32
4
q
0
,
q
2
=
m
31
−
m
13
4
q
0
,
q
3
=
m
12
−
m
21
4
q
0
q_0 = \frac{\sqrt{tr(R)+1}}{2}, q1=\frac{m_{23}-m_{32}}{4q_0}, q2=\frac{m_{31}-m_{13}}{4q_0}, q3=\frac{m_{12}-m_{21}}{4q_0}
q0=2tr(R)+1,q1=4q0m23−m32,q2=4q0m31−m13,q3=4q0m12−m21⑥四元数到旋转矩阵
四元数
q
=
q
0
+
q
1
i
+
q
2
j
+
q
3
k
=
[
s
,
v
]
T
q = q_0 + q_1i + q_2j + q_3k=[s, v]^T
q=q0+q1i+q2j+q3k=[s,v]T
R
=
v
v
T
+
s
2
I
+
2
s
v
ˆ
+
(
v
ˆ
)
2
=
[
1
−
2
q
2
2
−
2
q
3
2
2
q
1
q
2
+
2
q
0
q
3
2
q
1
q
2
−
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
2
+
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 = vv^T + s^2I + 2sv\ \^{} + (v\ \^{}\ )^2 \\ = \begin{bmatrix} 1-2q_2^2-2q_3^2 & 2q_1q_2+2q_0q_3 & 2q_1q_2-2q_0q_2 \\ 2q_1q_2-2q_0q_3 & 1-2q_1^2-2q_3^2 & 2q_2q_3 + 2q_0q_1 \\ 2q_1q_2+2q_0q_2 & 2q_2q_3-2q_0q_1 & 1-2q_1^2-2q_2^2\end{bmatrix}
R=vvT+s2I+2sv ˆ+(v ˆ )2=⎣
⎡1−2q22−2q322q1q2−2q0q32q1q2+2q0q22q1q2+2q0q31−2q12−2q322q2q3−2q0q12q1q2−2q0q22q2q3+2q0q11−2q12−2q22⎦
⎤
4. 四元数
四元数运算转换成矩阵运算
定义两个四元数下的符号+和
⊕
\oplus
⊕有:
q
+
=
[
s
−
v
T
v
s
I
+
v
ˆ
]
,
q
⊕
=
[
s
−
v
T
v
s
I
−
v
ˆ
]
q^+ = \begin{bmatrix} s & -v^T \\ v & sI+v\ \^{}\ \end{bmatrix}, q^{\oplus} = \begin{bmatrix} s & -v^T \\ v & sI-v\ \^{}\ \end{bmatrix}
q+=[sv−vTsI+v ˆ ],q⊕=[sv−vTsI−v ˆ ]对于这两个运算符有
q
1
q
2
=
q
1
+
q
2
=
q
2
⊕
q
1
q_1q_2 = q_1^+q_2 = q_2^{\oplus}q_1
q1q2=q1+q2=q2⊕q1
5. 未完待续…
版权声明:本文内容由互联网用户自发贡献,该文观点与技术仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容, 请发送邮件至 dio@foxmail.com 举报,一经查实,本站将立刻删除。