1.点与坐标系
2D情况：用两个坐标＋旋转角表达
3D情况：
1.点和向量
点就是空间中的基本元素，没有长度，没有体积，把两个点连接起来，就构成了向量。向量可以看成从某点指向另一点的一个箭头。
向量和坐标这两个概念不能混淆，一个向量是空间中的一样东西，比如a。这里的a并不需要和若干个实数相关联。只有当我们指定这个三维空间中的某个坐标系时，才可以谈论该向量在此坐标系下的坐标，也就是找到若干个实数对应这个向量。
三维空间中某个点的坐标点可以用**
R
3
\mathbb{R}^3
R3来描述。找到该空间中的一组基
(
e
1
,
e
2
,
e
3
)
(e_1,e_2,e_3)
(e1,e2,e3),那么任意向量a在这组基下就有一个坐标：
a
=
[
e
1
e
2
e
3
]
[
a
1
a
2
a
3
]
=
a
1
e
1
+
a
2
e
2
+
a
3
e
3
a= \left[ \begin{matrix} e_1 & e_2 & e_3 \\ \end{matrix} \right] \left[ \begin{matrix} a_1 \\ a_2\\ a_3\\ \end{matrix} \right]=a_1e_1+a_2e_2+a_3e_3
a=[e1e2e3]⎣
⎡a1a2a3⎦
⎤=a1e1+a2e2+a3e3
这里
(
a
1
,
a
2
,
a
3
)
T
(a_1,a_2,a_3)^T
(a1,a2,a3)T称为a**在此基下的坐标。坐标的具体取值，一是和向量本身有关，而是和坐标系（基）的选取有关。
对于** a , b ∈ R 3 a,b \in \mathbb{R}^3 a,b∈R3**,通常内积和外积为：
- 内积：
a ⋅ b = a T b = ∑ i = 1 3 = ∣ a ∣ ∣ b ∣ c o s ⟨ a , b ⟩ a·b=a^Tb=\sum_{i=1}^{3}=|a||b|cos\langle a , b \rangle a⋅b=aTb=i=1∑3=∣a∣∣b∣cos⟨a,b⟩
- 外积：
a × b = [ e 1 e 2 e 3 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 0 ] b = a Λ b a×b=\left[ \begin{matrix} e_1 & e_2 & e_3 \\ a_1&a_2&a_3\\ b_1&b_2&b_3 \end{matrix} \right]=\left[ \begin{matrix} a_2b_3-a_3b_2 \\ a_3b_1-a_1b_3\\ a_1b_2-a_2b_1 \end{matrix} \right]=\left[ \begin{matrix} 0 & -a_3 & a_2 \\ a_3&0&-a_1\\ -a_2&a_1&0 \end{matrix} \right]b=a^{\Lambda} b a×b=⎣ ⎡e1a1b1e2a2b2e3a3b3⎦ ⎤=⎣ ⎡a2b3−a3b2a3b1−a1b3a1b2−a2b1⎦ ⎤=⎣ ⎡0a3−a2−a30a1a2−a10⎦ ⎤b=aΛb
外积的结果是一个向量，他的方向垂直与这两个向量，大小为 ∣ a ∣ ∣ b ∣ s i n ⟨ a , b ⟩ |a||b|sin\langle a , b \rangle ∣a∣∣b∣sin⟨a,b⟩,是两个向量张成的四边形的有向面积。对于外积运算，我们引入 Λ \Lambda Λ，把a写成一个矩阵。这是一个反对称矩阵，这样就把外积写成矩阵与向量的乘法。把他变成线性运算。
2.坐标系的欧系变换
在slam中
- 固定的世界坐标系和移动机器人的坐标系
- 不同的传感器坐标系
两个坐标系之间的运动由一个旋转加上一个平移组成，这种运动称为刚体运动。相机就是一个刚体运动。
欧式变换由旋转和平移组成。我们首先考虑旋转。设某个单位正交基
(
e
1
,
e
2
,
e
3
)
(e_1,e_2,e_3)
(e1,e2,e3)经过一次旋转变成了
(
e
1
′
,
e
2
′
,
e
3
′
)
(e_1',e_2',e_3')
(e1′,e2′,e3′)。那么对于同一个向量a（向量并没有随着坐标系的旋转而发生运动），他在两个坐标系下的坐标为
[
a
1
,
a
2
,
a
3
]
T
[a_1,a_2,a_3]^T
[a1,a2,a3]T和
[
a
1
′
,
a
2
′
,
a
3
′
]
T
[a_1',a_2',a_3']^T
[a1′,a2′,a3′]T。
[
e
1
e
2
e
3
]
[
a
1
a
2
a
3
]
=
[
e
1
′
e
2
′
e
3
′
]
[
a
1
′
a
2
′
a
3
′
]
\left[ \begin{matrix} e_1 & e_2 & e_3 \\ \end{matrix} \right] \left[ \begin{matrix} a_1 \\ a_2\\ a_3\\ \end{matrix} \right]=\left[ \begin{matrix} e_1' & e_2' & e_3' \\ \end{matrix} \right] \left[ \begin{matrix} a_1' \\ a_2'\\ a_3'\\ \end{matrix} \right]
[e1e2e3]⎣
⎡a1a2a3⎦
⎤=[e1′e2′e3′]⎣
⎡a1′a2′a3′⎦
⎤
为了表述两个坐标系之间的关系，我们对上述等式的左右两边同时左乘
[
e
1
T
e
2
T
e
3
T
]
T
\left[ \begin{matrix}e_1^T & e_2^T & e_3^T \\\end{matrix} \right]^T
[e1Te2Te3T]T，那么左边的系数就变成了单位矩阵所以：
[
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
’
\left[ \begin{matrix} a_1 \\ a_2\\ a_3\\ \end{matrix} \right]=\left[ \begin{matrix} 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^T e_1'&e_3^Te_2'&e_3^Te_3' \end{matrix} \right] \left[ \begin{matrix} a_1' \\ a_2'\\ a_3'\\ \end{matrix} \right]=Ra’
⎣
⎡a1a2a3⎦
⎤=⎣
⎡e1Te1′e2Te1′e3Te1′e1Te2′e2Te2′e3Te2′e1Te3′e2Te3′e3Te3′⎦
⎤⎣
⎡a1′a2′a3′⎦
⎤=Ra’
我们把中间的矩阵拿出来，定义成一个矩阵R，这个矩阵有两组基之间的内积组成，刻画了旋转前后同一个向量的坐标系变换关系。只旋转是一样的，这个矩阵就是一样的。
旋转矩阵的性质：他是一个行列式为1的正交矩阵。反之，行列式为1的正交矩阵也是一个旋转矩阵。所以，可以将n维旋转矩阵的集合定义如下：
S
O
(
n
)
=
{
R
∈
R
n
×
n
∣
R
R
T
=
I
.
d
e
t
(
R
)
=
1
}
SO(n)=\lbrace{R\in \mathbb{R}^{n×n}|RR^T=I.det(R)=1} \rbrace
SO(n)={R∈Rn×n∣RRT=I.det(R)=1}
S
O
(
n
)
SO(n)
SO(n)是一个特殊正交群。
在欧式变换中，除了旋转还有平移。世界坐标系中的向量a，经过一次旋转R和一个平移t后，得到了a‘
a
′
=
R
a
+
t
a'=Ra+t
a′=Ra+t
3.变换矩阵和齐次坐标
我们在三维向量的末尾添加1，将其变成四维向量，称为齐次坐标。对于这个四维向量，我们可以把旋转和平移写在一个矩阵里，使得整个关系变成线性关系。该式中，矩阵T称为变换矩阵。
[
a
′
1
]
=
[
R
T
0
1
]
[
a
1
]
=
T
[
a
1
]
\left[ \begin{matrix} a'\\ 1 \end{matrix} \right]=\left[ \begin{matrix} R&T\\ 0&1 \end{matrix} \right]\left[ \begin{matrix} a\\ 1 \end{matrix} \right]=T\left[ \begin{matrix} a\\ 1 \end{matrix} \right]
[a′1]=[R0T1][a1]=T[a1]
关于变换矩阵T，它具有比较特别的结构，左上角为旋转矩阵，右侧为平移向量，左下角为0向量，右下角为1。这种矩阵又称为特殊欧式群。
S
E
(
3
)
=
{
T
=
[
R
T
0
1
]
∈
R
4
×
4
∣
R
∈
S
O
(
3
)
.
t
∈
R
3
}
SE(3)=\lbrace{T=\left[ \begin{matrix} R&T\\ 0&1 \end{matrix} \right]\in \mathbb{R}^{4×4}|R \in SO(3).t\in \mathbb{R}^{3}} \rbrace
SE(3)={T=[R0T1]∈R4×4∣R∈SO(3).t∈R3}
2.旋转向量和欧拉角
- SO(3)的旋转矩阵有9个量，但一次旋转只有三个自由度。因此这种表达方式是冗余的。同理，变换矩阵用16个量表达了6自由度的变换。
- 旋转矩阵自身带有约束，他必须是个正交矩阵，且行列式为1,。变换矩阵也是如此，当想估计或优化一个旋转矩阵或变换矩阵时，这些约束会使求解变得更加困难。
1.旋转向量
任意旋转都可以用一个旋转轴和一个旋转角来刻画。我们使用一个向量，其方向与旋转轴一致，而长度等于旋转角。这种向量称为旋转向量。
用某个R表示旋转。旋转向量θn，旋转轴为单位长度向量n，角度为θ。从旋转向量到旋转矩阵的转换过程由罗德里格斯公式表明：
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^{\Lambda}
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 θ θ = a r c o s t r ( R ) − 1 2 tr(R)=cos\theta tr(I)+(1+cos\theta)tr(nn)^T+sin\theta tr(n^{\Lambda})\\ =3cos\theta +(1-cos\theta)\\ 1+2cos\theta\\ \\ \\ \theta=arcos\frac{tr(R)-1}{2} tr(R)=cosθtr(I)+(1+cosθ)tr(nn)T+sinθtr(nΛ)=3cosθ+(1−cosθ)1+2cosθθ=arcos2tr(R)−1
关于转轴n，旋转轴上的向量在旋转后不发生改变，说明：
R
n
=
n
Rn=n
Rn=n
因此转轴n是矩阵R特征值1对应的特征向量。求解此方程，在归一化，就得到了旋转轴。
2.欧拉角
欧拉角提供了一种非常直观的方式来描述旋转——使用三个分离的转角，把一次旋转分解成三次绕不同轴的旋转。
- 绕物体的Z轴旋转，得到偏航角yaw
- 绕旋转之后的Y轴旋转，得到俯仰角pitch
- 绕旋转之后的X轴旋转，得到滚转角roll
万向锁问题
欧拉角一个重大的问题就是会碰到著名的万向锁问题。俯仰角为±90°时，第一次旋转和第三次旋转将会使用同一个轴，使得系统丢失了一个自由度。这被称为奇异性问题。理论证明只要想用三个实数来表达三维旋转，都会不可避免地碰到奇异性问题。所以欧拉角不适合插值和迭代，往往只用于人机交互中。
3.四元数
旋转矩阵用9个量描述3自由度的旋转，具有冗余性；欧拉角和旋转向量是紧凑的，但是具有奇异性。事实上，我们找不到不带奇异性的三维向量的描述方式。这有点类似用两个坐标表示地球表面（如经纬度），将必定存在奇异性:维度为±90°时经度无意义。
回忆以前学习过的复数，我们用复数集 C \mathbb{C} C表示复平面上的向量，而复数的乘法则表示复平面上的旋转，例如，乘上复数i相当于逆时针把一个复向量旋转90°。类似地，在表达三维空间旋转时，也有一种类似于复数的代数：四元数。他既是紧凑的，也没有奇异性。但是不够直观，运算稍复杂些。
1336
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
