VINS之边缘化

发表于 | 分类于

VINS之边缘化

Reference：

https://zhuanlan.zhihu.com/p/51330624

关于VINS中marg最老帧，先验项残差更新上述文章讲得挺清楚的了，根据代码里的内容再做下笔记，便于自己以后查看。

边缘化策略

在VINS中，通过判断次新帧是否为关键帧，边缘化策略分为两种：

  • 次新帧不为关键帧，只保留次新帧与窗口中其他帧的IMU约束，扔掉次新帧，把第一次在次新帧观测到的特征点转到以次新帧的前一帧为参考，在其他特征点中删除该帧的记录。这里对次新帧进行判断是否为关键帧而不是对窗口中最后一帧，原因应该是，若次新帧被删除，则次新帧与其前一帧的IMU约束能顺延加到窗口最后一帧上，若对窗口最后一帧判断，当窗口最后一帧被删除后，次新帧和窗口最后一帧之间的IMU约束不能方便地保存下来。

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    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];

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

  • 次新帧为关键帧，marg窗口中最老帧

如何marg最老帧

在VINS中，边缘化最老帧是发生在窗口优化之后

  • 假设在窗口优化之后，状态变量的结果是$x_0$

  • 找到与窗口最老帧相关的帧及特征点，构建最小二乘目标函数：$f(x)=$帧与帧之间的IMU约束+帧与点之间的视觉约束，使用高斯牛顿，每次迭代形式为$H\delta x=b$,那么在$x_0$处的迭代为$H_0\delta x=b_0$.其中

    $\delta x$包括两部分：最老帧的增量$\delta x_{01}$,其他优化变量的增量$\delta x_{02}$,将迭代方程重新写成如下形式：

    为marg掉$\delta x_{01}$,两边同乘舒尔项：

    由此可消掉$\delta x_{01}$，直接求解$\delta x_{02}$:

    由上式知，$\delta x_{01}$的信息已归到了$\delta x_{02}$的求解中，所以上式保留了先验信息。将上式简写为：

    在ceres里，不需要知道明确的目标函数，只要传入雅克比和残差，所以，接下来就是求基于$x_{02}$构建的先验项的雅克比和残差。下面是上述参考文章求先验项残差的主要思路，很清晰。

    基于$x_{02}$构建的先验信息，在之后的某个时刻对变量$x_k$迭代优化的过程可以写做下式,其中$H_B,H_C$对应IMU部分和相机重投影部分残差项.$x_{k2}$为$x_{02}$中的变量在对应时刻的值.$\boxminus$是广义上的减法:

    为了构建$x_k$状态的优化，在构建H时，需要得到$x_k$处的 $H_{prior}$和$b_{prior}$ 。首先根据$\delta x_{prior}$的定义，其线性化点是$x_{02}$，所以

    根据$H^$SVD分解求得到对应的雅克比矩阵$J^$ :

    对应代码MarginalizationInfo::marginalize()：

    1
    2
    3
    4
    5
    6
    7
    8
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes2(A);
        Eigen::VectorXd S = Eigen::VectorXd((saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array(), 0));
        Eigen::VectorXd S_inv = Eigen::VectorXd((saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array().inverse(), 0));

        Eigen::VectorXd S_sqrt = S.cwiseSqrt();
        Eigen::VectorXd S_inv_sqrt = S_inv.cwiseSqrt();
        linearized_jacobians = S_sqrt.asDiagonal() * saes2.eigenvectors().transpose();
        linearized_residuals = S_inv_sqrt.asDiagonal() * saes2.eigenvectors().transpose() * b;//初始残差e0,但是为什么少了一个负号

    接着根据$b_0$和$J^*$更新求解$x_k$处的先验项$b_{prior}$，通过一阶线性化求解$b_{prior}$:

    在ceres具体实现时，传入的是雅克比矩阵和残差项$e_{prior}$，所以需要求解出$e_{prior}$.利用$b_{prior}$的计算公式:

    对应代码：

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    16
    17
    //先验项中残差采用了一阶展开，但雅克比用的是线性化点的雅克比，没有变
    Eigen::Map<Eigen::VectorXd>(residuals, n) = marginalization_info->linearized_residuals + marginalization_info->linearized_jacobians * dx;
        if (jacobians)
        {

            for (int i = 0; i < static_cast<int>(marginalization_info->keep_block_size.size()); i++)
            {
                if (jacobians[i])
                {
                    int size = marginalization_info->keep_block_size[i], local_size = marginalization_info->localSize(size);
                    int idx = marginalization_info->keep_block_idx[i] - m;
                    Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> jacobian(jacobians[i], n, size);
                    jacobian.setZero();
                    jacobian.leftCols(local_size) = marginalization_info->linearized_jacobians.middleCols(idx, local_size);
                }
            }
        }

  • 若之前已经有先验项last_marginalization_info，在marg当前窗口最老帧k的时候，对于已有的先验部分，是将k帧的优化变量从last_marginalization_info优化变量中去掉，目前个人理解，上一轮先验项对其他变量的雅克比在这轮优化中还是没有变的，还是上一轮中的雅克比。