VINS-Mono

初始化

框架

缺点

（1）VINS-Mono的前段是采用的提取关键点然后采用光流法追踪，因此对于弱纹理，关键点少的环境鲁棒性和精度差；

（2）同样还是因为前段的问题，因为没有提取特征描述子，而是使用光流法进行的追踪匹配，一旦画面模糊或者图像丢失，相机就会丢，而且没有重定位模块；

（3）在恒速运动下，会使得IMU有一个自由度不客观，因此会发生漂移。

ORB SLAM的Local Map、VINS的滑窗

Local Map优化的是当前帧和其由共视关系的关键帧的位姿， 以及当前帧和共视帧看到的地图点，没有涉及到边缘化保留先验信息的操作，而VINS-mono的滑窗相对较为复杂，需要考虑通过边缘化进行先验信息的保留等操作。

逐次逼近式去畸变

liftProjective() 代替 OpenCV中undistortPoints()
原理：图像中，靠外侧的像素点的畸变程度大于靠内侧的像素点
方法：假设，想求得畸变坐标A‘的去畸变坐标A。先视A‘点为畸变点B的去畸变坐标，那么△A’B<△A’A，点B可以通过将A‘的坐标代入畸变模型中求得。再在线段AA’上取点B’，使得A’B=A’B’。这样反复迭代逼近8次，最终得到点A在相机归一化坐标系下的坐标

给后端提供的特征点信息

像素坐标->特征点提取算法
去畸变后归一化坐标->特征点去畸变算法
特征点id ->光流追踪算法
特征点速度

光流追踪

30hz输入图像
如果图像太暗或者太亮，提特征点比较难，要均衡化预处理
光流追踪cv::calcOpticalFlowPyrLK()，追踪时候会剔除外点
判断追踪到的特征点是否在图像有效范围内
双指针遍历所有特征点（包含内外点），只保留内点（被成功追踪到的特征点），从而降低空间复杂度
对需要发送到后端的10hz图像帧：1.对极约束F去除外点 2.特征点分布均匀化 3.提取新的特征点保证特征点数目衡为150个
所有特征点去畸变，计算每个特征点速度（用来后续时间戳标定）

对极约束F去除外点 rejectWithF()

因为比较耗时，不适合每帧都做，所以在给后端发送前进行本操作。调用cv::findFundamentalMat()找出外点，再用双指针遍历剔除外点实现瘦身

u = fx * x + width/2：假定了一个虚拟相机，固定fx=460、size。好处是在计算本质矩阵时，参数F_THRESHOLD和实际相机无关（因为可容忍的像素误差个数和相机的焦距有关了），不用因为相机的不同设置不同的参数阈值了；而且之前函数去畸变得到的是归一化相机坐标系下的坐标，所以坐标就和相机模型无关了（当然如果设置为真实值的话性能更好）

特征点均匀化

setMask()，比orb-slam里四叉树简单

对图像中所有被追踪到的特征点排序（权重为特征点已经被追踪到的次数：越多越稳定）
按照权重由小到大遍历所有特征点，视当前特征点为有效点，但是再这个特征点的圆形区域内的所有比其权重小的点都视为冗余点（使用值都为255的mask）

预积分

滤波里不需要预积分：只要维持当前的位姿估计，之前的那些位姿估计和他没有关系

• 原因：
  在优化问题中，需要不停的优化调整多个帧的位姿，所以原因有：

1. 第k时刻到第k+1时刻的PVQ（位置、速度、旋转）的积分公式是在世界坐标系下的，所以当k时刻之前帧的位姿被优化调整后，如果没有使用预积分，就要重新对所有帧重新算一遍，所以考虑将两个时刻间的pvq从世界坐标系转到k时刻的相机坐标系下，使得k到 k+1间的积分值抽离出来。则得到一个预积分量，这个量只和IMU在时间段内的测量值有关。因此就不用再在每次优化后重新计算。
2. 得到一个由IMU提供的每两帧间的约束，保证无论相邻两帧的位姿怎么被优化调整，都要满足IMU构成的帧间约束。

• 求法：
  左乘旋转，将参考坐标系从世界坐标系调整到第k个关键帧时刻的本体坐标系bk。得到k与k+1媜间的相对运动量，这样即使在优化过程中频繁的调整PVQ也不会对积分项产生影响，从而避免重复积分

系统初始化

松耦合

1. 用 SFM求解滑动窗口内所有帧的位姿（以第一帧作为参考坐标系）和所有路标点的 3D 位置
2. 将 SFM 的结果与 IMU 预积分的值进行对齐
   • 对陀螺仪偏置校准
   • 求解每一帧对应的速度、求解重力向量方向、恢复单目相机的尺度因子
   • 对重力方向进行精调

初始化时不校正bias_a

因为重力是初始化过程中待求的量，而加速度计偏置与重力耦合，而且系统的加速度相对于重力加速度很小，所以加速度计偏置在初始化过程中很难观测，因此初始化过程中先不考虑加速度计偏置的校正

误差卡尔曼滤波

误差传递

y = Ax
Σy = AΣxA⊤
要推导预积分量的协方差，我们需要知道 imu 噪声和预积分量之间的线性递推关系，这样才能将方差一直传递下去
然而像四元素是非线性的，所以考虑 一阶泰勒展开线性化、或 基于误差随时间变化的递推方程，

• 连续
  1. 对t时刻的误差项求导
  2. 借助一阶泰勒展开 由k时刻的err 求k+1时刻的err，得到下一时刻的IMU测量误差和上一时刻成线性关系
  3. 有了线性关系，就可以根据当前时刻的值，预测下一时刻的均值和协方差
  4. 计算误差项的雅克比的迭代公式 （为了提供给后面bias的J）
     
• 离散
  1. PVQ 增量误差在离散形式下的矩阵形式：
     
  2. 离散形式下的PVQ增量误差的J 和协方差 传递
     

预积分中零偏线性化建模

（因为变化非常小，所以可以用一阶泰勒近似）
因为零偏也是待优化变量，每次优化都会变化，而预积分公式中包含bias，则每次bias变化后都需要对预积分重新计算，计算量非常大，所以考虑用一阶泰勒近似：=原先的预积分结果+J*△X

• 预积分量关于零偏的J
  Jk+1 = F * Jk

IMU方差

• 方差标定
  Allen方差https://blog.csdn.net/weixin_38258767/article/details/111514038

• 预积分

• 预积分方差传递
  IMU在每个时刻的高斯白噪声是不一样的，没法通过标定得到，所以要求积分形成的预计分量的方差就要借助线性传递的方式，在此就需要知道imu噪声和预计分量之间的线性递推关系，然而，像四元素这样的状态量的递推关系是非线性的，所以需要将其线性化（建立一种基于误差随时间变化的递推方程），对当前时刻的位移、速度、bias(acc、gray)微分得到线性传递方程

##　
基于误差卡尔曼滤波得置信度
卡尔曼滤波后得到的最优值
误差卡尔曼滤波：送入的是误差

为何使用：９ｍｉｎ？？
旋转的误差状态通常是很小的值，所以参数的数目是和旋转的自由度想等，因为误差卡尔曼中，我们通常使用旋转向量表示旋转误差，因为四元素是过参数的表示形式：四个量来表示三个自由度的旋转（因为模为１），

VIO初始化

纯视觉的SFM三维重建 + 视觉惯性对齐，将SFM和预积分的结果对齐，实现视觉惯性联合初始化

• 为什么要初始化

1. 基于优化方式的VIO非常依赖初始值，建模完优化问题后，使用LM方法找到局部最小值，但是想要保证这个局部最小值为全局最小值，需要初值距离最小值足够接近，防止陷入局部最小值。
2. 离得越近，迭代次数越少，因为每次迭代都要重新计算 J、H△x=g，开销还是有点大的。
3. 松耦合，这个初始值也会给后面的滑窗优化提供一个比较好的
   

• 初始化哪些变量
  地图点：可以帮助我们更好的优化位姿，因此需要优化地图点做一个大的BA问题
  xc_b：外参
  ，所以不在初始化时候给出
• 下面两个不标定
  ba：加速度零偏 因为有重力分量在里面，所以初始化时很难与重力区分出来；同时，即使忽略ba，也不会显著影响初始化结果，所以放在非线性优化中求解
  pc_b：平移外参 在不借助先验的情况下是很难估计的，强行估计得化会很偏，给优化估计带来麻烦，所以不在初始化时估计。可以先用尺子测量得到初始值，因为并不需要非常准确的外参，参数值会在后面非线性优化中会持续对其进行优化

4.2 后端优化预处理

关键帧：如果两个图像中的像素误差超过阈值，则加入关键帧

信息矩阵

• 计算：

  信息矩阵 = 协方差矩阵的逆

  直接求逆会很麻烦且计算量大，可以借助概率方式简化求解，即通过计算联合高斯分布就可以得到：

  非对角元素：=0：两个元素相互独立，但是两个元素不相关则不一定为0 <0：正相关 >0：负相关

• 去除某个变量

  • 协方差矩阵：直接去掉与其相关的元素即可；信息矩阵：需要借助 边缘化 + 舒尔补

Slide Window

• 为何要滑动窗口：
  随着系统中增加新的相机姿态、看到新的环境特征，最小二乘残差会越来越多导致信息矩阵越来越大，计算量会不断增加。因此为了保持优化变量的个数在一定范围内，使用滑动窗口算法动态增加或移除优化变量。

• 流程：
  循环做如下两步：增加新的变量进入最小二乘系统优化，如果变量数目达到了一定的维度，则移除老的变量

• 如何移除老的变量

  利用边际概率移除老的变量 ，因为直接丢弃变量和对应的测量值，会损失信息，而且也会导致信息矩阵变得稠密了（因为 原本独立的变量之间变得相关了）。所以使用边际概率，将丢弃的变量所携带的信息传递给剩余的变量，让本来间接相关的变量之间此时直接相关

• 问题

• 存在的问题：

  • 原因：
    滑动窗口优化时，1 不同残差对于同一个变量计算雅克比矩阵时线性化点可能不一致 或 2 信息矩阵变成了两部分，且这两部分本应该在同一个地方求雅克比，但是实际上这两部分计算雅克比时的线性化点不同。这些会导致最小二乘的信息矩阵的零空间发生变化，从而在求解时引入错误信息（因为先验项的值是不对的），使得多个解的问题变成了一个确定解、不可观的变量变成了可观的。
  • 解决方法（First Estimated Jacobian）：
    在不同残差对同一个状态求雅克比时，要保证线性化点一致，也就是要固定住线性化点，这样就能避免零空间退化而使得不可观变量变得可观。
    slam每次迭代后，状态变量会不断更新，计算雅克比时要固定线性化点(线性化时的状态变量、求雅克比时的变量），固定在原来的点附近计算雅克比，而不是每次迭代更新以后的点的位置。
    VINS中没有采用EFJ策略：因为作者未发现明显的yaw不可观变化导致的轨迹漂移，反而加入EFJ后会导致结果不佳

一致性 可观性

• 一致性：指线性化点的一致不变
• 可观性：非线性函数中，对任意两个不相等的状态量(自变量)，都有不同的输出结果
  

H矩阵稀疏性

H矩阵的稀疏性是由雅克比矩阵引起的。因为对于代价函数中的其中一个errij，只描述了Ti观测到pj，和其他位姿和路标点无关，所以这条观测边只和这两个顶点有关，其他的部分的变量的导数都是0。

而H是由J构造的。H矩阵中，非对角线上的矩阵块不为0，则表示其对应的两个变量之间存在约束关系。

舒尔补矩阵

• 可用来对矩阵进行三角化，以简化协方差矩阵求逆：对多元高斯分布p(a,b)分解得到边际概率P(a)和条件概率p(b|a)，从而求得 边际概率和条件概率 的信息矩阵

  • 为什么要讨论 P(a), P(b|a) 的信息矩阵

    因为基于优化的 SLAM 问题中，直接操作的是信息矩阵，而不是协方差矩阵。所以，可借助边际概率、条件概率的信息矩阵从总体信息矩阵中简单方便的marg掉某一变量的影响

• 物理意义：

  • 非对角线上的非零矩阵块，表示对应的两个相机有共视关系。如果该块为零，则表示没有共同观测。

Schur消元

• 用处：消元后，无需再求解被marg变量，又不能直接删除掉与其相关的路标点信息，这样会减少约束，丢失信息。所以使用Schur消元
• 优势：没有丢弃帧间约束，只是利用计算的手段，自然的去掉无关的变量
• 由于对角块矩阵求逆相对简单，所以对线性方程组高斯消元，消去H矩阵右上角非对角部分E，就可以借助舒尔补首先求得位姿增量△Xc，然后再利用求得的△Xc求得空间点增量△Xp

边缘化

（就是 将联合概率分布分解为边缘概率分布和条件概率分布的过程、利用shur补减少优化参数）

• 原因：

1. 在系统优化的过程中，要不断将pose和feature移除滑窗，然而如果直接丢弃，会损失一些约束关系，从而导致求解的精度下降
2. 只有两个轴向的加速度不为0的时候，才能保证尺度可观，而像悬停、匀速等退化运动会经常存在，所以如果不加先验的边缘化会导致系统尺度不可观，因此如果没有历史信息做约束的话是无法求解的

• 方法：
  在移除pose和feature时，使用边际概率将相关联的约束转变为一个先验的约束条件作为prior放到优化问题中，使得丢弃的变量所携带的信息传递给剩余的变量，让本来间接相关的变量之间此时直接相关
• VINS两种策略：（这两种措施主要是针对悬停、匀速等退化运动）
  对比次新帧和次次新帧的视差量,来决定marg掉次新帧或者最老帧：

1. 当次新帧为关键帧时，marg掉滑动窗口内最老帧及其看到的路标点和相关联的IMU数据，将其转化为先验信息加到整体的目标函数中。如果该关键帧是观察到某个地图点的第一帧，则需要把该地图点的深度转移到后面的图像帧中去。
   流程：
   1)把上一次先验项中的残差项传递给当前先验项，并从中去除需要丢弃的状态量
   2)将滑窗内第0、1帧间的 IMU预积分因子放到marginalization_info中
   3)挑选出第一次观测帧为第О帧的路标点，将对应的多组视觉观测放到marginalization_info中
   4)得到每次IMU和视觉观测对应的参数块，雅可比矩阵，残差值
   5)多线程计整个先验项的参数块，雅可比矩阵和残差值
   eg. 在悬停或微小运动时，会一直边缘化新的视差较小的视觉帧，在保留了旧帧的同时，也保留了加速度信息，保证了尺度的可观性。会频繁的MARGIN_NEM，这样也就保留了那些比较旧但是视差比较大的 pose。这种情况如果一直MARGIN_OLD 的话，视觉约束不够强，状态估计会受IMU积分误差影响，具有较大的累积误差。
2. 当次新帧不是关键帧时，将直接扔掉当前帧的前一帧及它的视觉观测边，而不是边缘化，因为当前帧和次新帧很相似，当前帧跟路标点之间的约束和次新帧与路标点的约束很接近,直接丢弃并不会造成整个约束关系丢失过多信息（但要保留次新帧的 IMU数据，从而保证IMU预积分的连贯性）？？要把当前帧和前一帧之间的IMU预积分转换为当前帧和前二帧之间的IMU预积分。？
   eg. 当处于速度较大的恒速运动时，加速度信息会伴随旧帧移除，因此会发生尺度漂移，因此系统对恒速运动处理还不是很好。

如果当前帧图像和上一帧添加到滑窗的图像帧视差很小，则测量的协方差(重投影误差)会很大，进而会恶化优化结果

旋转外参优化方式

标志位：
2：没有任何先验，设定初始值为单位阵，在初始化中完成旋转外参标定
1：已经有比较好的旋转外参的值，初始化则不用标定，直接送给后端滑窗优化器，因为在真值附近，所以容易优化到真值
0：数据集中，提供精确地外参数，直接认为一个固定值
提特征点的精度，体现于重投影误差的置信度，通过虚拟相机统一设置为1.5pixel的误差的，认为提取点的精度在1.5pixel误差范围内
预积分的置信度是通过状态的递归得到来计算出协方差矩阵

检查IMU可观性

1. 计算带重力的速度的增量平均值
2. 方差
3. 标准差
4. 如果标准差小于阈值0.25，则认为激励不够，表示这段时间里的运动是比较单调的

纯视觉的单目slam问题（三维重建）

在11帧的滑窗中，选一个枢纽帧，要与最后一帧足够远（才能得到足够远的平移），但是不能太远（特征关联的点就会太少，保证结果可信且能三角化出更多的点）。
对极几何求得枢纽帧（设定为单位阵）与最后一帧间的位姿，得到三角化出特诊点的位置，PNP求解得到枢纽帧与最后一帧中所有帧的位姿（因为光流追踪，所以枢纽帧与最后一帧三角化得的地图点也可以被这些帧看到）。得到其他帧位姿后，又可以优化出更多的点（因为每一帧都会提取新特征点）。再求枢纽帧前面帧的位姿（与上面同样方法）。通过ceres优化器做一个小规模的重投影误差的global BA

获得匹配好的img和imu

IMU还没来：最后一个IMU时间要大于feature最早的时间
IMU最早的时间要大于等于第一个图像的时间，对于这个图像帧，是没有办法找到这个IMU数据

• 图像帧前有IMU数据
  取出队列中靠前的图像帧
  从队列中删除这个帧
  取出在图像之前的所有IMU数据
  
  
  

视觉SLAM总结——ORB SLAM2中关键知识点总结
视觉SLAM总结——SVO中关键知识点总结
视觉SLAM总结——LSD SLAM中关键知识点总结