VINS-Mono 代码解析五、滑动窗口

一、非线性最小二乘求解

二、滑动窗口算法

（1）理论

（2）滑动窗口算法关键步骤可视化

（3）滑动窗口算法中关键问题

先验矩阵不变，但是先验残差得变！虽然先验信息矩阵固定不变，但随着迭代的推进，变量被不断优化，先验残差需要跟随变化。否则，求解系统可能奔溃。
方法: 先验残差的变化可以使用一阶泰勒近似。

（4）VINS-Mono 中的滑动窗口算法

• 当滑动窗口中第二新的图像帧为关键帧,则 marg 最老的帧,以及上面的路标点。
• 当滑动窗口中第二新的图像帧不是关键帧,则丢弃这一帧上的视觉测量信息,IMU 预积分传给下一帧。

三、代码

实际滑动窗口的地方，如果第二最新帧是关键帧的话，那么这个关键帧就会留在滑动窗口中，时间最长的一帧和其测量值就会被边缘化掉；如果第二最新帧不是关键帧的话，则把这帧的视觉测量舍弃掉而保留IMU测量值在滑动窗口中,这样的策略会保证系统的稀疏性。这一部分跟后端非线性优化是一起进行的，这一部分对应的非线性优化的损失函数的先验部分。

void Estimator::slideWindow()
{
    TicToc t_margin;
    if (marginalization_flag == MARGIN_OLD)// keyframe
    {
        double t_0 = Headers[0].stamp.toSec();
         //(1) 保存最老帧信息，删除的是滑窗第一帧。
        back_R0 = Rs[0];
        back_P0 = Ps[0];
        
         //(2) 依次把滑窗内信息前移
        if (frame_count == WINDOW_SIZE)
        {
            for (int i = 0; i < WINDOW_SIZE; i++)
            {
                Rs[i].swap(Rs[i + 1]);

                std::swap(pre_integrations[i], pre_integrations[i + 1]);

                dt_buf[i].swap(dt_buf[i + 1]);
                linear_acceleration_buf[i].swap(linear_acceleration_buf[i + 1]);
                angular_velocity_buf[i].swap(angular_velocity_buf[i + 1]);

                Headers[i] = Headers[i + 1];
                Ps[i].swap(Ps[i + 1]);
                Vs[i].swap(Vs[i + 1]);
                Bas[i].swap(Bas[i + 1]);
                Bgs[i].swap(Bgs[i + 1]);
            }
            
            //(3) 把滑窗末尾(10帧)信息给最新一帧(11帧),WINDOW_SIZE = 10.
            Headers[WINDOW_SIZE] = Headers[WINDOW_SIZE - 1];
            
            //使用最近的一帧来进行初始化
            Ps[WINDOW_SIZE] = Ps[WINDOW_SIZE - 1];
            Vs[WINDOW_SIZE] = Vs[WINDOW_SIZE - 1];
            Rs[WINDOW_SIZE] = Rs[WINDOW_SIZE - 1];
            Bas[WINDOW_SIZE] = Bas[WINDOW_SIZE - 1];
            Bgs[WINDOW_SIZE] = Bgs[WINDOW_SIZE - 1];
             /*注意，在(2)中，已经实现了所有信息的前移，此时，最新一帧已经成为了滑窗中的第10帧，这里只是把原先的最新一帧的信息作为下一次最新一帧的初始值。*/

             
//这里可以使用reset,而不需要delete,重新new
//bug fixed!!!这里一定要注意,因为processIMU中根据 if (!pre_integrations[frame_count])来判断并进行初始化
            //(4) 新实例化一个IMU预积分对象给下一个最新一帧
            delete pre_integrations[WINDOW_SIZE];
            pre_integrations[WINDOW_SIZE] = new IntegrationBase{acc_0, gyr_0, Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]};

            //(5) 清空第11帧的buf
            dt_buf[WINDOW_SIZE].clear();
            linear_acceleration_buf[WINDOW_SIZE].clear();
            angular_velocity_buf[WINDOW_SIZE].clear();

             //(6)删除最老帧对应的全部信息.
            if (true || solver_flag == INITIAL)
            {
                map<double, ImageFrame>::iterator it_0;
                //在all_image_frame中找到窗口中的第一帧(swap将第二帧调整到了第一帧)
                it_0 = all_image_frame.find(t_0);
                delete it_0->second.pre_integration;
                it_0->second.pre_integration = nullptr;
 
                for (map<double, ImageFrame>::iterator it = all_image_frame.begin(); it != it_0; ++it)
                {
                    if (it->second.pre_integration)
                        delete it->second.pre_integration;
                    it->second.pre_integration = NULL;
                }

                all_image_frame.erase(all_image_frame.begin(), it_0);
                all_image_frame.erase(t_0);

            }
            slideWindowOld();
        }
    }
    else//(1)取出最新一帧的信息,删除的是滑窗第10帧。
    {
        if (frame_count == WINDOW_SIZE)
        {
            for (unsigned int i = 0; i < dt_buf[frame_count].size(); i++)
            {
                double tmp_dt = dt_buf[frame_count][i];
                Vector3d tmp_linear_acceleration = linear_acceleration_buf[frame_count][i];
                Vector3d tmp_angular_velocity = angular_velocity_buf[frame_count][i];

    //(2) 当前帧和前一帧之间的 IMU 预积分转换为当前帧和前二帧之间的 IMU 预积分
    //直接将最后一帧的复制过来不行吗? 注意这里不能复制!!!因为删除了倒数第二帧,所以需要继续预积分到frame_count.
                pre_integrations[frame_count - 1]->push_back(tmp_dt, tmp_linear_acceleration, tmp_angular_velocity);

                //最新帧的数据都将转移到倒数第二帧!
                dt_buf[frame_count - 1].push_back(tmp_dt);
                linear_acceleration_buf[frame_count - 1].push_back(tmp_linear_acceleration);
                angular_velocity_buf[frame_count - 1].push_back(tmp_angular_velocity);
            }

            //(3) 用最新一帧的信息覆盖上一帧信息
            Headers[frame_count - 1] = Headers[frame_count];//删除倒数第二帧
            Ps[frame_count - 1] = Ps[frame_count];
            Vs[frame_count - 1] = Vs[frame_count];
            Rs[frame_count - 1] = Rs[frame_count];
            Bas[frame_count - 1] = Bas[frame_count];
            Bgs[frame_count - 1] = Bgs[frame_count];

            
            //(4) 因为已经把第11帧的信息覆盖了第10帧，所以现在把第11帧清除
            // 这里delete和new是否会导致计算量大? @todo可以考虑写一个重置的函数
            delete pre_integrations[WINDOW_SIZE];
            pre_integrations[WINDOW_SIZE] = new IntegrationBase{acc_0, gyr_0, Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]};

            dt_buf[WINDOW_SIZE].clear();
            linear_acceleration_buf[WINDOW_SIZE].clear();
            angular_velocity_buf[WINDOW_SIZE].clear();

            slideWindowNew();//(5) 滑窗
        }
    }
}

子程序  Estimator::slideWindowOld()

void Estimator::slideWindowOld()
{
    sum_of_back++;

    bool shift_depth = solver_flag == NON_LINEAR ? true : false;
    if (shift_depth)
    {
        Matrix3d R0, R1;
        Vector3d P0, P1;
        R0 = back_R0 * ric[0];           //Rwc，滑窗原先的最老帧(被merge掉)的旋转(c->w)
        R1 = Rs[0]   * ric[0];           //滑窗原先第二老的帧(现在是最老帧)的旋转  (c->w)
        P0 = back_P0 + back_R0 * tic[0]; //pwc，滑窗原先的最老帧(被merge掉)的平移 (c->w)
        P1 = Ps[0] + Rs[0] * tic[0];     //滑窗原先第二老的帧(现在是最老帧)的平移   (c->w)
        
        //上面计算的是相邻2帧相机的位姿,下面的函数将窗口中特征点在第一帧的深度转换到新的第一帧中。
        f_manager.removeBackShiftDepth(R0, P0, R1, P1);
        //把首次在原先最老帧出现的特征点转移到原先第二老帧的相机坐标里(仅在slideWindowOld()出现过)
    }
    else
        f_manager.removeBack();//只在初始化期间被调用
         //当最新一帧是关键帧时，用于merge滑窗内最老帧(仅在slideWindowOld()出现过)
}