VINS-Mono（四）Visual-Inertial紧耦合优化

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];
    }

Table of Contents