视觉SLAM14讲笔记-第3讲-三维空间刚体运动

空间向量之间的运算包括：
数乘、加法、减法、内积、外积。
内积：可以描述向量间的投影关系，结果是一个标量。
$$a\cdot b=\sum _{i=1}^3{a}_i{b}_i=\nmid a\nmid \nmid b\nmid cos⟨a,b⟩$$

外积：外积的方向垂直于这两个向量，大小为两个向量张成的四边形的有向面积，结果还是一个向量。
$$a\times b=a^{\wedge }\cdot b$$
$a^{\wedge }$ 为 $a$ 的反对称矩阵。

旋转向量的引出：由两个向量的外积引出，在右手法则下，我们用右手的4个指头从a转向b，大拇指朝向就是旋转向量的方向，事实上也是$a\times b$的方向。

旋转矩阵是正交矩阵。即满足 $A^T\cdot A=E$ 。即旋转矩阵的逆为自身转置的矩阵。且是一个行列式为1的正交矩阵。注意：旋转矩阵不一定是对称矩阵。

齐次坐标系的引出：因为空间运动不止有旋转，还有平移，例如
$$a^{\prime }=Ra+t$$
转换为矩阵形式
$$\left[\begin{array}{c}{a}^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]\left[\begin{array}{c}a\\ 1\end{array}\right]=T\left[\begin{array}{c}a\\ 1\end{array}\right]$$
T为变换矩阵，$\left[\begin{array}{c}a\\ 1\end{array}\right]$为向量a的齐次形式。即在一个三维向量的末尾添加1,将其变成了四维向量，称为齐次坐标。

关于变换矩阵T的逆，也有公式，如下：
$$T^{-1}=\left[\begin{array}{cc}{R}^T& -R^{T}t\\ 0& 1\end{array}\right]$$

群的引出：上面我们看到的旋转矩阵R是特殊正交群（Special Orthogonal Group）。变换矩阵T称为特殊欧式群（Special Euclidean Group）。

旋转向量的第2次引出：由于旋转矩阵由（9个元素）和变换矩阵由（16个元素）组成，存在冗余，且自身带有约束（旋转矩阵必须是个正交矩阵），不利于导数的计算（后面使用非线性优化的方式，要对求解元素求导）。而旋转向量可以用3个元素描述，平移向量可以用3个元素描述。

罗德里格斯公式的引出：指导旋转向量到旋转矩阵的转换。
$$R=\cos \theta \cdot I+(1-cos\theta )n{n}^T+sin\theta n^{\wedge }$$
反之也可以得到旋转矩阵到旋转向量的转换。

欧拉角的引出：为了直观的表述绕X、Y、Z角度变化了多少，引出了欧拉角，根据绕不同轴的前后顺序，可以有不同类型的欧拉角，一般最常见的是rpy角（它是按ZYX的旋转顺序，rpy对应yaw-pitch-roll，即偏航-俯仰-滚转）。
但是有奇异性，所以在slam中很少用欧拉角表达姿态。

四元数的引出：正是由于欧拉角的奇异性，所以才有了四元数的引出。缺点是不够直观。一个四元数拥有1个实部和3个虚部。
$$q=q_0+q_{1}i+q_{2}j+q_{3}k$$
Eigen中我们习惯使用下面对应表示
$$q=w+xi+yj+zk$$

假设某个旋转是绕单位向量$n=\left[\begin{array}{ccc}{n}_x,& n_y,& n_z\end{array}\right]$进行了角度为$\theta$的旋转
旋转向量和四元数的转换：
$$q={\left[\begin{array}{cccc}cos\frac{\theta }{2},& n_{x}sin\frac{\theta }{2},& n_{y}sin\frac{\theta }{2},& n_{z}sin\frac{\theta }{2}\end{array}\right]}^T$$
反之，四元数也可以转换为旋转向量
$$\{\begin{array}{l}\theta =2arccos{q}_0\\ {\left[\begin{array}{ccc}{n}_x,& n_y,& n_z\end{array}\right]}^T={\left[\begin{array}{ccc}{q}_1,& q_2,& q_3\end{array}\right]}^T/sin\frac{\theta }{2}\end{array}$$

四元数之间的运算不过多复述，感觉目前用的地方并不多。

这里再提一下四元数与旋转矩阵之间的转换
假设四元数为$q=q_0+q_{1}i+q_{2}j+q_{3}k$，则
R = [ 1 − 2 q 2 2 − 2 q 3 2 2 q 1 q 2 − 2 q 0 q 3 2 q 1 q 3 + 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 3 − 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 = \begin{bmatrix} 1-2q_2^2-2q_3^2&2q_1q_2-2q_0q_3&2q_1q_3+2q_0q_2\\ 2q_1q_2+2q_0q_3&1-2q_1^2-2q_3^2&2q_2q_3-2q_0q_1 \\2q_1q_3-2q_0q_2&2q_2q_3+2q_0q_1&1-2q_1^2-2q_2^2 \end{bmatrix} R= ​1−2q22​−2q32​2q1​q2​+2q0​q3​2q1​q3​−2q0​q2​​2q1​q2​−2q0​q3​1−2q12​−2q32​2q2​q3​+2q0​q1​​2q1​q3​+2q0​q2​2q2​q3​−2q0​q1​1−2q12​−2q22​​ ​

由旋转矩阵到四元数
$q_0=\frac{\sqrt{tr(R)+1}}{2},$ $q_1=\frac{m_{23}-m_{32}}{4q_0},$

$q_2=\frac{m_{31}-m_{13}}{4q_0},$ $q_2=\frac{m_{12}-m_{21}}{4q_0},$

3D空间的几种变换：
欧式变换：也称为刚体变换，长度、夹角，体积不随着变换而改变。
相似变换：增加了缩放系数，体积比不变。
仿射变换：进一步抽象，变换后仍然保持平行性，同时增加了缩放系数。
射影变换：再进一步抽象，从真实世界到相机照片的变换是一个射影变换。
随着变换的进一步抽象，自由度的变化也在变化，如下：
$$\begin{array}{clc}\text{变换名称}& \text{矩阵形式}& \text{自由度}\\ 欧式变换& \left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]& 6\\ 相似变换& \left[\begin{array}{cc}sR& t\\ 0& 1\end{array}\right]& 7\\ 仿射变换& \left[\begin{array}{cc}A& t\\ 0& 1\end{array}\right]& 12\\ 射影变换& \left[\begin{array}{cc}A& t\\ a^T& 1\end{array}\right]& 15\end{array}$$

习题1:验证旋转矩阵是正交矩阵
参考网址：https://zhuanlan.zhihu.com/p/419854977