从VINS-MONO到VIO学习（一）

写在前面：本文是基于深蓝学院从零开始手写VIO课程，结合本人对于VIO的理解书写而成的，因为刚刚入门SLAM，难免有一些理解错误以及不太透彻的地方，希望大家不吝赐教。

什么是VIO？

VIO(Visual-Inertial Odometry)是一种视觉惯性融合的里程计。一般来说，传感器由相机和IMU构成。相机采集信息的频率比较低，以图片的形式记录数据，主要用于特征点的提取和匹配以恢复出运动轨迹。但是相机有一些局限性，比如在光照条件较差，或者纹理不太清晰的地方可能会跟踪丢失，并且对于单目相机而言，它没办法得到拍摄物体真实的尺度，这也是我们要引入惯性单元的原因。

IMU传感器简介

IMU传感器一般分为六轴和九轴两大类，其中SLAM系统一般都是使用典型的6轴IMU。六轴IMU由两个部分构成：加速度计以及陀螺仪。其中加速度计是用于测量加速度，陀螺仪是用于测量角速度。通过IMU的信息我们可以解算出被测物体的位姿。但是IMU也是有局限性的，IMU采样频率很高，基本上频率在100-1000Hz之间，并且一般SLAM系统所使用的IMU为MEMS器件，精度不是很高，其示数受到自身温度，零偏，震动等因素的影响，并且在进行位姿结算的时候IMU的读数是要进行积分运算的，这就导致了误差的累计，如果不及时加以补偿，最终很可能导致结果出现很大错误。

松耦合和紧耦合

松耦合：IMU只提供自身所获取的读数，并不作为优化的内容进行优化。
紧耦合：IMU所测量的结果也作为参数加入到后端的优化之中。

三种表示旋转的方法

1. 旋转矩阵

旋转矩阵（定义为R），用于描述同一个向量，从一个坐标系到另一个坐标系的旋转关系。旋转矩阵是行列式为1的正交矩阵，当然所有行列式为1的正交矩阵也可以被成为旋转矩阵。所有旋转矩阵所构成的群称为特殊正交群[SO(3)]。
请注意：特殊正交群属于李群。

特殊正交群定义： S O ( n ) = { R ∈ R n × n ∣ R R T = I , d e t ( R ) = 1 } SO(n)=\{R \in\Bbb R^{n×n} | RR^{T}=I,det(R)=1\} SO(n)={R∈Rn×n∣RRT=I,det(R)=1}

2. 欧拉角

欧拉角是一种描述旋转的方法，它是由三个角度构成，它表示的是沿着对应的坐标轴旋转的角度。也就是说欧拉角是使用了三次沿着坐标轴的旋转来表示物体的一次旋转。
请注意：欧拉角每一次沿轴的旋转都是在上一次旋转的基础之上的。并且欧拉角旋转的顺序也会影响旋转的结果，比如XYZ欧拉角就是指先沿着X轴旋转然后沿着Y轴旋转最后沿着Z轴旋转。

1. 绕物体的Z轴旋转，得到偏航角yaw;
2. 绕旋转之后的Y轴旋转，得到俯仰角pitch;
3. 绕旋转之后的X轴旋转，得到滚转角roll；
   

对于欧拉角来说，它是使用三维向量表示三维旋转的，但是需要注意的问题是，欧拉角会面临着万向锁的问题，所谓的万向锁就是在旋转的时候可能会损失自由度，导致奇异。

3. 四元数

四元数相较与旋转矩阵更加紧凑，它只使用四个量来表示旋转，并且不存在万向锁的问题。单位四元数是一种比较好的表示3D旋转的方法。四元数由一个实部和三个虚部构成，并且拥有如下性质。

一些量对于时间的导数

角速度：$$\begin{array}{r}\omega =\underset{\Delta t\to 0}{\lim }\frac{\delta \theta }{\Delta t}\end{array}$$
四元数：$$\begin{array}{rl}\dot{q}& =\underset{\Delta t\to 0}{\lim }\frac{q(t+\Delta t)-q(t)}{\Delta t}\\ & =\underset{\Delta t\to 0}{\lim }\frac{q\otimes \Delta q-q}{\Delta t}\\ & =q\otimes \left[\begin{array}{c}0\\ \frac{1}{2\omega }\end{array}\right]\end{array}$$
旋转矩阵：$$\begin{array}{r}\dot{R}=R{\omega }^{\wedge }\end{array}$$
上式被称为泊松公式，$\wedge$ 被称为反对称矩阵算子，也可表示为：${\omega }_{\times }$或$[\omega {]}_{\times }$

一些常用的公式

$$\begin{array}{r}当\phi 很小的时候exp({\phi }^{\wedge })\approx I+{\phi }^{\wedge }\end{array}$$
$$\begin{array}{r}ln(Rexp({\Phi }^{\wedge }){)}^{\vee }=ln(R{)}^{\vee }+J_r^{-1}\Phi \end{array}$$
$$\begin{array}{r}{R}^{T}exp({\Phi }^{\wedge })R=exp((R^T\Phi {)}^{\wedge })\end{array}$$