序号	内容
1	【数理知识】自由度 degree of freedom 及自由度的计算方法
2	【数理知识】刚体 rigid body 及刚体的运动
3	【数理知识】刚体基本运动，平动，转动
4	【数理知识】向量数乘，内积，外积，matlab代码实现
5	【数理知识】协方差，随机变量的的协方差，随机变量分别是单个数字和向量时的协方差
6	【数理知识】旋转矩阵的推导过程，基于向量的旋转来实现，同时解决欧式变换的非线性局限

1. 点和向量的表示

点（Point）：是空间中的基本元素，没有长度，没有体积。

假设在三维空间中有一点 $a$，其坐标可以表示为 ${\left[\begin{array}{c}x,y,z\end{array}\right]}^{\text{T}}$。

而把两个点连接起来就构成了向量。

---

向量（Vector）：可以看成某点指向另一点的一个箭头，请不要把向量与它的坐标这两个概念混淆，一个向量是空间当中的一样东西，比如说向量 $\vec{a}$。这里 $\vec{a}$ 并不是和若干个实数相关联。只有当我们指定这个三维空间中的某个坐标系时，才可以谈论该向量在此坐标系下的坐标，也就是找到若干个实数对应这个向量。

向量 $\vec{a}$ 在线性空间基 $\left[\begin{array}{c}{\vec{e}}_1,{\vec{e}}_2,{\vec{e}}_3\end{array}\right]$ 下的坐标为 ${\left[\begin{array}{c}x,y,z\end{array}\right]}^{\text{T}}$，那么存在以下公式

$$\begin{array}{r}\vec{a}=\left[\begin{array}{ccc}{\vec{e}}_1& {\vec{e}}_2& {\vec{e}}_3\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\end{array}\right]={\vec{e}}_{1}x+{\vec{e}}_{2}y+{\vec{e}}_{3}z\\ =x{\vec{e}}_1+y{\vec{e}}_2+z{\vec{e}}_3\end{array}$$

---

2. 基于向量推导旋转矩阵 $R$

对于同一个向量 $\vec{a}$，设在单位正交基 $\left[\begin{array}{c}{\vec{e}}_1,{\vec{e}}_2,{\vec{e}}_3\end{array}\right]$ 下的坐标为 ${\left[\begin{array}{c}x,y,z\end{array}\right]}^{\text{T}}$，
经过一次旋转，单位正交基变成了 $\left[\begin{array}{c}{\vec{e}}_1^{\prime },{\vec{e}}_2^{\prime },{\vec{e}}_3^{\prime }\end{array}\right]$，而向量 $\vec{a}$ 在新的正交基下的坐标为 ${\left[\begin{array}{c}{x}^{\prime },y^{\prime },z^{\prime }\end{array}\right]}^{\text{T}}$。

由于该向量并没有随着坐标系的旋转而发生运动，因此根据坐标的定义，我们有

$$\begin{array}{rl}\vec{a}& =\left[\begin{array}{ccc}{\vec{e}}_1& {\vec{e}}_2& {\vec{e}}_3\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\end{array}\right]\\ & =\left[\begin{array}{ccc}{\vec{e}}_1^{\prime }& {\vec{e}}_2^{\prime }& {\vec{e}}_3^{\prime }\end{array}\right]\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ z^{\prime }\end{array}\right]\end{array}$$

---

假如在等式两边同时左乘矩阵 $\left[\begin{array}{c}{\vec{e}}_1^{\text{T}}\\ {\vec{e}}_2^{\text{T}}\\ {\vec{e}}_3^{\text{T}}\end{array}\right]$，有

$$\begin{array}{rl}\left[\begin{array}{c}{\vec{e}}_1^{\text{T}}\\ {\vec{e}}_2^{\text{T}}\\ {\vec{e}}_3^{\text{T}}\end{array}\right]\left[\begin{array}{ccc}{\vec{e}}_1& {\vec{e}}_2& {\vec{e}}_3\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\end{array}\right]& =\left[\begin{array}{c}{\vec{e}}_1^{\text{T}}\\ {\vec{e}}_2^{\text{T}}\\ {\vec{e}}_3^{\text{T}}\end{array}\right]\left[\begin{array}{ccc}{\vec{e}}_1^{\prime }& {\vec{e}}_2^{\prime }& {\vec{e}}_3^{\prime }\end{array}\right]\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ z^{\prime }\end{array}\right]\\ \left[\begin{array}{ccc}{\vec{e}}_1^{\text{T}}{\vec{e}}_1& {\vec{e}}_1^{\text{T}}{\vec{e}}_2& {\vec{e}}_1^{\text{T}}{\vec{e}}_3\\ {\vec{e}}_2^{\text{T}}{\vec{e}}_1& {\vec{e}}_2^{\text{T}}{\vec{e}}_2& {\vec{e}}_2^{\text{T}}{\vec{e}}_3\\ {\vec{e}}_3^{\text{T}}{\vec{e}}_1& {\vec{e}}_3^{\text{T}}{\vec{e}}_2& {\vec{e}}_3^{\text{T}}{\vec{e}}_3\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\end{array}\right]& =\left[\begin{array}{ccc}{\vec{e}}_1^{\text{T}}{\vec{e}}_1^{\prime }& {\vec{e}}_1^{\text{T}}{\vec{e}}_2^{\prime }& {\vec{e}}_1^{\text{T}}{\vec{e}}_3^{\prime }\\ {\vec{e}}_2^{\text{T}}{\vec{e}}_1^{\prime }& {\vec{e}}_2^{\text{T}}{\vec{e}}_2^{\prime }& {\vec{e}}_2^{\text{T}}{\vec{e}}_3^{\prime }\\ {\vec{e}}_3^{\text{T}}{\vec{e}}_1^{\prime }& {\vec{e}}_3^{\text{T}}{\vec{e}}_2^{\prime }& {\vec{e}}_3^{\text{T}}{\vec{e}}_3^{\prime }\end{array}\right]\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ z^{\prime }\end{array}\right]\end{array}$$

显而易见有

$$\begin{array}{rlr}{\vec{e}}_1^{\text{T}}{\vec{e}}_1=1,& {\vec{e}}_1^{\text{T}}{\vec{e}}_2=0,& {\vec{e}}_1^{\text{T}}{\vec{e}}_3=0,\\ {\vec{e}}_2^{\text{T}}{\vec{e}}_1=0,& {\vec{e}}_2^{\text{T}}{\vec{e}}_2=1,& {\vec{e}}_2^{\text{T}}{\vec{e}}_3=0,\\ {\vec{e}}_3^{\text{T}}{\vec{e}}_1=0,& {\vec{e}}_3^{\text{T}}{\vec{e}}_2=0,& {\vec{e}}_3^{\text{T}}{\vec{e}}_3=1,\end{array}$$

因此，可进一步化简为

$$\begin{array}{rl}\left[\begin{array}{ccc}{\vec{e}}_1^{\text{T}}{\vec{e}}_1& {\vec{e}}_1^{\text{T}}{\vec{e}}_2& {\vec{e}}_1^{\text{T}}{\vec{e}}_3\\ {\vec{e}}_2^{\text{T}}{\vec{e}}_1& {\vec{e}}_2^{\text{T}}{\vec{e}}_2& {\vec{e}}_2^{\text{T}}{\vec{e}}_3\\ {\vec{e}}_3^{\text{T}}{\vec{e}}_1& {\vec{e}}_3^{\text{T}}{\vec{e}}_2& {\vec{e}}_3^{\text{T}}{\vec{e}}_3\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\end{array}\right]& =\left[\begin{array}{ccc}{\vec{e}}_1^{\text{T}}{\vec{e}}_1^{\prime }& {\vec{e}}_1^{\text{T}}{\vec{e}}_2^{\prime }& {\vec{e}}_1^{\text{T}}{\vec{e}}_3^{\prime }\\ {\vec{e}}_2^{\text{T}}{\vec{e}}_1^{\prime }& {\vec{e}}_2^{\text{T}}{\vec{e}}_2^{\prime }& {\vec{e}}_2^{\text{T}}{\vec{e}}_3^{\prime }\\ {\vec{e}}_3^{\text{T}}{\vec{e}}_1^{\prime }& {\vec{e}}_3^{\text{T}}{\vec{e}}_2^{\prime }& {\vec{e}}_3^{\text{T}}{\vec{e}}_3^{\prime }\end{array}\right]\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ z^{\prime }\end{array}\right]\\ \left[\begin{array}{c}x\\ y\\ z\end{array}\right]& =\left[\begin{array}{ccc}{\vec{e}}_1^{\text{T}}{\vec{e}}_1^{\prime }& {\vec{e}}_1^{\text{T}}{\vec{e}}_2^{\prime }& {\vec{e}}_1^{\text{T}}{\vec{e}}_3^{\prime }\\ {\vec{e}}_2^{\text{T}}{\vec{e}}_1^{\prime }& {\vec{e}}_2^{\text{T}}{\vec{e}}_2^{\prime }& {\vec{e}}_2^{\text{T}}{\vec{e}}_3^{\prime }\\ {\vec{e}}_3^{\text{T}}{\vec{e}}_1^{\prime }& {\vec{e}}_3^{\text{T}}{\vec{e}}_2^{\prime }& {\vec{e}}_3^{\text{T}}{\vec{e}}_3^{\prime }\end{array}\right]\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ z^{\prime }\end{array}\right]\\ & \stackrel{def}{=}R\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ z^{\prime }\end{array}\right]\end{array}$$

这个 $R$ 描述了向量的旋转，称为旋转矩阵（Rotation Matrix）。

---

上述推导过程是基于向量的代数关系来的，还有使用三维坐标轴旋转角度的推导过程，可以参考文章：第3章-数理知识基础 -＞ 坐标转换。

---

旋转矩阵 $R$ 有性质：

• 旋转矩阵 $R$ 是一个行列式为 1 的正交矩阵。反之，行列式为 1 的正交矩阵也是一个旋转矩阵。所有 $n$ 维旋转矩阵构成特殊正交群 $SO(n)$。 S O ( n ) = { R ∈ R n × n ∣ R R T = I , det ( R ) = I } SO(n) = \{ R \in \R^{n \times n} | RR^\text{T} = I, \text{det}(R) = I \} SO(n)={R∈Rn×n∣RRT=I,det(R)=I}

• 旋转矩阵 $R$ 是正交矩阵（Orthogonal Matrix，正交矩阵即逆为自身转置的矩阵），旋转矩阵的逆 $R^{-1}$ 描述了一个相反的旋转

---

3. 向量的欧式变换

在计算机视觉和机器人技术中，欧式变换（Euclidean/Rigid Translation，或称为刚性变换）是一种非常常见的变换。

欧式变换包括两个部分：

1. 旋转：它可以通过旋转矩阵来表示。
2. 平移：它可以通过平移向量来表示。

不清楚的可以参考文章：【数理知识】刚体基本运动，平动，转动。

假设有如下旋转和平移过程：世界坐标系中存在一个向量 ${\vec{a}}_1$，经过一次旋转 $R_{1t2}$ 和一次平移 $t_{1t2}$ 后，得到了向量 ${\vec{a}}_2$，那么有如下关系
$${\vec{a}}_2=R_{1t2}{\vec{a}}_1+t_{1t2}$$

---

4. 欧式变换具有非线性的局限性

欧式变换是刚性变换，它包括了旋转和平移。虽然平移是线性的，但旋转却是非线性的。

对于旋转，我们可以使用旋转矩阵、四元数或旋转轴和旋转角来描述。不论是哪种描述方式，旋转的性质都是非线性的。

假如我们进行两次变换：$R_{1t2},t_{1t2}$ 和 $R_{2t3},t_{2t3}$

$$\begin{array}{rl}{\vec{a}}_2& =R_{1t2}{\vec{a}}_1+t_{1t2}\\ {\vec{a}}_3& =R_{2t3}{\vec{a}}_2+t_{2t3}\\ & =R_{2t3}(R_{1t2}{\vec{a}}_1+t_{1t2})+t_{2t3}\end{array}$$

上述变换里有两个旋转矩阵，但是两次旋转的组合 $R_{2t3}\times R_{1t2}$不等于两个旋转矩阵的简单线性组合，比如说加法。

同理，如果我们使用四元数来描述旋转，四元数的乘法也是非线性的。

---

5. 使用变换矩阵 $T$ 解决非线性

针对欧式变换的非线性问题，我们可以通过齐次坐标（Homogeneous Coordinates）的方法进行解决。

齐次坐标通过添加一个额外的坐标来扩展标准坐标。可以用单个矩阵表示旋转和平移，使得变换更简单。

---

具体解决办法是通过向三维向量的末尾添加 1，将其变为一个四维向量，称为齐次坐标。针对此欧式变换

$${\vec{a}}_2=R_{1t2}{\vec{a}}_1+t_{1t2}$$

将旋转矩阵和平移向量写进同一个变换矩阵（Transformation Matrix）中，同时扩张维度后为

$$\begin{array}{rl}\left[\begin{array}{c}{\vec{a}}_2\\ 1\end{array}\right]& =\left[\begin{array}{cc}{R}_{1t2}& t_{1t2}\\ 0_{1\times 3}& 1\end{array}\right]\left[\begin{array}{c}{\vec{a}}_1\\ 1\end{array}\right]\\ & =T\left[\begin{array}{c}{\vec{a}}_1\\ 1\end{array}\right]\end{array}$$

$T$ 即为变换矩阵。

---

齐次坐标的意义就在于将欧式变换表示为线性关系。

因此欧式变换变成齐次坐标后有

$$\begin{array}{rl}{\vec{a}}_2& =R_{1t2}{\vec{a}}_1+t_{1t2}\\ \left[\begin{array}{c}{\vec{a}}_2\\ 1\end{array}\right]& =T\left[\begin{array}{c}{\vec{a}}_1\\ 1\end{array}\right]\end{array}$$

---

变换矩阵 $T$ 的性质：

• 变换矩阵 $T$ 构成特殊欧式群 $SE$：
  S E ( 3 ) = { T = [ R t 0 1 × 3 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } SE(3) = \{ T = \left[\begin{matrix} R & t \\ 0_{1\times3} & 1 \\ \end{matrix}\right] \in \R^{4\times4} | R \in SO(3), t \in \R^3 \} SE(3)={T=[R01×3​​t1​]∈R4×4∣R∈SO(3),t∈R3}

• 变换矩阵的逆表示一个反向的变换：
  $$T^{-1}=\left[\begin{array}{cc}{R}^{\text{T}}& -R^{\text{T}}t\\ 0_{1\times 3}& 1\end{array}\right]$$

---

Ref

1. 《视觉SLAM十四讲从理论到实践》第3讲 三维空间刚体运动 - 知乎
2. SLAM学习笔记（一）第三讲三维空间刚体运动