vins-mono(2)初始化
系统初始化:
目的:
测定绝对尺度s、陀螺仪偏置bg、加速度偏置ba、重力加速度G以及每个时刻的速度v；即用于标定系统固有参数
背景:
该系统具有高度的非线性特性。纯视觉单目SLAM无法获取真实的尺度信息，并且难以确定惯性参考系；通常以第一帧作为世界坐标系来计算姿态（如ORB SLAM采用这种方法），而在速度估计方面也存在不足。这对系统的控制与规划具有重要意义。尽管IMU与视觉传感器可以相互补充，在能够提供全局尺度信息的同时有助于推断惯性参考框架；然而其中由于传感器噪声和偏差的影响，在实际应用中仍需依赖其他方法进行修正和完善。将两者的优势结合起来，在初步估算的基础上逐步优化各项参数。
实现:
基于loosely-coupled方案的方法中，在滑动窗口内首先通过SFMA算法计算出每帧的位置姿态以及各类路标点的空间坐标。随后将这些结果与IMU预积分数据进行对准。进而解算出重力方向、尺度因子、陀螺仪偏差以及各帧对应的运动速度。文中指出，在实际应用中发现：由于惯性测量单元（IMU）零偏移会对系统性能造成严重干扰，并且由于惯性测量单元零偏移的数量级相差悬殊。然而由于惯性测量单元零偏移的数量级相差悬殊.而且惯性测量单元零偏移数值非常小.因此可忽略这一项的影响
①纯视觉初始化
②视觉IMU对齐–视觉惯性联合初始化
③与世界坐标系对齐
a.纯视觉初始化
构建一个包含多帧的滑窗窗口，并对当前帧与前一帧进行匹配。当两帧之间匹配的特征点数超过30个，并且存在足够的视差（旋转量超过20像素），则利用5点算法恢复两帧之间的相对运动参数。随后通过三角剖算法确定相应的三维空间中的点坐标。将这些三维点与滑窗内的其他帧的二维特征点结合，通过PNP算法计算出所有框架的位姿信息及三维坐标。从而确定了基准框架下的各子框架位置信息。
b.视觉IMU联合初始化流程:
①若旋转外参数qbc未知,则先估计旋转外参数
②利用旋转约束估计陀螺仪bias
③利用平移约束估计重力方向,速度,以及尺度初始值
④对重力向量进行进一步优化
本研究旨在计算世界坐标系与初始相机坐标系之间的旋转矩阵qwc，并实现轨迹的校准以使其与世界坐标系一致。
视觉和IMU之间的几何约束为(这是对齐的约束):
其中s为尺度因子,上面有一横代表非米制单位轨迹.
b②.陀螺仪bias校正
目标函数:visual给出的相邻帧之间的旋转=IMU预积分旋转值
外部链接中的图片无法正常存储，请确保将图片保存后重新上传至该链接：...
b③.估计重力方向,速度,以及尺度初始值
待估计变量是:
残差可被视为连续的两个帧之间通过预积分计算得到的增量与预测值之间的差异
论文中提出求解最小二乘问题的方法,但在程序实现中则采用了Cholesky分解技术来解决该问题,并且假设系统中没有噪声干扰.
b④.重力向量优化
上一步计算所得重力加速度包含了加速度计偏差的影响。因此需要对其进行优化处理。在步骤③中得到的结果g未施加模长约束（固定值为9.81），可能属于随机值；然而在实际情况下地球引力场强度已知。因此在此阶段对重力矢量的方向性进行约束是合理的。采用球面坐标系来进行参数化
;
我的理解: w₁和w₂被视为待优化的变量,是从第③步中获得的g向量在两个不同方向上的持续修正结果,b₁和b₂作为归一化的单位向量,经过每一次迭代计算后,仅保留更新的方向用于后续迭代过程以确保其模长维持在9.81单位长度的同时遵循与步骤③相同的优化方程
c.将相机坐标系对齐世界坐标系
在c0坐标系中对所有变量进行旋转变换至w坐标系，并调整相机位置并调整各特征点的尺度至米制单位。
基于全局坐标系下的重力加速度与相机坐标系下的重力加速度之间的关系计算得到w和c之间的旋转矩阵