估计器初始化简述
单目紧耦合VIO是一个高度非线性的系统，需要在一开始就进行准确的初始化估计。通过将IMU预积分与纯视觉结构进行松耦合对齐，我们得到了必要的初始值。
理解：这里初始化是指通过之前imu预积分得到的数值和视觉结构得到的数值进行对齐整理，综合运算得到的是我们的初始值。
具体流程如下：
- 检查最新帧和之前所有帧之间的特征对应关系：如果能在滑动窗口中找到稳定的特征跟踪（超过30个被跟踪特征）和足够的视差（超过20个旋转补偿像素），就使用五点法恢复这两帧之间的相对运动；如果没有稳定的特征跟踪和足够的视差，就把当前帧留在滑动窗口中并等待下一帧进来。
- 若运动恢复成功，就对两帧中共同观测到的所有特征进行三角化，尺度任意。
- 有了第二步的三角化结果，就可以对窗口中的其他帧进行PnP求解（3D-2D）。
- 最后使用全局BA对所有的观测到的特征的重投影误差，优化变量是相机位姿和路标点位置。
总的来说就是
1.SFM纯视觉估计滑动窗所有帧的位姿和3D路标点逆深度；
2.SFM与IMU预积分松耦合，对齐求解初始化参数（bg,ba,s,v...）；
估计器初始化预备知识
五点算法与八点算法
在slam 和sfm领域，恢复相机位姿和3D点的坐标是其重要的任务，描述一个场景的3D点在不同相机的图像坐标之间的关系被称为对极几何关系。对极几何关系描述的矩阵通常有基本矩阵(fundamental matrix)、本质矩阵（essential matrix）、单应矩阵(homography matrix)。
基本矩阵的求解算法有7点法、8点法；本质矩阵的求解算法有5点法、8点法；单应矩阵的求解算法为DLT算法。
同时关于本质矩阵：
PnP求解
给定n个3D参考点{c1,C2,..,cn}到摄像机图像上2D投影点{u1, u,... un}的匹配点对；，已知3D点在世界坐标系下的坐标，2D点在图像坐标系下的坐标，已知摄像机的内参数K。需要求世界坐标系与摄像机坐标系之间的位姿变换{R|t}，这个时候就需要PnP来做这些事情
可以用来相机位姿跟踪，物体位姿跟踪，AR/VR, 机器人操作, SLAM中位姿初值求解.... .
BA（光束平差法）
光束： 源于 bundle of light， 指的是三维空间中的点投影到像平面上的光束，而重投影误差正是利用这些光束来构建的，因此强调光束也正是描述其优化模型的来源！
平差：感觉像测绘里面的内容！由于测量仪器的精度不完善和人为因素及外界条件的影响，测量误差总是不可避免的。为此，观测值的个数往往要多于确定未知量所必须观测的个数。有了多余观测，势必在观测结果之间产生矛盾，测量平差的目的就在于消除这些矛盾而求得观测量的最可靠结果并评定测量成果的精度，测量平差采用的原理就是“最小二乘法”
BA到底是干嘛的？ 用一句话来描述BA那就是，BA的本质是一个优化模型，其目的是最小化重投影误差。本质是一个优化模型应该很容易理解，那么什么是重投影误差呢？
第一次投影：指的就是相机在拍照的时候三维空间点投影到图像上，然后我们利用这些图像对一些特征点进行三角定位
第二次投影：利用计算得到的三维点的坐标和我们计算得到的相机矩阵 进行第二次投影
现在我们知道什么是重投影了，那重投影误差到底是什么样的误差呢？
这个误差是指的真实三维空间点在图像平面上的投影（测量值）和重投影（预测值） 的差值，噪声等原因的存在，使得两者不会相等
也就是这个差值不可能恰好为0，而优化的目的就是：找出最优的相机参数及三维空间点的坐标，使得将这些差值最小化，这就涉及到最优化问题了
初始化过程解释
外参中的旋转
用机器人手眼标定的方法，计算出外参中的旋转。
qb(k+1)_b(k) 是陀螺仪预积分得到的, qc(k+1)_c是用8点法对前后帧对应的特征点进行计算得到的。
SFM
先在关键帧窗口里面，找到第l帧，第l帧与最后一帧有足够的像素位移，并且能用8点法算出旋转和位移。以l帧的姿态为世界坐标系。先从l帧开始与最后一帧进行三角定位，再用pnp估计出(I帧)下一帧的位姿，下一帧再与最后一帧三角定位得出更多的三维点。重复到倒数第二帧。从l帧开始往第一帧，逐渐帧pnp，再与第l帧进行三角定位得到更多的三维点。每帧pnp时的位姿初值都用上一个关键帧的的位姿。剩下的那些还没有被三角定位的特征点，通过它被观察到的第一帧和最后一帧进行三角定位。
【PNP的应用场景有两个，一种是世界坐标系3D点及其在相机坐标系投影的2D坐标点，求解出来的位姿是相机坐标系相对于世界坐标系的；另一是上一帧的3D坐标在下一帧上的2D投影，求出的位姿是下一帧相对于上一帧的】
基于滑动窗口的纯视觉单目初始化
初始化的原因是单目惯性紧耦合系统是一个非线性程度很高的系统，首先单目是无法获得空间中的绝对尺度，而IMU又必然存在偏置，在后面进行求解的时候还需要用到重力加速度（包括大小和方向），对于速度比较敏感的条件下，比如说无人机，又要精确的速度信息，因此，如何有效的在紧耦合系统处理之前计算出这些量，对整个紧耦合系统的鲁棒性有着重大的意义（其实这里就可以理解成相机标定一样，没有正确的标定好相机的内参，相机在进行定位的时候必然不准，而且很有可能会挂掉）。所以初始化要做的事 就是计算出绝对尺度s、陀螺仪偏置bg、加速度偏置ba、重力加速度G和每个IMU时刻的速度v，VINS中重点说明了加速度计偏置值一般都会和重力加速度耦合到一起（也就是被重力加速度给吸收掉），重力加速度的量级要远大于其加速度偏置，而且在初始化时间内加速度计偏置比较小，很难真正的计算得到，因此忽略加速度计偏置的影响，在初始化中不再计算。初始化的作用是不言而喻的，直接影响整个紧耦合系统的鲁棒性以及定位精度，并且初始化一般都需要一个比较漫长的时间，VINS大概需要十秒左右，ORB_SLAM2结合IMU的时间设定在15秒完成初始化 。
纯视觉初始化首先构建一个滑动窗口，包含一组数据帧。论文中提及使用的是对极几何模型的5点法求解单目相机的相对变换，包括相对旋转和无尺度信息的位移。其实基本上每个单目模型都是使用对极几何在初始化中求解两帧的相对变换，这里需要注意的是旋转是具有尺度不变性的。然后三角化得到相应的3d点坐标，有这些3d点和滑动窗口中其他的帧的2d点就可以进行PNP求解获得滑动窗口中的所有的位姿和特征点3d坐标，至此，纯视觉初始化就完成了，真正复杂的是视觉惯性联合初始化，也就是我们初始化的重点和难点。
视觉惯性联合初始化
视觉惯性联合初始化 定义的名字叫Visual-Inertia Alignment，即视觉惯性联合初始化（而在ORBSLAM2+IMU的论文里，作者定义的名称就叫IMU initialization，即IMU初始化），为什么定义这样一个名词，我觉得有两个意义，第一在进行陀螺仪偏置初始化的时候要同时使用到IMU测量的旋转和视觉测量的旋转，也就是要联合视觉和惯性的数据。第二这里求得的尺度S的值不仅仅是IMU的，还是视觉和IMU整个系统的尺度。在具体的讲解初始化每个过程的时候，有必要来个总体的概括，初始化在物理意义上的定义其实就是固有参数的标定，在数学模型上的定义其实就是 矩阵方程求解，就是来自于最原始的PVQ积分公式，其中Q旋转对应着陀螺仪，而PV对应着加速度计 。
参考链接
- VINS-Mono论文翻译：https://blog.csdn.net/yys2324826380/article/details/105046864/ 闭关修炼到走火入魔
- 光束平差法 https://blog.csdn.net/hltt3838/article/details/109642247? 他人是一面镜子，保持谦虚的态度
- 流程概述 https://zhuanlan.zhihu.com/p/138632367 yikang
- 5点法、8点法 https://zhuanlan.zhihu.com/p/374399877 3D视觉工坊
- 本质矩阵 https://blog.csdn.net/qq_22424571/article/details/82669525 AI小丸子
- 知识点总结 https://blog.csdn.net/hltt3838/article/details/106075292? 他人是一面镜子，保持谦虚的态度