视觉SLAM十四讲学习笔记（三）

第三讲 三维空间刚体运动

3.1 旋转矩阵

外积的反对称矩阵写法

$$a\times b=\left[\begin{array}{ccccc}0& & -a_3& & a_2\\ a_3& & 0& & -a_1\\ -a_2& & a_1& & 0\end{array}\right]b$$
记作a^b,其中 ^是反对称符号。

向量的内外积即使在不谈论它们的坐标时也可以计算，例如内积可以通过长度和夹角来计算。

坐标系之间的欧式变换

两个坐标系之间的运动由旋转加上平移组成，称为刚体运动
对于两个坐标系下的两组坐标表示的同一个向量，有
$$\left[\begin{array}{ccccc}{e}_1& & e_2& & e_3\end{array}\right]\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[\begin{array}{ccccc}{e}_1^{\prime }& & e_2^{\prime }& & e_3^{\prime }\end{array}\right]\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]$$
左右两边同时左乘${\left[\begin{array}{ccccc}{e}_1& & e_2& & e_3\end{array}\right]}^T$得
$$\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[\begin{array}{ccccc}{e}_1^T{e}_1^{\prime }& & e_1^T{e}_2^{\prime }& & e_1^T{e}_3^{\prime }\\ e_2^T{e}_1^{\prime }& & e_2^T{e}_2^{\prime }& & e_2^T{e}_3^{\prime }\\ e_3^T{e}_1^{\prime }& & e_3^T{e}_2^{\prime }& & e_3^T{e}_3^{\prime }\end{array}\right]\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]=R{a}^{\prime }$$
R的各分量是两个坐标系基的内积，由于基向量的长度为一，因此每个分量其实是基向量夹角的余弦值。

旋转矩阵的性质

行列式为1的正交矩阵，可以将n维旋转矩阵的集合定义如下：
$$SO(n)=\{R\in {\mathbb{R}}^{n\times n}|R{R}^T=I,\det (R)=1\}$$
SO(n)称为特殊正交群
进而有$a^{\prime }=R^{-1}a=R^{T}a$

考虑平移

$$a^{\prime }=Ra+t$$
考虑坐标系1,2，a在两个坐标系下的坐标分别为$a_1$，$a_2$：
$$a_1=R_{12}{a}_2+t_{12}$$
其中，$R_{12}$表示把坐标系2的向量变换到坐标系1中。
$t_{12}$对应的是坐标系1原点指向坐标系2原点的向量，在坐标系1下取的坐标。
注：$t_{12}\ne t_{21}$

变换矩阵与齐次坐标

存在的问题

变换关系不是线性关系
$$b=R_{1}a+t_{1}c=R_{2}b+t_2$$
则有$c=R_2(R_{1}a+t_1)+t_2$

齐次坐标和变换矩阵

$$\left[\begin{array}{c}{a}^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{ccc}R& & t\\ 0^T& & 1\end{array}\right]\left[\begin{array}{c}a\\ 1\end{array}\right]=T\left[\begin{array}{c}a\\ 1\end{array}\right]$$
T称为变换矩阵

特殊欧式群

$$SE(3)=\{T=\left[\begin{array}{ccc}R& & t\\ 0^T& & 1\end{array}\right]\in R^{4\times 4}|R\in SO(3),t\in R^3\}$$
$$T^{-1}=\left[\begin{array}{ccc}{R}^T& & -R^{T}t\\ 0^T& & 1\end{array}\right]$$
用$T_{12}$表示从坐标系1到坐标系2的变换。

3.2 实践：Eigen

要点

1. 声明矩阵

Matrix<数据类型，行，列>;
//内置类型
//Vector3d实质上是Eigen::Matrix
Vector3d v_3d;
//Matrix3d实质上是Eigen::Matrix
Matrix3d matrix_33;
//动态大小的矩阵
Matrix matrix_dynamic;
//更简单的
MatrixXd matrix_x;

2. 初始化操作

//初始化为零
Matrix3d::Zero();
//输入数据
//按行输入
matrix_23 << 1,2,3,4,5,6

3. 输出

//直接输出
cout << "..." << matrix_23 << endl;
//用()访问矩阵中的元素
for (int i = 0; i < 2 ; i++) {
	for (int j = 0; j < 3 ; j++) cout << matrix_23(i,j) << "\t";
	cout << endl;
}

4. 运算

//矩阵和向量相乘
//注意数据类型匹配，需要用.cast<数据类型>()进行显示转换
Matrix result = matrix_23.cast() * V_3d;
//随机数矩阵
matrix_33 = Matrix3d::Random();
/*
转置：.transpose()
各元素和：.sum()
迹：.trace()
逆：.inverse()
行列式：.determinant()
*/

//特征值
//实对称矩阵可以保证对角化成功
SelfAdjointEigenSolver eigen_solver(matrix_33.transpose()*matrix_33);
//特征值
eigen_solver.eigenvalues()
//特征向量
eigen_solver.eigenvectors()

//解方程
//1. 直接求逆
Matrix x = matrix_NN.inverse() * V_Nd;
//2. 矩阵分解
Matrix x = matrix_NN.colPivHouseHolderQr().solve(v_Nd);
//3. 对于正定矩阵，还可以用cholesky分解来解方程
Matrix x = matrix_NN.ldlt().solve(v_Nd)

5. 添加头文件

include_directories("/usr/include/eigen3")

由于Eigen库只有头文件，所以不需要再用target_link_libraries语句将程序链接到库上。

3.3 旋转向量和欧拉角

旋转向量

存在的问题

1. SO(3)用9个量表达了3个自由度
   SE(3)用16个量表达了6个自由度
   存在冗余
2. 旋转矩阵自身存在约束，会使得估计或优化变得更困难

解决方案

用一种方式紧凑地描述旋转和平移

1. 任意旋转都可以用一个旋转轴和一个旋转角来刻画
   可以使用一个向量，其方向与旋转轴一致，而长度等于旋转角。这种向量称为旋转向量。
2. 对于变换矩阵，使用一个旋转向量和一个平移向量即可表达一次变换。这时的变量维数正好是六维。

罗德里格斯公式

$$R=cos\theta I+(1-cos\theta )n{n}^T+sin\theta n^{\wedge }$$

$\theta$和n的推导

$$\begin{array}{rl}tr(R)& =cos\theta tr(I)+(1-cos\theta )tr(n{n}^T)+sin\theta tr(n^{\cdot })\\ & =3cos\theta +(1-cos\theta )\\ & =1+2cos\theta \end{array}$$
进而有
$$\theta =\arccos \frac{tr(R)-1}{2}$$
关于转轴n，旋转轴上的向量在旋转后不发生改变，即
$$Rn=n$$
因此，n是矩阵R特征值1对应的特征向量对应的单位向量。
即有两个转换公式
$$\{\begin{array}{l}\theta =\arccos \frac{tr(R)-1}{2}\\ Rn=n\end{array}$$

欧拉角

存在的问题

旋转矩阵和旋转向量不够直观。

解决方案

使用3个分离的转角，把一个旋转分解成3次绕不同轴的旋转。
假设一个刚体的前方为X轴，右侧为Y轴，上方为Z轴。ZYX转角相当于把任意旋转分解成以下3个轴上的转角：

1. 绕物体的Z轴旋转，得到偏航角yaw
2. 绕旋转之后的Y轴旋转，得到俯仰角pitch
3. 绕旋转之后的X轴旋转，得到滚转角roll
   此时，可以使用$[r,p,y{]}^T$这样一个三维的向量描述任意旋转。

万向锁问题

俯仰角为$\pm 90^{\circ }$时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失了一个自由度。

也就是说，第三次旋转可以被包含进第一次旋转中，从而丢失了一个自由度。

从理论角度：
当$\theta =\frac{\pi }{2}$时，公式变为：
可以看到，改变$\varphi$和$\psi$的效果相同，而旋转轴始终是z轴。
从旋转表示方式角度：
一次旋转应当由4个变量表示：3个角度和1个旋转顺序
若只用3个变量表示，在特定情况下会丢失旋转顺序。

这使得欧拉角不适用于插值和迭代，往往只用于人机交互中。

3.4 四元数

定义

一个四元数拥有一个实部和三个虚部
$$q=q_0+q_{1}i+q_{2}j+q_{3}k$$
这三个虚部满足以下关系式：
$$\{\begin{array}{l}{i}^2=j^2=k^2=-1\\ ij=k,ji=-k\\ jk=i,kj=-i\\ ki=j,ik=-j\end{array}$$
可以用一个标量和一个向量来表达四元数
$$q=[s,v{]}^T,s=q_0\in R,v=[q_1,q_2,q_3]\in R^3$$

四元数的运算

现有两个四元数$q_a,q_b$，它们的向量表示为$[s_a,v_a{]}^T,[s_b,v_b{]}^T$，或者原始四元数表示为
$$q_a=s_a+x_{a}i+y_{a}j+z_{a}k,q_b=s_b+x_{b}i+y_{b}j+z_{b}k$$

1. 加法和减法
2. 乘法
   $$\begin{array}{rl}{q}_a{q}_b=& 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\end{array}$$
   也可写为
   $$q_a{q}_b=[s_a{s}_b-v_a^T{v}_b,s_a{v}_b+s_b{v}_a+v_a\times v_b{]}^T$$
   四元数乘法不可交换
3. 模长
   四元数的模长定义为
   $$\Vert q_a\Vert =\sqrt{s_a^2+x_a^2+y_a^2+z_a^2}$$
   可以验证，两个四元数乘积的模即模的乘积。这使得单位四元数相乘后仍是单位四元数。
4. 共轭
   四元数的共轭是把虚部取成相反数：
   $$q^{\ast }q=q{q}^{\ast }=[s_a^2+v^{T}v,0{]}^T$$
5. 逆
   $$q^{-1}=q^{\ast }/{\Vert q_a\Vert }^2$$
   进而有
   $$q{q}^{-1}=q^{-1}q=1$$
   若q为单位四元数，则有$q^{-1}=q^{\ast }$
   乘积的逆具有和矩阵的逆相似的性质：
   $$(q_a{q}_b{)}^{-1}=q_b^{-1}{q}_a^{-1}$$
6. 数乘

用四元数表示旋转

• 把空间三维点用一个虚四元数来描述：
  $$p=[0,x,y,z{]}^T=[0,v{]}^T$$
  旋转后的p’可表示为这样的乘积：
  $$p^{\prime }=qp{q}^{-1}$$
  p’的虚部即为旋转之后点的坐标

四元数到其他旋转表示的转换

设$q=[s,v]$，定义
$$q^{+}=\left[\begin{array}{ccc}s& & -v^T\\ v& & sI+v^{\cdot }\end{array}\right],q^{\circ }=\left[\begin{array}{ccc}s& & -v^T\\ v& & sI-v^{\cdot }\end{array}\right]$$
易得$q_1{q}_2=q_1^{+}{q}_2=q_2^{\circ }{q}_1$
进而有
$$p^{\prime }=qp{q}^{-1}=q^{+}{p}^{+}{q}^{-1}=q^{+}{q^{-1}}^{\circ }p$$
即有
$$R=q^{+}{q^{-1}}^{\circ }=\left[\begin{array}{ccc}1& & 0\\ 0^T& & v{v}^T+s^{2}I+2s{v}^{\cdot }+(v^{\cdot }{)}^2\end{array}\right]$$
由于p是虚四元数，有
$$R=v{v}^T+s^{2}I+2s{v}^{\cdot }+(v^{\cdot }{)}^2$$
进而有
$$tr(R)=4s^2-1$$
带入上文结论可得
$$\theta =\arccos (2s^2-1)$$
即
$$\theta =2\arccos s$$
在四元数旋转公式中，用q的虚部代替p，可得该向量在旋转中不动，即构成旋转轴，则有
$$\{\begin{array}{l}\theta =2\arccos q_0\\ [n_x,n_y,n_z{]}^T=[q_1,q_2,q_3]/sin(\frac{\theta }{2})\end{array}$$

相似、仿射、射影变换

1. 相似变换

多了一个缩放自由度
$$T_S=\left[\begin{array}{ccc}sR& & t\\ 0^T& & 1\end{array}\right]三维相似变换的集合又叫相似变换群，记作Sim(3)$$

2. 仿射变换

$$T_S=\left[\begin{array}{ccc}A& & t\\ 0^T& & 1\end{array}\right]$$
仿射变换只要求A是一个可逆矩阵，而不必是正交矩阵

3. 射影变换

$$T_S=\left[\begin{array}{ccc}A& & t\\ a^T& & v\end{array}\right]$$

3.6 实践：Eigen几何模块

见书上代码

3.7 可视化演示

显示运动轨迹

显示相机的位姿