几个重要的对象

`estimator`

VINS Mono的主要功能被封装成了一个类Estimator，在vins_estimator_node中，通过创建一个Estimator的实例estimator来实现功能。我把estimator称作VINS系统，VINS系统有两种状态：

1
2
3
4
5
6


enum SolverFlag
{
    INITIAL, // 还未成功初始化
    NON_LINEAR // 已成功初始化，正处于紧耦合优化状态
};

`f_manager`

与滑动窗口中特征点相关的操作，被封装为了一个类FeatureManager。在estimator对象内部创建了一个FeatureManager的实例f_manager，作为estimator的成员变量，它负责完成滑动窗口中的特征点管理，三角化，以及关键帧相关的操作。

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27


class FeatureManager
{
    ...
    list<FeaturePerId> feature; // 通过FeatureManager可以得到滑动窗口内所有的角点信息
    ...
}

// FeaturePerId的定义
class FeaturePerId
{
    // 以feature_id为索引，并保存了出现该角点的第一帧的id，
    ...
    const int feature_id;
    int start_frame;
    vector<FeaturePerFrame> feature_per_frame;  
}

// FeaturePerFrame的定义
class FeaturePerFrame
{
    // 保存了归一化坐标，图像坐标以及深度    
    ...
    Vector3d point;
    Vector2d uv;
    double z;
}

`sfm_f`

sfm_f是用于视觉初始化的图像特征点数据（不是有f_manager吗？为什么还要再整一个sfm_f？）：

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13


// 用于视觉初始化的图像特征点数据
vector<SFMFeature> sfm_f;

// SFMFeature的定义
struct SFMFeature
{
    bool state;
    int id;
    vector<pair<int,Vector2d>> observation;
    double position[3];
    double depth;
};  

`all_image_frame`

1
2
3
4


// 存储所有的ImageFrame对象（每读取一帧图像就会构建ImageFrame对象）
    // 键是图像帧的时间戳，值是ImageFrame对象，ImageFrame对象中保存了图像帧的位姿，相应的预积分和图像特征点信息
    map<double, ImageFrame> all_image_frame; 

`sfm_tracked_points`

1
2


map<int, Vector3d> sfm_tracked_points;

``

1
2

初始化概述

当Estimator::processImage()接收到图像特征点数据后：

先调用FeatureManager::addFeatureCheckParallax()函数，把图像特征点数据存入FeatureManager对象，并判断是否把滑动窗口中的第二最新帧选为关键帧，然后选择滑动窗口时的边缘化策略marginalization_flag(MARGIN_OLD or MARGIN_SECOND_NEW)。
然后检查相机与IMU的外参是否已经标定，如果未标定，则进行在线标定(在线标定有失败的可能性)。
接着再去检查VINS系统的状态，当VINS系统第一次启动，以及VINS系统重启的时候，会处于INITIAL状态。当VINS系统处于INITIAL状态时，会存在两种情况，一是滑动窗口中图像特征点数据的帧数（当前读入的帧也计算在内）没有达到滑动窗口大小WINDOW_SIZE，另一种情况是滑动窗口已经满了。
- 滑动窗口未塞满：
  由于图像特征点数据是一帧一帧的塞到滑动窗口中的，所以在VINS系统刚刚启动或者刚刚重启的时候，滑动窗口会没有塞满，此时不进行初始化操作，而且每来一帧图像特征点数据，都会塞到滑动窗口中，直到数量达到WINDOW_SIZE。（代码中窗口大小实际上是WINDOW_SIZE + 1，不过问题不大）
- 滑动窗口已塞满：
  当滑动窗口塞满后，先检查相机与IMU的外参是否已经标定，以及离上次初始化操作的时间间隔是否大于一个阈值（比如代码中的0.1s）。如果外参已经标定，且时间间隔大于阈值，才进行初始化操作，否则不进行初始化。初始化操作由Estimator::initialStructure()完成。
  在滑动窗口已塞满的情况下，如果由于不满足条件而未进行初始化操作，或者初始化操作失败，则会根据marginalization_flag对窗口进行滑动操作Estimator::slideWindow()。通过滑动窗口可以腾出一个位置，留给下一帧读取的图像特征点数据。在读取新的图像特征点数据后，又返回步骤1。

初始化入口函数

VINS Mono初始化的入口函数为bool Estimator::initialStructure()。VINS初始化的流程为：

通过计算重力协方差，检测IMU是否有足够的excitation。如果有则进入下一步，否则VINS初始化失败。（在新版本代码中，进行了检测，但是即使激励不够，也会进入下一步）
纯视觉初始化
- Estimator::relativePose()在滑动窗口中选择与滑动窗口最新帧有足够数量的共视特征点（30个）以及足够视差（20 pixels）的帧l（l是该帧在滑动窗口中的帧号，注意是字母l，不是数字1），使用五点法恢复两帧之间的相对位姿。如果这一步失败，则VINS初始化失败。
- GlobalSFM::construct()计算滑动窗口中全部初始帧的相机位姿和特征点的3D位置（这里似乎是把帧l的相机坐标系作为世界坐标系，但是VINS Mono的论文中说是把滑动窗口中的第一帧的相机坐标系作为世界坐标系。不知道是不是我理解的不对，不过这个问题不大）。在GlobalSFM::construct()中，使用一种鸡生蛋、蛋生鸡的方式交替进行特征点三角化和pnp解算位姿的操作，对任意一帧的pnp解算失败都会导致GlobalSFM::construct()失败，进而VINS初始化失败。如果滑动窗口中全部初始帧的位姿和特征点的3D位置结算成功，最后会使用cere进行BA优化。
- 在视觉初始化的最后一步，对所有帧求解一次pnp。因为前面几步只得到了滑动窗口内所有关键帧的位姿，但由于初始化不一定会一次成功，所以会有一些图像特征点数据帧滑动到了滑动窗口外（这些帧的数据保存在all_image_frame中），此时需要把那些不在滑动窗口中的帧的位姿结算出来。在这一步，如果有任意一帧pnp解算位姿失败，则VINS初始化失败。最后根据各帧相机坐标系的姿态和外参，得到用各帧IMU坐标系的姿态。
视觉-惯性对齐。对齐过程在Estimator::visualInitialAlign()中完成。
- 陀螺仪bias初始化。在滑动窗口中，用每两帧之间通过SFM求解出来的旋转与IMU预积分的旋转构建一个最小二乘问题，解出陀螺仪的bias。初始化陀螺仪的bias后，要重新计算IMU预积分值。
- 计算各图像帧对应的速度v，以及重力向量g和尺度因子s。在滑动窗口中，用每两帧之间通过SFM求解出来的位姿信息，和IMU预积分出来的位置和速度，构建一个最小二乘问题，解出v、g、s。
- 对重力向量g的refinement。用上一步求出的重力向量，固定重力加速度的模值，在正切空间重新参数化重力向量（从3个自由度变为2个自由度），对重力向量方向进行精调。
- 得到以g为z轴的世界坐标系，并根据尺度因子s对地图进行缩放。如果程序成功运行到了这一步，则VINS初始化成功。

初始化过程中任意一步的失败，都会导致VINS初始化失败，然后重新开始初始化。VINS的初始化过程就像要经历九九八十一难，说起九九八十一难，我就想起了***，明年，中美，合拍，关注。

纯视觉初始化

计算重力协方差

TODO

`Estimator::relativePose()`

`GlobalSFM::construct()`

解算滑动窗口外所有帧的位姿

视觉-惯性对齐

`VisualIMUAlignment()`

在Estimator::visualInitialAlign()中，调用位于initial_aligment.cpp中的VisualIMUAlignment()实现视觉SFM的结果与IMU预积分结果对齐：

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13


// visual-inertial alignment：视觉SFM的结果与IMU预积分结果对齐
bool VisualIMUAlignment(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs, Vector3d &g, VectorXd &x)
{
    //估测陀螺仪的Bias，对应论文V-B-1
    solveGyroscopeBias(all_image_frame, Bgs);

    //求解V 重力向量g和 尺度s
    if(LinearAlignment(all_image_frame, g, x))
        return true;
    else 
        return false;
}

`solveGyroscopeBias()`

解算陀螺仪的Bias。

`LinearAlignment()`

求解各帧对应的速度v，以及重力向量g和尺度因子s。

VINS-Mono（四）初始化

Table of Contents