ORB_VI思想框架

根据教程 https://blog.csdn.net/wangshuailpp/article/details/78461171 整理所得到

------教程真的常读常新-------------------------------------------

思想框架

一.IMU 部分

1. 为什么进行IMU 预积分？
   答：IMU预计分真实的意义在于其结果是一段短时间内（tk～tk+1）IMU真实旋转，位移和速度的变化量，而IMU短时间的变化量是非常准确的。使用这个准确的数据和视觉信息进行紧耦合可以提高VIO的准确性并且消除单目尺度不确定性。
2. IMU 预积分计算出的量怎么使用，起到什么作用？？
   答：论文中给定积分形式推导出的预积分存在很多未知量，没法直接使用，因此采取的办法是：将预积分中的位置变量单独从预积分公式中分离出来，在优化或者滤波算法中并不直接计算其大小，而是通过信息矩阵给这些未知变量附上权重，而信息矩阵通过协方差矩阵求得的。
   在SLAM中，一般概率模型都是建模成高斯分布，如相机的位姿都是一个高斯分布，轨迹和特征点形成了一个多元高斯分布p(x1,x2,x3,pt1…)，然后图优化或者BA就从一个概率问题变成一个最小二乘问题。要把一部分变量从多元高斯分布从分离出来，其实协方差矩阵很好分离，难分的是信息矩阵（协方差的逆），因为我们通常在SLAM里知道的是H矩阵，而不是协方差矩阵。对于H矩阵的分离，需要用到schur complement来分割。因此，从这个多元高斯分布中去掉一个变量的正确做法是把他从这个多元高斯分布中marginalize out.（来源，贺一佳讲解边缘化：https://blog.csdn.net/heyijia0327/article/details/52822104）
   #总结：预积分–》未知变量–》信息矩阵给未知变量赋权–》信息矩阵–》协方差矩阵
   所以关键就在于如何计算协方差矩阵
3. IMU 预积分计算出什么量？
   答：IMU预积分的作用是计算出
   （1）IMU数据的观测值（就是IMU预积分值），视觉中观测值是用来计算残差的（也就是误差）
   （2）残差的协方差矩阵 ，通过观测值给未知变量赋对应的权值（因为有很多观测值）
   （3）雅各比矩阵，残差的雅各比矩阵是优化中下降的方向（也就是梯度） 这三个量
4. IMU 预积分计算出的量起到什么作用？？
   答：具体使用来说，这三个量为后面的联合初始化提供初值以及后端优化提供IMU的约束关系。

一.VIO初始化部分

1. 我们初始化的原因是单目惯性紧耦合系统是一个非线性程度很高的系统，首先单目是无法获得空间中的绝对尺度，而IMU又必然存在偏置，在后面进行求解的时候还需要用到重力加速度（包括大小和方向），对于速度比较敏感的条件下，比如说无人机，又要精确的速度信息，因此，如何有效的在紧耦合系统处理之前计算出这些量，对整个紧耦合系统的鲁棒性有着重大的意义（其实这里就可以理解成相机标定一样，没有正确的标定好相机的内参，相机在进行定位的时候必然不准，而且很有可能会挂掉）。所以初始化要做的事其实说起来很简单，就是计算出绝对尺度s、陀螺仪偏置bg、加速度偏置ba、重力加速度G和每个IMU时刻的速度v，VINS中重点说明了加速度计偏置值一般都会和重力加速度耦合到一起（也就是被重力加速度给吸收掉），重力加速度的量级要远大于其加速度偏置，而且在初始化时间内加速度计偏置比较小，很难真正的计算得到，因此忽略加速度计偏置的影响，在初始化中不再计算。初始化的作用是不言而喻的，直接影响整个紧耦合系统的鲁棒性以及定位精度，并且初始化一般都需要一个比较漫长的时间，VINS大概需要十秒左右，ORB_SLAM2结合IMU的时间设定在15秒完成初始化。

2. 纯视觉初始化在第V点的A部分，首先构建一个滑动窗口，包含一组数据帧。论文中提及使用的是对极几何模型的5点法求解单目相机的相对变换，包括相对旋转和无尺度信息的位移。其实基本上每个单目模型都是使用对极几何在初始化中求解两帧的相对变换，这里需要注意的是旋转是具有尺度不变性的（其实就是单位旋转，不会有尺度信息，你仔细想想是不是？），至于为什么单目没有尺度信息，我想罗嗦一句，但其实很多学习单目视觉SLAM的人都没有真正搞明白过，单目视觉没有尺度的源头是最开始两帧间的对极几何求位姿，再具体点就是求F/E或者H的时候需要将其降参一位，F从6维降到5维，H从9维降到8维，这里所降的维度就是尺度，而且必须要降。然后三角化得到相应的3d点坐标，有这些3d点和滑动窗口中其他的帧的2d点就可以进行PNP求解获得滑动窗口中的所有的位姿和特征点3d坐标，至此，纯视觉初始化就完成了。是不是很简单？当然啊，毕竟只是简单的视觉初始化，而真正复杂的是视觉惯性联合初始化，也就是我们初始化的重点和难点。

3. 视觉惯性联合初始化

视觉惯性联合初始化在第V点的B部分，这里作者给定义的名字叫Visual-Inertia Alignment，即视觉惯性联合初始化（而在ORBSLAM2+IMU的论文里，作者定义的名称就叫IMU initialization，即IMU初始化），为什么定义这样一个名词，我觉得有两个意义，第一在进行陀螺仪偏置初始化的时候要同时使用到IMU测量的旋转和视觉测量的旋转，也就是要联合视觉和惯性的数据。第二这里求得的尺度S的值不仅仅是IMU的，还是视觉和IMU整个系统的尺度。在具体的讲解初始化每个过程的时候，有必要来个总体的概括，初始化在物理意义上的定义其实就是固有参数的标定，在数学模型上的定义其实就是公式（6）的矩阵方程求解，而公式（6）其实就是来自于最原始的PVQ积分公式，其中Q旋转对应着陀螺仪，而PV对应着加速度计。