vins-mono后端优化（2）

陈于落雁

已于 2024-01-21 10:36:49 修改

阅读量1.1k

点赞数 36

分类专栏： vins-mono详解文章标签：算法

于 2023-11-24 23:10:47 首次发布

本文链接：https://blog.csdn.net/weixin_44587828/article/details/134422104

版权

vins-mono详解专栏收录该内容

7 篇文章 4 订阅

订阅专栏

前言

在上篇文章vins-mono后端优化（一）中详细梳理了后端优化中关于预积分残差雅克比和视觉重投影残差雅克比的计算方法。在后端优化函数中，还包含比较重要的滑窗约束。所谓滑窗约束是指在优化中保持滑动窗口内包含11帧位姿信息和速度、bias，而每次更新滑窗需要边缘化掉最老帧或者次新帧，在这个过程中最老帧或者次新帧会包含和滑窗内的状态量相关的约束，需要将这些约束添加到优化中，这个过程也叫做边缘化。

如何简单理解为什么滑窗中需要被滑掉的状态量也能和滑窗内数据构成约束呢？

举个例子：假如愚公移山故事中，愚公没有感动上天，只能世世代代挖山，那么等愚公死后他的子孙也会继续干，可能他的很多子孙并没有见过愚公，但是仍然要受到愚公挖山“祖训”的约束，有了这约束就能保证他的后代的个人事业就不能完全摒弃挖山这件事。在滑窗边缘化中，死去的愚公就是即将被边缘化的最老帧或者次新帧，而他的子孙就是新进滑窗的关键帧，而滑掉的帧和滑窗内的帧构成的约束就是愚公的“祖训”。通过这个简单的例子大概可以描述为什么会有滑窗边缘化约束这件事，但是具体如何实现的还得经过科学推导。

边缘化流程梳理

舒尔补

在进行边缘化中需要用到舒尔补，下面简单介绍舒尔补是如何实现边缘化计算的。

对于边缘化约束，因为要构建优化问题所以会涉及到残差和雅克比矩阵的计算，对于高斯牛顿法求解优化问题，通常是以下形式：

$H\Delta x=g$

而其中H矩阵是由雅克比矩阵转置和雅克比相乘得到，g是雅克比转置和残差相乘乘以-1得到，因此如果能构建和上述矩阵形式类似的关于待优化变量的非齐次方程，就能得到相应的残差和雅克比矩阵。下面详细介绍该方程的建立和舒尔补进行边缘化参数的过程。

首先将H矩阵分解成为4个分块矩阵，对应的deltax和g也同样进行分块：

$\begin{bmatrix} \Lambda _{a} &\Lambda _{b} \\ \Lambda _{b}^{T}&\Lambda _{c} \end{bmatrix}\begin{bmatrix} \Delta x_{a}\\ \Delta x_{b} \end{bmatrix}=\begin{bmatrix} g_{a}\\ g_{b} \end{bmatrix}$

其中deltax a是需要进行边缘化掉的状态量，然后经过如下的变换：

$\begin{bmatrix} I & 0\\ -\Lambda _{b}^{T}\Lambda _{a}^{-1}&I \end{bmatrix}\begin{bmatrix} \Lambda _{a} &\Lambda _{b} \\ \Lambda _{b}^{T} & \Lambda _{c} \end{bmatrix}\begin{bmatrix} \Delta x_{a} \\ \Delta x_{b} \end{bmatrix}=\begin{bmatrix} I & 0\\ -\Lambda _{b}^{T}\Lambda _{a}^{-1}&I \end{bmatrix}\begin{bmatrix} g_{a}\\ g_{b} \end{bmatrix}$

可以得到以下形式

$\begin{bmatrix} \Lambda _{a} & \Lambda _{b}\\ 0& \Lambda _{c}-\Lambda _{b}^{T}\Lambda _{a}^{-1}\Lambda _{b} \end{bmatrix}\begin{bmatrix} \Delta x_{a}\\ \Delta x_{b} \end{bmatrix}=\begin{bmatrix} g_{a}\\ g_{b}-\Lambda _{b}^{T}\Lambda _{a}^{-1}g_{a} \end{bmatrix}$

根据上式，在矩阵的第二行可以得到以下式子

$(\Lambda _{c}-\Lambda _{b}^{T}\Lambda _{a}^{-1}\Lambda _{b})\Delta x_{b}=g_{b}-\Lambda _{b}^{T}\Lambda _{a}^{-1}g_{a}$

上式基本得到了边缘化的预期效果，观察上式可以发现，关于deltax_a的方程去掉了，但是关于它的相关系数已经“掺杂”在deltax_b之中，这个deltax_b就相当于上面愚公例子里面的愚公的子孙，deltax_a就是愚公。简言之，上式中deltax_a已经对deltax_b构成了约束。

当然这里只是直观简单介绍了舒尔补实现方式，关于舒尔补的详细证明，可以参见《鲁棒控制》-俞立，其中对H矩阵有一定的限制条件，比如说H要对称，Lamda_a要正定等。

构建H矩阵

为了实现舒尔补的边缘化效果，首先要构建H矩阵，这一步骤其实包含手写VIO后端的过程，对于理解slam有很好的启发。

构建H矩阵要清楚被边缘化掉的状态量和与这些状态量相关滑窗内其他的状态量信息，因为要把被边缘化的状态量按照上面推导添加到矩阵的前面的行，被构成约束的状态量添加到矩阵的后面。当边缘化掉最老帧时候，被边缘化的状态量有滑窗内第0帧的位姿（7维），速度和bias（9维），第1帧imu预积分量以及所有可以被滑窗内第0帧图像观测到的地图点。为什么要把第一帧观测到的地图点进行边缘化掉呢？因为如果不这样做，边缘化后的H矩阵不再稀疏，将使得状态增量的求解变得复杂，而地图点比较多，把这些地图点边缘化后即使H矩阵是稠密矩阵，他的稠密维数也会大大降低，利于求解。分析与被边缘化状态量相关的状态量之前，需要理清滑窗优化的两个约束——imu预积分约束和视觉重投影误差约束。那么第1帧的预积分相关的状态量有：第0帧和第1帧的位姿以及速度bias。和第0帧图像观测到的地图点相关的状态量有滑窗内其他能观测到同一地图点的图像帧，相机和imu外参以及地图点的逆深度，其中需要边缘化掉的主要状态量是地图点。

下面通过源代码进行梳理构建H矩阵的过程：

边缘化处理有一个专门的类进行管理，整个边缘化的核心部分也是由这个类的成员函数完成。在边缘化中，首先需要创建这个类的对象，后面的操作都是基于这个对象完成。

class MarginalizationInfo
{
  public:
    ~MarginalizationInfo();
    int localSize(int size) const;  //这两个函数是对于位姿相关信息进行处理，因为位姿是7维状态量，存在过参数化，他的雅克比是6*6的，因此local是6，global是7
    int globalSize(int size) const;
    void addResidualBlockInfo(ResidualBlockInfo *residual_block_info);  //收集残差块信息，为了构建H矩阵
    void preMarginalize();
    void marginalize();
    std::vector<double *> getParameterBlocks(std::unordered_map<long, double *> &addr_shift);

    std::vector<ResidualBlockInfo *> factors;
    int m, n;
    std::unordered_map<long, int> parameter_block_size; //global size
    int sum_block_size;
    std::unordered_map<long, int> parameter_block_idx; //local size
    std::unordered_map<long, double *> parameter_block_data;

    std::vector<int> keep_block_size; //global size
    std::vector<int> keep_block_idx;  //local size
    std::vector<double *> keep_block_data;

    Eigen::MatrixXd linearized_jacobians;
    Eigen::VectorXd linearized_residuals;
    const double eps = 1e-8;

};

次新帧是关键帧

先介绍倒数第二帧是关键帧，边缘化掉滑窗内最老帧的情况：

{
            if (pre_integrations[1]->sum_dt < 10.0)
            {
                IMUFactor* imu_factor = new IMUFactor(pre_integrations[1]);
                //构造函数第三个参数代表的是和预积分约束相关的参数块 第0 1帧的位姿 速度 零偏起始地址
                ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(imu_factor, NULL,
                                                                           vector<double *>{para_Pose[0], para_SpeedBias[0], para_Pose[1], para_SpeedBias[1]},
                                                                           vector<int>{0, 1});//这里表示第0个和第1个参数块（para_Pose[0], para_SpeedBias[0]）是要被边缘化掉的
                marginalization_info->addResidualBlockInfo(residual_block_info);
            }
        }

先是对预积分参数块处理，判断第一个预积分的有效性，当预积分时间小于10s时候认为预积分有效，这里判断其实在相机图像流正常时候一定符合条件。然后第一步创建一个imu的factor，这里和优化部分的factor是一样的，具体介绍可以参见vins-mono后端优化（1）。第二步是创建一个残差块信息的对象，这个ResdiualBlockInfo的结构体如下：

struct ResidualBlockInfo
{
    ResidualBlockInfo(ceres::CostFunction *_cost_function, ceres::LossFunction *_loss_function, std::vector<double *> _parameter_blocks, std::vector<int> _drop_set)
        : cost_function(_cost_function), loss_function(_loss_function), parameter_blocks(_parameter_blocks), drop_set(_drop_set) {}

    void Evaluate();

    ceres::CostFunction *cost_function;
    ceres::LossFunction *loss_function;
    std::vector<double *> parameter_blocks;
    std::vector<int> drop_set;

    double **raw_jacobians;
    std::vector<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> jacobians;
    Eigen::VectorXd residuals;

    int localSize(int size)
    {
        return size == 7 ? 6 : size;
    }
};

在构造函数中给一些成员变量赋值，可以看到该结构体中包含一个ceres的CostFunction对象，一个ceres的lossfunction的对象，还有一个vector类型的double的二维数组（一维数组的指针）——记录所有的残差块，以及一个一维数组drop_set——记录被边缘化掉的参数块的索引。还有两个和计算雅克比相关的变量，以及一个残差块的Eigen对象。

在进行边缘化第一帧预积分时候，和第一帧预积分相关的参数块包括（滑窗内）第0帧的位姿、第0帧的速度和bias、第1帧的位姿和第一帧的速度bias。其中需要边缘化的参数块是第0帧的位姿和速度bias。

在构造了残差信息块对象后，把残差块信息添加到边缘化信息对象中去。

void MarginalizationInfo::addResidualBlockInfo(ResidualBlockInfo *residual_block_info)
{
    factors.emplace_back(residual_block_info);//收集所有残差块信息

    std::vector<double *> &parameter_blocks = residual_block_info->parameter_blocks;//这是和约束相关的参数块的首地址
    std::vector<int> parameter_block_sizes = residual_block_info->cost_function->parameter_block_sizes();//各个参数块大小
    //收集的是和约束相关的参数块信息 放到parameter_block_size管理
    for (int i = 0; i < static_cast<int>(residual_block_info->parameter_blocks.size()); i++)
    {
        double *addr = parameter_blocks[i];
        int size = parameter_block_sizes[i];
        //这是一个哈希表unordered map 避免重复添加
        parameter_block_size[reinterpret_cast<long>(addr)] = size;
    }
    //待边缘化参数块
    for (int i = 0; i < static_cast<int>(residual_block_info->drop_set.size()); i++)
    {
        //先准备好待边缘化的参数块的unordered map 值暂时设为0
        double *addr = parameter_blocks[residual_block_info->drop_set[i]];
        parameter_block_idx[reinterpret_cast<long>(addr)] = 0;
    }
}

可以看到，首先将所有的残差信息块收集到边缘化信息对象的factor成员变量中。然后，用两个变量parameter_blocks parameter_block_sizes分别记录残差块的首地址和参数块的维度。再将这些参数块的信息添加到一个哈希表parameter_block_size中，键值是参数块的地址，value是参数块的维度。这样做是为了避免多次将同一个参数块重复添加。同样，将待边缘化掉的参数块放到parameter_block_idx中，value是0。这个函数功能就结束了。

视觉重投影的相关残差块信息也是同样处理，边缘化掉的参数是第0帧位姿和所有能被第0帧观测到的3d点。

{
            int feature_index = -1;
            for (auto &it_per_id : f_manager.feature)
            {
                it_per_id.used_num = it_per_id.feature_per_frame.size();
                if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
                    continue;

                ++feature_index;

                int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;
                //只找能被第0帧看到的特征点，且第零帧是该特征点的起始帧
                if (imu_i != 0)
                    continue;

                Vector3d pts_i = it_per_id.feature_per_frame[0].point;
                //遍历所有能看到这个特征点的KF，建立和第0帧的约束
                for (auto &it_per_frame : it_per_id.feature_per_frame)
                {
                    imu_j++;
                    if (imu_i == imu_j)
                        continue;

                    Vector3d pts_j = it_per_frame.point;
                    if (ESTIMATE_TD)
                    {
                        ProjectionTdFactor *f_td = new ProjectionTdFactor(pts_i, pts_j, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocity,
                                                                          it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td,
                                                                          it_per_id.feature_per_frame[0].uv.y(), it_per_frame.uv.y());
                        ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(f_td, loss_function,
                                                                                        vector<double *>{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]},
                                                                                        vector<int>{0, 3});
                        marginalization_info->addResidualBlockInfo(residual_block_info);
                    }
                    else
                    {
                        //图像重投影约束处理和预积分残差块处理一致
                        ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
                        ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(f, loss_function,
                                                                                       vector<double *>{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]},
                                                                                       vector<int>{0, 3});
                        marginalization_info->addResidualBlockInfo(residual_block_info);
                    }
                }
            }
        }

至此可以认为构建H矩阵的准备工作已经完成。为了更加清晰这个准备过程，先对这部分进行小结。

构建H矩阵准备工作总结：

该部分主要是创建一个边缘化的对象，接着把所有和边缘化相关的参数块添加到一个残差块对象中，然后把所有参数块和被边缘化参数块分别添加到边缘化对象的两个哈希表中。

完成了准备工作，将进行下一步preMarginalize()函数。

void MarginalizationInfo::preMarginalize()
{
    for (auto it : factors)//遍历所有残差块信息
    {
        it->Evaluate();

        std::vector<int> block_sizes = it->cost_function->parameter_block_sizes();//得到每个残差块的参数块大小
        for (int i = 0; i < static_cast<int>(block_sizes.size()); i++)
        {
            long addr = reinterpret_cast<long>(it->parameter_blocks[i]);//得到该参数块地址
            int size = block_sizes[i];//参数块大小
            //把各个参数块备份起来
            if (parameter_block_data.find(addr) == parameter_block_data.end())
            {
                double *data = new double[size];
                memcpy(data, it->parameter_blocks[i], sizeof(double) * size);//深拷贝
                parameter_block_data[addr] = data;
            }
        }
    }
}

该函数会对每个残差块进行处理，先调用残差块对象的Evaluate接口，这个接口并不是直接调用ceres::CostFunction的evaluate函数，具体实现如下：

void ResidualBlockInfo::Evaluate()
{
    residuals.resize(cost_function->num_residuals());//确定残差的维数

    std::vector<int> block_sizes = cost_function->parameter_block_sizes();//确定和边缘化相关的参数块数目 比如说预积分相关参数块数目是4
    raw_jacobians = new double *[block_sizes.size()];//ceres维护的都是double数组 这里是给雅克比准备的数组 
    jacobians.resize(block_sizes.size());

    for (int i = 0; i < static_cast<int>(block_sizes.size()); i++)
    {
        jacobians[i].resize(cost_function->num_residuals(), block_sizes[i]);//雅克比大小 = 残差数目 x 参数块数目 残差维数预积分15 重投影2
        raw_jacobians[i] = jacobians[i].data();//建立起raw_jacobians和jacobians的联系 .data是指针 之后操作raw_jacobians就可以改变jacobians
        //dim += block_sizes[i] == 7 ? 6 : block_sizes[i];
    }
    //调用各自重载的接口 计算残差和雅克比 这里应该调用的是对应的图像和预积分重写的factor的类 实现一个多态
    cost_function->Evaluate(parameter_blocks.data(), residuals.data(), raw_jacobians);
    //如果有核函数 要对残差进行处理
    if (loss_function)
    {
        double residual_scaling_, alpha_sq_norm_;

        double sq_norm, rho[3];

        sq_norm = residuals.squaredNorm();//获得残差的模
        loss_function->Evaluate(sq_norm, rho);//调用ceres核函数的evaluate rho[0]核函数这个点的值 rho[1] 一阶导 rho[2]二阶导

        double sqrt_rho1_ = sqrt(rho[1]);//一阶导开根号

        if ((sq_norm == 0.0) || (rho[2] <= 0.0)) //对于柯西核 二阶导都是小于0
        {
            residual_scaling_ = sqrt_rho1_;
            alpha_sq_norm_ = 0.0;
        }
        else
        {
            const double D = 1.0 + 2.0 * sq_norm * rho[2] / rho[1];
            const double alpha = 1.0 - sqrt(D);
            residual_scaling_ = sqrt_rho1_ / (1 - alpha);
            alpha_sq_norm_ = alpha / sq_norm;
        }

        for (int i = 0; i < static_cast<int>(parameter_blocks.size()); i++)
        {
            //相当于雅克比乘以一个残差的一阶导开根号
            jacobians[i] = sqrt_rho1_ * (jacobians[i] - alpha_sq_norm_ * residuals * (residuals.transpose() * jacobians[i]));
        }
        //同理 残差也要乘以尺度因子
        residuals *= residual_scaling_;
    }
}

该函数实际实现的功能是计算出和边缘化相关的参数块的残差和雅克比矩阵，并且把存在核函数情况也进行了处理。

然后回到preMarginalize()函数，进行完残差和雅克比计算后，需要将所有参数块备份到parameter_block_data中，预处理函数就结束了。

接下来进入marginalize()函数：

该函数正式进行H矩阵的构建。先计算出待边缘化参数块的维度大小m和边缘化相关的其他参数块大小n。然后通过多线程加速往H矩阵和g矩阵中添加参数块，最后利用舒尔补进行边缘化计算。

H等于雅克比矩阵的转置乘以雅克比，而且H矩阵是一个对称矩阵，因此求出H矩阵的下三角矩阵后上三角根据转置即可得到。

void*  ThreadsConstructA(void* threadsstruct)
{
    ThreadsStruct* p = ((ThreadsStruct*)threadsstruct); //先把void指针恢复成ThreadsStruct
    for (auto it : p->sub_factors)//遍历所有残差块
    {
        for (int i = 0; i < static_cast<int>(it->parameter_blocks.size()); i++)
        {
            int idx_i = p->parameter_block_idx[reinterpret_cast<long>(it->parameter_blocks[i])];//待边缘化参数块
            int size_i = p->parameter_block_size[reinterpret_cast<long>(it->parameter_blocks[i])];//参数块的大小
            if (size_i == 7)
                size_i = 6;
            Eigen::MatrixXd jacobian_i = it->jacobians[i].leftCols(size_i);
            //遍历每一个参数块 构建大H矩阵 具体是雅克比转置乘以雅克比
            for (int j = i; j < static_cast<int>(it->parameter_blocks.size()); j++)
            {
                int idx_j = p->parameter_block_idx[reinterpret_cast<long>(it->parameter_blocks[j])];
                int size_j = p->parameter_block_size[reinterpret_cast<long>(it->parameter_blocks[j])];
                if (size_j == 7)
                    size_j = 6;
                Eigen::MatrixXd jacobian_j = it->jacobians[j].leftCols(size_j);
                if (i == j)
                    p->A.block(idx_i, idx_j, size_i, size_j) += jacobian_i.transpose() * jacobian_j;
                else
                {
                    p->A.block(idx_i, idx_j, size_i, size_j) += jacobian_i.transpose() * jacobian_j;
                    p->A.block(idx_j, idx_i, size_j, size_i) = p->A.block(idx_i, idx_j, size_i, size_j).transpose();
                }
            }
            p->b.segment(idx_i, size_i) += jacobian_i.transpose() * it->residuals;
        }
    }
    return threadsstruct;
}

根据上面代码可以看到，g矩阵是雅克比转置乘以残差得到，和14讲的高斯牛顿计算时候相差一个负号，这是因为在后面构成新的H'*deltax=g'时，进行分解得到新的雅克比和残差，计算那个残差时候也没有加负号。至此，H矩阵构建完成，如果求解H deltax = g，那么就可以通过求解deltax得到增量，叠加到初值x上完成一次优化迭代，经过几次循环即可得到手写VIO后端。

舒尔补边缘化实现

得到H矩阵和g矩阵，就可以通过上面舒尔补部分内容进行边缘化操作。

//构建Amm是舒尔补推导中的左上角待边缘化参数块 保证其正定性
    Eigen::MatrixXd Amm = 0.5 * (A.block(0, 0, m, m) + A.block(0, 0, m, m).transpose());
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes(Amm);//特征值分解

    //ROS_ASSERT_MSG(saes.eigenvalues().minCoeff() >= -1e-4, "min eigenvalue %f", saes.eigenvalues().minCoeff());
    //利用特征值分解求取逆矩阵
    Eigen::MatrixXd Amm_inv = saes.eigenvectors() * 
    Eigen::VectorXd((saes.eigenvalues().array() > eps).select(saes.eigenvalues().array().inverse(), 0)).asDiagonal() * 
    saes.eigenvectors().transpose();
    //printf("error1: %f\n", (Amm * Amm_inv - Eigen::MatrixXd::Identity(m, m)).sum());

    Eigen::VectorXd bmm = b.segment(0, m);
    Eigen::MatrixXd Amr = A.block(0, m, m, n);
    Eigen::MatrixXd Arm = A.block(m, 0, n, m);
    Eigen::MatrixXd Arr = A.block(m, m, n, n);
    Eigen::VectorXd brr = b.segment(m, n);
    A = Arr - Arm * Amm_inv * Amr;
    b = brr - Arm * Amm_inv * bmm;//构成边缘化后的非齐次方程 新的Hx=g 

    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes2(A);//对新的H特征值分解 目的是求出新的H的雅克比 H = J.transpose() * J
    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;

在舒尔补中需要保证H矩阵的左上角Lamda_a矩阵要正定，因此第一步处理就是保证其正定性，这里的操作本人不是十分理解，A（H）本来是对称的，那么左上角的方阵块也是对称的，0.5倍乘以这个方阵块加方阵块的转置貌似没什么变化。

然后求Lamda_a的逆矩阵，采用的方法是特征值分解，然后根据特征向量的关系得到逆矩阵，然后根据舒尔补计算出新的A、b矩阵。

接下来要对新得到的A矩阵特征值分解：

$A\alpha =\lambda \alpha$

$A= \alpha\lambda \alpha ^{-1}=J^{T}J$

$J=\sqrt{\lambda }\alpha ^{T}$

根据上式分解，J矩阵就是由特征值构成的矩阵开根号乘以特征向量矩阵的逆（正交矩阵也是转置），根据牛顿高斯公式可以得到残差：

$JJ^{T}\delta x=-J^{T}e$

$H\delta x=g$

$e=-(J^{T})^{-1}g=-(\alpha \sqrt{\lambda }) ^{-1}g=-(\sqrt{\frac{1}{\lambda }}\alpha ^{T})g$

在代码公式中计算残差时候没有负号，是因为在构建H*deltax=g的时候就没有负号，其他代码和上述公式计算一致。

marginalize()函数执行完成后，将所有边缘化剩余的参数块存进last_marginalization_parameter_blocks里，将边缘化操作的所有信息存进last_marginalization_info里，为下次构建优化问题做准备。

上面讲的所有内容都是在次新帧是关键帧前提下进行的操作，当次新帧不是关键帧时候进行的处理和上面略有不同。

次新帧是非关键帧

次新帧是非关键帧需要找到上次边缘化对象中相关参数块是否包含即将被边缘化掉的次新帧相关位姿。只有当次新帧和上次被边缘化掉的图像帧有共视关系才对相应的参数块进行处理。

在进行处理时，主要是把包含次新帧位姿的相关参数块用舒尔补边缘化掉，构建新的约束，和上面介绍的次新帧是关键帧情况相似。区别是次新帧是关键帧需要把第0帧的位姿、速度bias、第0帧观测到的3d点通过舒尔补边缘化得到新的J和e；次新帧不是关键帧把上次边缘化信息中的包含次新帧位姿的参数块边缘化掉，得到新的J和e。

在这个过程中需要定义一个边缘化的factor，类似于imu和视觉重投影的factor。

class MarginalizationFactor : public ceres::CostFunction
{
  public:
    MarginalizationFactor(MarginalizationInfo* _marginalization_info);
    virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const;

    MarginalizationInfo* marginalization_info;
};

MarginalizationFactor::MarginalizationFactor(MarginalizationInfo* _marginalization_info):marginalization_info(_marginalization_info)
{
    int cnt = 0;
    for (auto it : marginalization_info->keep_block_size)
    {
        mutable_parameter_block_sizes()->push_back(it);//手动确定参数块大小（保留下来的参数块）
        cnt += it;
    }
    //printf("residual size: %d, %d\n", cnt, n);
    set_num_residuals(marginalization_info->n);//确定参数块维数（留下来的）
};

和其他factor不同之处在于，该类继承的是ceres::CostFunction，而不是SizedCostFunction，这说明在编译阶段参数块维度是不可观的，因此确定维数在构造函数中完成。但是一样的是都要重写Evaluate函数，实现多态。而这个函数需要计算出残差和雅克比。

bool MarginalizationFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
    int n = marginalization_info->n;//边缘化剩下的参数块大小
    int m = marginalization_info->m;//待边缘化参数个数
    Eigen::VectorXd dx(n);
    for (int i = 0; i < static_cast<int>(marginalization_info->keep_block_size.size()); i++)//遍历留下来的参数块
    {
        int size = marginalization_info->keep_block_size[i];//留下来的参数块尺寸
        int idx = marginalization_info->keep_block_idx[i] - m;//更新 留下来参数块索引，从0开始
        //这里的x 记录的是parameters传入的是滑窗内去掉边缘化参数块后其他参数块前移得到的新地址指针和大小 就是受到边缘化约束的参数值
        Eigen::VectorXd x = Eigen::Map<const Eigen::VectorXd>(parameters[i], size);
        //x0是
        Eigen::VectorXd x0 = Eigen::Map<const Eigen::VectorXd>(marginalization_info->keep_block_data[i], size);
        if (size != 7)
            dx.segment(idx, size) = x - x0;
        else
        {
            dx.segment<3>(idx + 0) = x.head<3>() - x0.head<3>();
            dx.segment<3>(idx + 3) = 2.0 * Utility::positify(Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * Eigen::Quaterniond(x(6), x(3), x(4), x(5))).vec();
            if (!((Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * Eigen::Quaterniond(x(6), x(3), x(4), x(5))).w() >= 0))
            {
                dx.segment<3>(idx + 3) = 2.0 * -Utility::positify(Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * Eigen::Quaterniond(x(6), x(3), x(4), x(5))).vec();
            }
        }
    }
    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);
            }
        }
    }
    return true;
}

计算残差的基本思想是经过舒尔补后的保留下来的状态量和优化后的相应参数块相差不能太大。得到的残差需要经过雅克比的补偿：

$e=e_{0}+J\delta x$

相当于残差在x0处的线性展开，而且这里的雅可比矩阵是不变的，该问题也成为FEJ问题，作者本人说FEJ对整个系统影响不大，知乎上有大佬关于这个问题做详细分析，可以参考以下两篇文章：

如何理解SLAM中的First-Estimates Jacobian？ - 知乎

滑窗优化、边缘化、舒尔补、FEJ及fill-in问题_fej算法-CSDN博客

现在已经梳理完成次新帧是关键帧和不是关键帧两种情况，然而还没有涉及到如何将边缘化作为约束添加到problem对象中。

将边缘化约束添加到后端优化中

有了上面的梳理，把边缘化约束添加到残差块中就是手到擒来的小case了：

//添加边缘化约束
    if (last_marginalization_info)//等价于if（last_marginalization_info ！= NULL）
    {
        // construct new marginlization_factor
        MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
        problem.AddResidualBlock(marginalization_factor, NULL,
                                 last_marginalization_parameter_blocks);
    }

这里的factor就是次新帧不是关键帧中的factor，添加残差块信息和预积分约束、视觉重投影约束一样了。至此，关于滑窗优化的全部内容已经梳理完成。

滑动窗口边缘化总结

在后端优化中，imu预积分约束和视觉重投影约束相对比较容易理解，即使计算雅克比相对复杂，但是也都是直接调用一些ceres的接口函数。为了保证后端优化数据量合理，vins-mono使用滑动窗口边缘化处理。边缘化的核心原理就是舒尔补，舒尔补将被边缘化掉的参数块信息整合到留下的被形成约束的参数块中，进而得到新的误差和雅克比表达形式。边缘化对于次新帧是否是关键帧有两种不同的处理，是关键帧将第0帧的位姿、速度bias、第1帧预积分和第0帧观测到的地图点边缘化掉；次新帧不是关键帧，需要把上一次构建的边缘化对象中的和次新帧形成共视的相关约束的参数块边缘化掉。而最终把边缘化约束添加到整个优化问题对象的残差块中，也就完成了整个边缘化。

在边缘化处理好之后，前端里程计最后还需要一步处理——滑窗。这个滑窗只是把数据位置交换和一些管理权的交接，在下篇文章中将会详细介绍。欢迎点赞、关注、交流。