VINS系统初始化成功后，系统状态会置为NON_LINEAR。此时Estimator::processImage()接收到图像特征点数据后，会进行Visual-Inertial的紧耦合优化：

 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
28
29
30
31
32
33
34
35
36

if (solver_flag == INITIAL) // 需要初始化
{...    
}
else // 已经成功初始化，进行正常的VIO紧耦合优化
    {
        TicToc t_solve;
        solveOdometry();
        ROS_DEBUG("solver costs: %fms", t_solve.toc());

        // 失效检测，如果失效则重启系统
        if (failureDetection())
        {
            ROS_WARN("failure detection!");
            failure_occur = 1;
            clearState();
            setParameter();
            ROS_WARN("system reboot!");
            return;
        }

        // 对窗口进行滑动
        TicToc t_margin;
        slideWindow();
        f_manager.removeFailures();
        ROS_DEBUG("marginalization costs: %fms", t_margin.toc());

        // prepare output of VINS
        key_poses.clear();
        for (int i = 0; i <= WINDOW_SIZE; i++)
            key_poses.push_back(Ps[i]);
        last_R = Rs[WINDOW_SIZE];
        last_P = Ps[WINDOW_SIZE];
        last_R0 = Rs[0];
        last_P0 = Ps[0];
    }

紧耦合优化入口函数

Visual-Inertial紧耦合优化的入口函数为：Estimator::solveOdometry()：

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

void Estimator::solveOdometry()
{
    if (frame_count < WINDOW_SIZE)
        return;
    if (solver_flag == NON_LINEAR)
    {
        // 三角化
        TicToc t_tri;
        f_manager.triangulate(Ps, tic, ric);
        ROS_DEBUG("triangulation costs %f", t_tri.toc());

        // 滑动窗口紧耦合优化
        optimization();
    }
}

三角化FeatureManager::triangulate()

在Estimator::solveOdometry()中，先调用FeatureManager::triangulate()， 三角化一些特征点，确保f_manager中的所有特征点都有一个深度值。

紧耦合优化Estimator::optimization()

对窗口进行滑动Estimator::slideWindow()

去除滑出了滑动窗口的特征点

FeatureManager::removeFailures()