【SLAM】VINS-MONO解析——sliding window

8.sliding window

8.1 理论基础

实际上，这一部分跟后端非线性优化是一起进行的，这一部分对应的非线性优化的损失函数的先验部分。理论基础部分的代码基本在第7章部分。
8.1.1 上一次非线性优化结束，最后的H矩阵就是本轮非线性优化的先验矩阵的前身。
8.1.2 构造先验矩阵
(1)移动需要marg掉的pose和路标点
对应的，J矩阵的一些列需要删除掉；H矩阵的一些行需要删掉。在操作先验矩阵的时候，需要把要marg掉的行和列移动到H矩阵的左上角，b向量相应的部分移到上方去，方便采用shur的方法marg掉，如下式所示：

mm对应的是要被marg掉的部分，rr是要留下来的部分。

(2)边缘化:构造先验矩阵

这个就是边缘化操作，边缘化之后，得到的先验矩阵H和先验向量b分别是：

滑窗会导致H矩阵不再稀疏。

8.1.3 构造非线性优化的H矩阵
这一部分实际上就是非线性优化部分关于构造H矩阵的内容。
(1)对先验矩阵进行扩维，扩充的维度就是下一次优化增加的状态量的维度

和直接 Bundle Adjustment 相比，多了一个先验矩阵的维护。

(2)与IMU和视觉部分的H矩阵进行叠加
实际上，每一轮优化的时候，H矩阵都是一块一块地增加的，所以先加上H矩阵，之后每添加一个残差项，总的H矩阵都会对应地自动增加。

8.1.4 进行非线性优化(FEJ)
这一部分实际上就是非线性优化部分关于LM/DOGLEG算法求解部分的内容。最后得到优化后的状态量。
这块涉及到FEJ问题。
先说结论，FEJ的意思就是说在总的大H矩阵中，先验矩阵对应的部分在本次优化的迭代过程中的值保持不变。

再说理由，
(1)当状态量1被merg掉之后，原本相互独立的量会变的不独立，表现在H矩阵中就是产生fill-in现象。

(2)当再加入新的观测时，如8.1.3-(1)粉色部分，H矩阵有的部分，既与先验有关，也与新加入的信息有关。

(3)假如说这部分H矩阵块不固定，每次迭代会得到新的线性化点，这个新的线性化点会与先验的线性化点不一样，可能会导致信息矩阵的零空 间发生变化，从而在求解时引入错误信息。
为什么？
因为信息矩阵 Λ 不满秩。对应的零空间为 N, 用高斯牛顿求解时有

增量 δx 在零空间维度下变化，并不会改变我们的残差。这意味着 系统可以有多个满足最小化损失函数的解 x。

(4)假如说不固定先验部分对应的线性化点，会使原本不可观的信息变得可观，多个解的问题变成了一个确定解。而这个是错误的。

(5)所以，采用FEJ，也就是first estimated jacobian。不同残差对同一个状态求雅克比时，线性化点必须一致。这样就能避免零空间退化而使得不可观变量变得可观。也就是说，在总的大H矩阵中，先验矩阵对应的部分在本次优化的迭代过程中的值保持不变。

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

其它部分，就和非线性优化一样了。但是有一点需要注意，就是在g20中，传入的是Jacobian，对于IMU残差和重投影残差，Jacobian都是知道的，但是这个先验部分，我们知道的是H，所以就需要根据marg后的状态量反解出一个Jacobian。

8.1.5 返回步骤8.1.1，进行下一次的非线性优化。
在vins中，实际上它是8.1.4是循环的第一步。另外，崔神说vins并没有采用FEJ。

8.2 代码

slideWhindow()这个函数在初始化和非线性优化部分都出现过。对于marg的是old还是new，这是在5.2.1部分进行的。

8.2.1 if (marginalization_flag == MARGIN_OLD)
删除的是滑窗第一帧。
(1) 保存最老帧信息

double t_0 = Headers[0].stamp.toSec();
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帧)

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帧，这里只是把原先的最新一帧的信息作为下一次最新一帧的初始值。

(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)删除最老帧对应的全部信息

map<double, ImageFrame>::iterator it_0;
it_0 = all_image_frame.find(t_0);//6.找到滑窗内最老一帧信息
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)
{//7.把滑窗内最老一帧以前的帧的预积分信息全删掉
    if (it->second.pre_integration)
        delete it->second.pre_integration;
    it->second.pre_integration = NULL;
}
//8.删掉滑窗内最老帧以前的所有帧(不包括最老帧)，和最老帧
all_image_frame.erase(all_image_frame.begin(), it_0);
all_image_frame.erase(t_0);   
}

(7) slideWindowOld()

void Estimator::slideWindowOld()
{
    sum_of_back++;//统计一共有多少次merge滑窗第一帧的情况

    bool shift_depth = solver_flag == NON_LINEAR ? true : false;
    if (shift_depth)
    {
        Matrix3d R0, R1;
        Vector3d P0, P1;
        R0 = back_R0 * ric[0];//滑窗原先的最老帧(被merge掉)的旋转(c->w)
        R1 = Rs[0] * ric[0];//滑窗原先第二老的帧(现在是最老帧)的旋转(c->w)
        P0 = back_P0 + back_R0 * tic[0];//滑窗原先的最老帧(被merge掉)的平移(c->w)
        P1 = Ps[0] + Rs[0] * tic[0];//滑窗原先第二老的帧(现在是最老帧)的平移(c->w)
        f_manager.removeBackShiftDepth(R0, P0, R1, P1);//把首次在原先最老帧出现的特征点转移到原先第二老帧的相机坐标里(仅在slideWindowOld()出现过)
    }
    else
        f_manager.removeBack();//当最新一帧是关键帧时，用于merge滑窗内最老帧(仅在slideWindowOld()出现过)
}

8.2.2 if (marginalization_flag == MARGIN_NEW)
删除的是滑窗第10帧。

(1)取出最新一帧的信息

else
{
    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 预积分

    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 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();

(5) 滑窗

        slideWindowNew();//为什么这里可以不对前一帧进行边缘化而是直接丢弃，原因就是当前帧和前一帧很相似。
    }//因此当前帧与地图点之间的约束和前一帧与地图点之间的约束是接近的，直接丢弃并不会造成整个约束关系丢失信息
}

再看看slideWindowNew()里面的具体内容。

void Estimator::slideWindowNew()
{//因为已经把第11帧的信息覆盖了第10帧，所以现在把第11帧清除
    sum_of_front++;//统计一共有多少次merge滑窗第10帧的情况
    f_manager.removeFront(frame_count);//唯一用法：当最新一帧(11)不是关键帧时，用于merge滑窗内最新帧(10)(仅在slideWindowNew()出现过)
}