0.内容概览
1.坐标和基
\mathbf{a}=[\mathbf{e_{1}}, \mathbf{e_{2}}, \mathbf{e_{3}}]\begin{bmatrix}
a_{1} \\
a_{2} \\
a_{3}
\end{bmatrix} = a_{1}\mathbf{e_{1}} + a_{2}\mathbf{e_{2}} + a_{3}\mathbf{e_{3}}
2.标准正交基
- 两两正交
- 单位长度
- 矩阵的逆等于矩阵的转置
\begin{bmatrix}
\mathbf{e_{1}^{T}} \\
\mathbf{e_{2}^{T}} \\
\mathbf{e_{3}^{T}}
\end{bmatrix}
\begin{bmatrix}
\mathbf{e_{1}}, \mathbf{e_{2}}, \mathbf{e_{3}}
\end{bmatrix} =
\begin{bmatrix}
\mathbf{e_{1}^{T}e_{1}} && \mathbf{e_{1}^{T}e_{2}} && \mathbf{e_{1}^{T}e_{3}} \\
\mathbf{e_{2}^{T}e_{1}} && \mathbf{e_{2}^{T}e_{2}} && \mathbf{e_{2}^{T}e_{3}} \\
\mathbf{e_{3}^{T}e_{1}} && \mathbf{e_{3}^{T}e_{2}} && \mathbf{e_{3}^{T}e_{3}}
\end{bmatrix} = \begin{bmatrix}
1 && 0 && 0 \\
0 && 1 && 0 \\
0 && 0 && 1
\end{bmatrix}
3.向量运算
3.1 向量点乘
\mathbf{a \cdot b} = \mathbf{a^{T}b} = \sum_{i=0}^{2}a_{i}b_{i} = \mathbf{\left |a \right | \left | b \right | cos\left \langle a,b \right \rangle}
3.2 向量叉乘
a \times b = \begin{Vmatrix}
\mathbf{e_{1}} & \mathbf{e_{2}} & \mathbf{e_{3}} \\
a_{1} & a_{2} & a_{3} \\
b_{1} & b_{2} & b_{3}
\end{Vmatrix} = \begin{bmatrix}
a_{2}b_{3} - a_{3}b_{2} \\
a_{3}b_{1} - a_{1}b_{3} \\
a_{1}b_{2} - a_{2}b_{1}
\end{bmatrix} = \\ \begin{bmatrix}
0 && -a_{3} && a_{2} \\
a_{3} && 0 && -a_{1} \\
-a_{2} && a_{1} && 0
\end{bmatrix}\mathbf{b} \xrightarrow{\text{def}} \mathbf{a^{\wedge}b}
其中a^{\wedge}是一个反对称矩阵，就可以将外积a\times b写成矩阵与向量的乘法a^{\wedge}b，将它变成线性运算，任意向量都对应着唯一的一个反对称矩阵。
4.欧式变换
- (1) 旋转矩阵
\begin {bmatrix}
\mathbf{e_{1}, e_{2}, e_{3}}
\end {bmatrix} \begin {bmatrix}
\mathbf{a_{1}} \\
\mathbf{a_{2}} \\
\mathbf{a_{3}}
\end {bmatrix} = \begin {bmatrix}
\mathbf{e_{1}^{'}, e_{2}^{'}, e_{3}^{'}}
\end {bmatrix} \begin {bmatrix}
\mathbf{a_{1}^{'}} \\
\mathbf{a_{2}^{'}} \\
\mathbf{a_{3}^{'}}
\end {bmatrix}
\begin {bmatrix}
\mathbf{a_{1}} \\
\mathbf{a_{2}} \\
\mathbf{a_{3}}
\end {bmatrix} = \begin {bmatrix}
\mathbf{e_{1}^{T}e_{1}^{'}} && \mathbf{e_{1}^{T}e_{2}^{'}} && \mathbf{e_{1}^{T}e_{3}^{'}} \\
\mathbf{e_{2}^{T}e_{1}^{'}} && \mathbf{e_{2}^{T}e_{2}^{'}} && \mathbf{e_{2}^{T}e_{3}^{'}} \\
\mathbf{e_{3}^{T}e_{1}^{'}} && \mathbf{e_{3}^{T}e_{2}^{'}} && \mathbf{e_{3}^{T}e_{3}^{'}}
\end {bmatrix} \begin {bmatrix}
\mathbf{a_{1}^{'}} \\
\mathbf{a_{2}^{'}} \\
\mathbf{a_{3}^{'}}
\end {bmatrix} = \mathbf{Ra^{'}}
\mathbf{R}=\begin{bmatrix}
\mathbf{e_{1}^{T}} \\
\mathbf{e_{2}^{T}} \\
\mathbf{e_{3}^{T}}
\end{bmatrix}\begin{bmatrix}
\mathbf{e_{1}^{'}},
\mathbf{e_{2}^{'}},
\mathbf{e_{3}^{'}}
\end{bmatrix}
\mathbf{R^{T}R}=\begin{bmatrix}
\mathbf{e_{1}^{'T}} \\
\mathbf{e_{2}^{'T}} \\
\mathbf{e_{3}^{'T}}
\end{bmatrix}\begin{bmatrix}
\mathbf{e_{1}},
\mathbf{e_{2}},
\mathbf{e_{3}}
\end{bmatrix}\begin{bmatrix}
\mathbf{e_{1}^{T}} \\
\mathbf{e_{2}^{T}} \\
\mathbf{e_{3}^{T}}
\end{bmatrix}\begin{bmatrix}
\mathbf{e_{1}^{'}},
\mathbf{e_{2}^{'}},
\mathbf{e_{3}^{'}}
\end{bmatrix}=\mathbf{I} = \mathbf{RR^{T}}
det(\mathbf{R^{T}R})=det(\mathbf{I}) =1 \\
det(\mathbf{R^{T}})det(\mathbf{R}) = 1 \\
det(\mathbf{R})^{2} = 1 \\
det(\mathbf{R}) = 1 \\
\mathbf{R}^{-1} = \mathbf{R^{T}}
- 可以参考旋转矩阵的行列式为什么等于1?，旋转矩阵行列式为1更一般解释：旋转矩阵本质上是将一组标准正交基转换成另一组标准正交基，这两组正交基分别用矩阵I,I^{'}表示，则有：
\mathbf{RI=I^{'}} \\
\rightarrow det(\mathbf{RI}) = det(\mathbf{R})det(\mathbf{I})=det(\mathbf{I^{'}}) \\
\rightarrow det(\mathbf{R})=1
- (2) 特殊正交群：
SO(n)=\left \{ R \in \mathbb{R}^{n\times n} | \mathbf{RR^{T}=I},det(\mathbf{R}=1) \right \}
- (3) 变换矩阵
\mathbf{T}=\begin{bmatrix}
\mathbf{R} && \mathbf{t} \\
\mathbf{0} && 1
\end{bmatrix}
\mathbf{TT^{-1}} = \mathbf{I} \\
\rightarrow \begin{bmatrix}
\mathbf{R} && \mathbf{t} \\
\mathbf{0} && 1
\end{bmatrix} \mathbf{T^{-1}} = \mathbf{I} \\
\rightarrow \begin{bmatrix}
\mathbf{R} && \mathbf{t} \\
\mathbf{0} && 1
\end{bmatrix} \begin{bmatrix}
\mathbf{R}^{-1} && \mathbf{x} \\
\mathbf{0} && 1
\end{bmatrix} = \mathbf{I} \\
\rightarrow \mathbf{Rx+ t} = 0 \\
\rightarrow x = -\mathbf{R^{-1}t} \\
\mathbf{T^{-1}} = \begin{bmatrix}
\mathbf{R^{-1}} && \mathbf{-R^{-1}t} \\
\mathbf{0} && 1
\end{bmatrix} = \begin{bmatrix}
\mathbf{R^{T}} && \mathbf{-R^{T}t} \\
\mathbf{0} && 1
\end{bmatrix}
- (4) 特殊欧式群
SE(3)=\left \{
\mathbf{T} = \begin{bmatrix}
\mathbf{R} && \mathbf{t} \\
\mathbf{0} && 1
\end{bmatrix} \in \mathbb{R}^{4\times 4} |
\mathbf{R} \in \mathbf{SO(3)},
\mathbf{t} \in \mathbb{R}^{3}
\right \}
5. 旋转向量和欧拉角
任意旋转都可以使用一个旋转轴和一个旋转角来刻画，从旋转向量到旋转矩阵的转换过程由罗德里格斯公式(Rodrigues's Formula)描述：
\mathbf{R}=cos\theta\mathbf{I}+(1-cos\theta)\mathbf{nn^{T}}+sin\theta\mathbf{n^{\wedge}}
两边取迹有：
tr(\mathbf{R})=cos\theta tr(\mathbf{I}) + (1-cos\theta)tr(\mathbf{nn^{T}}) + sin\theta\mathbf{n^{\wedge}} \\
= 3cos\theta + (1- cos\theta) = 1 + 2cos\theta \\
\rightarrow \theta = arccos(\frac{tr(\mathbf{R}) - 1}{2})
旋转轴\mathbf{n}上向量在旋转后不发生变化：
\mathbf{Rn=n}
3.5 欧拉角
”偏航-俯仰-滚转“(yaw-pitch-roll)三个角度描述一个旋转。
yaw：航向角，绕Z轴旋转，向上
pitch：俯仰角，绕Y轴旋转，向左
roll：滚转角，绕X轴旋转，向前
需要注意：
(1) 欧拉角描述旋转时，需要说明旋转轴顺序，如ypr(ZYX)；
(2) 会碰到著名的万向锁问题(Gimbal Lock)；
前面轴向变换会影响后面的轴方向
6. 四元数
二维情况下，旋转可以由单位复数来描述，类似地，三维旋转可以由单元四元数描述。一个四元数\mathbf{q}拥有一个实部和三个虚部：
\mathbf{q} = q_{0} + q_{1}i+q_{2}j+q_{3}k
其中，i,j,k为四元数的三个虚部，这三个虚部满足：
\begin{cases}
i^{2}=j^{2}=k^{2}=-1 \\
ij=k,ji=-k \\
jk=i,kj=-i \\
ki=j,ik=-j
\end{cases}
将i,j,k看成三个坐标轴，那么它们乘法和复数一样，相互之间乘法和外积一样，有时，人们也用一个标量和一个向量表示四元数：
\mathbf{q}=[s,\mathbf{v}]^{T},s=q_{0}\in \mathbb{R},\mathbf{v}=[q_{1}, q_{2}, q_{3}]^{T}\in\mathbb{R}^{3}
四元数运算：
\mathbf{q_{a}\pm q_{b}} = [s_{a} \pm s_{b}, \mathbf{v_{a} \pm v_{b}}] \\
\mathbf{q_{a}q_{b}} = (s_{a}+x_{a}i+y_{a}j+z_{a}k)(s_{b}+x_{b}i+y_{b}j+z_{b}k) \\
= 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 \\
= [s_{a}s_{b} - \mathbf{v_{a}^{T}v_{b}}, s_{a}\mathbf{v_{b}} + s_{b}\mathbf{v_{a}} + \mathbf{v_{a} \times v_{b}}]^{T} \\
\left \| \mathbf{q_{a}} \right \| = \sqrt{s_{a}^{2}+x_{a}^{2} + y_{a}^{2}+ z_{a}^2} \\
\left \| \mathbf{q_{a}q_{b}} \right \| = \left \| \mathbf{q_{a}} \right \|\left \| \mathbf{q_{b}} \right \| \\
\mathbf{q_{a}^{*}} = [s_{a} - x_{a}i - y_{a}j - z_{a}k] = [s_{a}, -\mathbf{v_{a}}]^{T} \\
\mathbf{q_{a}^{*}q_{a}}=\mathbf{q_{a}q_{a}^{*}} = [s_{a}^{2} + \mathbf{v^{T}v}, \mathbf{0}]^{T} = [\left \| \mathbf{q_{a}} \right \|^{2}, 0] \\
\mathbf{q^{-1}} = \mathbf{q^{*} / \left \| q \right \| ^ {2}} \\
\mathbf{qq^{-1} = q^{-1}q = 1} \\
\mathbf{q_{a}q_{b}}^{-1} = \mathbf{q_{b}^{-1}q_{a}^{-1}} \\
k\mathbf{q} = [ks, k\mathbf{v}]^{T}
注意，四元数与它逆的乘积为实四元数\mathbf{1}。当需要用四元数表示旋转时，如，对一个空间三维点\mathbf{p}=[x, y, z] \in \mathbb{R}^{3}，经过一个由单位四元数\mathbf{q}指定的旋转后得到\mathbf{p}^{'}。
\mathbf{p} = [0, x, y, z]^{T} = [0, \mathbf{v}]^{T} \\
\mathbf{p}^{'} = \mathbf{qpq^{-1}}
四元数乘法：
\mathbf{q}^{+} = \begin{bmatrix}
s && -\mathbf{v}^{T} \\
\mathbf{v} && s\mathbf{I} + \mathbf{v}^{\wedge}
\end{bmatrix}, \mathbf{q}^{\oplus } = \begin{bmatrix}
s && -\mathbf{v}^{T} \\
\mathbf{v} && s\mathbf{I} - \mathbf{v}^{\wedge}
\end{bmatrix} \\
\mathbf{q_{1}q_{2}=q_{1}^{+}q_{2}=q_{2}^{\oplus}q_{1}} \\
\mathbf{p}^{'} = \mathbf{qpq^{-1}} = \mathbf{q^{+}p^{+}q^{-1}} = \mathbf{q^{+}q^{-1\oplus}p} \\
\mathbf{q^{-1}} = \mathbf{q^{*} / \left \| q \right \| ^ {2}} \\
\left \| \mathbf{q} \right \|^{2} = 1 \\
\mathbf{q^{*}} = [s - xi - yj - zk] = [s, -\mathbf{v}]^{T} \\
\mathbf{q^{+}(q^{-1})^{\oplus}} = \begin{bmatrix}
s && -\mathbf{v}^{T} \\
\mathbf{v} && s\mathbf{I} + \mathbf{v}^{\wedge}
\end{bmatrix}\begin{bmatrix}
s && \mathbf{v}^{T} \\
-\mathbf{v} && s\mathbf{I} + \mathbf{v}^{\wedge}
\end{bmatrix} \\
= \begin{bmatrix}
1 && \mathbf{0} \\
\mathbf{0^{T}} && \mathbf{vv^{T}} + s^{2}\mathbf{I}+2s\mathbf{v^{\wedge}} + \mathbf{v^{\wedge}}^{2}
\end{bmatrix}
由于\mathbf{p^{'}，p}都是虚四元数，所以该矩阵右小角给出了从四元数到旋转矩阵的变换关系：
\mathbf{R} = \mathbf{vv^{T}} + s^{2}\mathbf{I}+2s\mathbf{v^{\wedge}} + \mathbf{v^{\wedge}}^{2} \\
tr(\mathbf{R}) = tr(\mathbf{vv^{T}} + s^{2}\mathbf{I}+2s\mathbf{v^{\wedge}} + \mathbf{v^{\wedge}}^{2}) \\
= v_{1}^{2} + v_{2}^{2} + v_{3}^{2} + 3s^{2} + tr(\mathbf{v^{\wedge}}^{2}) \\
= v_{1}^{2} + v_{2}^{2} + v_{3}^{2} + 3s^{2} - 2(v_{1}^{2} + v_{2}^{2} + v_{3}^{2}) \\
s^{2} + v_{1}^{2} + v_{2}^{2} + v_{3}^{2} = 1 \\
\rightarrow v_{1}^{2} + v_{2}^{2} + v_{3}^{2} = 1 - s^{2} \\
tr(\mathbf{R}) = 3s^{2} - (1 - s^{2}) = 4s^{2} - 1 \\
tr(\mathbf{R}) = 1 + 2cos\theta \\
\rightarrow 1 + 2cos\theta = 4s^{2} - 1 \\
\rightarrow cos\theta = 2s^{2} - 1 = cos\frac{\theta}{2}cos\frac{\theta}{2} - sin\frac{\theta}{2}sin\frac{\theta}{2}= 2(cos\frac{\theta}{2})^{2} - 1 \\
\rightarrow \theta = 2arccos(s) \\
s = cos\frac{\theta}{2} \\
s^{2} + \left \| \mathbf{v} \right \|^{2} = 1 \\
\rightarrow \left \| \mathbf{v} \right \| = sin\frac{\theta}{2} \\
\begin{cases}
\theta=2arccos(q_{0}) \\
[n_{x}, n_{y}, n_{z}]^{T} = [q_{1}, q_{2}, q_{3}]^{T} / sin\frac{\theta}{2}
\end{cases}
7.参考资料
- [1] 高翔,张涛,刘毅,等.视觉SLAM十四讲:从理论到实践[M].电子工业出版社,2019.
- [2] 旋转矩阵的行列式为什么等于1?
- [3] 无伤理解欧拉角中的“万向死锁”现象