VINS-Mono代码阅读笔记（十）：vins_estimator中的非线性优化

最新推荐文章于 2024-05-03 15:36:17 发布

文科升

最新推荐文章于 2024-05-03 15:36:17 发布

阅读量4.2k

点赞数 9

分类专栏： SLAM 文章标签： VINS-Mono slam

本文链接：https://blog.csdn.net/moyu123456789/article/details/103582051

版权

SLAM 专栏收录该内容

35 篇文章 90 订阅

订阅专栏

本篇笔记紧接着VINS-Mono代码阅读笔记（九）：vins_estimator中的相机-IMU对齐，初始化完成之后就获得了要优化的变量的初始值，后边就是做后端优化处理了。这部分对应论文中第VI部分，紧耦合的单目VIO，这部分也是该系统的核心部分。

1.前言

在我们对经典的视觉SLAM框架和现有的知识一无所知的情况下，我们看到搭载有相机的移动设备（无人车或者无人机等）从A地点运动到B地点，我们能通过移动设备上的相机看到整个运动过程中出现在相机镜头中的景物，这样就形成了一帧一帧的图片。怎样从这些连续的图片中估计出移动设备运动的轨迹，这就是视觉SLAM要做的事情。

怎样把两张图片联系起来呢？三维世界中的一个个物体称之为路标点，如果拍摄的两张图像中出现了同一棵树，那么这两张图片就能建立联系，通过树在两张图片中的位置变换就能判断出相机的移动方向。相关的理论被发展为视觉里程计。简单来说，就是通过在一帧帧的图像中提取特征点，并对这些特征点进行匹配跟踪，综合利用对极几何、三角测量、PnP等知识估计出相机的位姿从而得到运动轨迹。

由于受传感器测量误差的影响、前端SLAM处理误差等多个因素的影响，误差是难以避免的，这就产生了“噪声”。后端优化则是对视觉里程计中估计出的相机位姿进行“去噪处理”，也就是对前端视觉里程计估计出的位姿信息进行修正，使其更加精确。后端优化的方法使用比较广泛的是滤波器和非线性优化。SLAM中常用的非线性优化算法学习笔记是笔者之前学习优化算法时做的一篇笔记。

VINS-Mono论文中提到紧耦合，那么紧耦合和松耦合（低耦合）是什么意思呢？

相信做软件开发的人都知道软件设计里对系统“高内聚，低耦合”的设计理念。什么意思呢？简单来说就是软件中相互独立的模块（或者各个类）尽量功能“内聚”起来，比如一个函数只做一件事情，一个类只做和其本身相关的事情。低耦合则是从业务层面来说的，一个模块只做一个业务，过多的业务相互掺杂堆在一起不仅软件架构不够清晰，对后期代码的维护来说也带来了很大难度。

论文中提到的紧耦合（TIGHTLY COUPLED）的非线性优化指的是将视觉约束也加入到非线性优化当中一块进行优化。对应的松耦合的非线性优化就是将视觉约束后计算出的位姿加入到非线性优化中来进行优化。

2.理论推导分析

回忆SLAM中常用的非线性优化算法学习笔记我们知道，在非线性问题中直接对优化变量进行求解是困难的，因此总体的求解思路是将非线性问题转化为线性问题，对优化变量求解其增量的方式来进行求解。无论是高斯牛顿还是列文伯格-马夸尔特方法，最终要求解的增量方程形式为：

$\large H\Delta x=g$

这个也就是十四讲中所说的求解增量方程是整个优化问题的核心所在。这里的 $\large H$ 是有一定意义的，在高斯牛顿法中 $\large H=J^{T}J$ ，而在列文伯格-马夸尔特方法中 $\large H=J^{T}J+\lambda I$ 。

对于以上增量方程，只简单从形式上来看的话，到了这里就能很容易求解出 $\large \Delta x=H^{-1}g$ ，那么问题就转变为对 $\large H$ 进行求逆了，好在研究者们发现了 $\large H$ 矩阵的稀疏性，这大大简化了问题的求解。

实际求解中有多种利用 $\large H$ 矩阵稀疏性加速计算的方法，这里简述一下Schur消元法，也就是边缘化（Marginalization）求解的方法。

根据 $\large H$ 矩阵的形式，对 $\large H\Delta x=g$ 方程变形如下：

$\large \begin{bmatrix} B & E\\ E^{T}&C \end{bmatrix}\begin{bmatrix} \Delta x_c\\ \Delta x_p \end{bmatrix}=\begin{bmatrix} v\\ w \end{bmatrix}$

其中 $\large \Delta x_c$ 为相机位置的增量， $\large \Delta x_p$ 为路标点的增量， $\large B$ 和 $\large C$ 都为对角块矩阵。

那么采用消去 $\large E$ 的方法来求解，得：

$\large \begin{bmatrix} I & -EC^{-1}\\ 0 & I \end{bmatrix}\begin{bmatrix} B &E \\ E^{T} &C \end{bmatrix}\begin{bmatrix} \Delta x_c\\ \Delta x_p \end{bmatrix}=\begin{bmatrix} I & -EC^{-1}\\ 0 & I \end{bmatrix}\begin{bmatrix} v\\ w \end{bmatrix}$

$\large \Rightarrow$ $\large \begin{bmatrix} B-EC^{-1}E^{T} & 0\\ E^{T}& C \end{bmatrix}\begin{bmatrix} \Delta x_c\\ \Delta x_p \end{bmatrix}=\begin{bmatrix} v-EC^{-1}w\\ w \end{bmatrix}$

整理后把第一行和 $\large \Delta x_p$ 无关的行拿出来： $\large \left [ B-EC^{-1}E^{T} \right ]\Delta x_c=v-EC^{-1}w.$ 从该方程中求解出 $\large \Delta x_c$ 然后带进原方程中求解 $\large \Delta x_p$

具体可详细阅读十四讲中的第十章相关内容。

以上是非线性优化的一些预备知识，在VINS-Mono系统的非线性优化中都有使用。下面结合论文中第VI部分来看VINS-Mono中的非线性优化的理论部分，首先要清楚优化的变量是什么？目标函数（也就是最小二乘法）表达式是什么。

1）要优化的变量

滑动窗口中所有的状态向量定义如下：

$\large \chi =[x_0,x_1,...x_n,x_c^b,\lambda _0,\lambda _1,...\lambda _m]$

其中 $\large x_k$ 和 $\large x_c^b$ 定义如下：

$\large x_k=[p_{b_k}^w,v_{b_k}^w,q_{b_k}^w,b_a,b_g],k\in [0,n]$

$\large x_c^b=[p_c^b,q_c^b]$

这里 $\large x_k$ 是在获取第 $\large k$ 个图像的时候IMU的状态，包括位置、速度、IMU在世界坐标系中的方向、在IMU坐标系中的加速度偏差和陀螺仪偏差。

$\large n$ 为关键帧的个数， $\large m$ 是在滑动窗口中特征的总数。 $\large \lambda _l$ 是第 $\large l$ 个特征的逆深度，该逆深度来自第一个观测帧。

2）要求解的目标函数

目标函数表达式如下：

$\large \underset{\chi }{min}\left \{ \left \| r_p-H_p\chi \right \|^{2}+\sum_{k\in \ss }\left \| r_{\ss}(\hat{z}_{b_{k+1}}^{b_k},\chi ) \right \|^{2}_{P_{b_{k+1}}^{b_k}}+\sum_{(l,j)\in C}\rho \left ( \left \| r_C(\hat{z}_{l}^{C_j},\chi ) \right \|_{P_{l}^{c_j}}^2 \right ) \right \}$

以上表达式中三种残差所计算的距离都是马哈拉诺比斯距离（Mahalanobis norm），目标函数表达式中变量说明如下：

（1）IMU残差： $\large r_{\ss }(\hat{z}_{b_{k+1}}^{b_k},\chi )$ 为要求解的IMU残差。

（2）视觉残差： $\large r_C(\hat{z}_l^{c_j},\chi )$ 为要求解的视觉残差。

（3）这里的 $\large \rho$ 是Huber损失函数，是一种衡量误差的方法，定义如下：

$\large \rho (s)=\left\{\begin{matrix} s & s\leq 1\\ 2\sqrt{s}-1 & s>1. \end{matrix}\right.$

（4）目标函数公式中的 $\large \ss$ 是所有IMU测量的集合， $\large C$ 是在当前的滑动窗口中至少被观测过两次的特征的集合。

（5） $\large \left \{ r_p,H_p \right \}$ 是来自边缘化的先验信息。

代码中使用Google开源的Ceres solver来求解该非线性问题。

3）IMU测量残差

在滑动窗口中两个连续的帧 $\large b_k$ 和 $\large b_{k+1}$ ，它们的IMU测量值的预积分残差定义如下：

$\large r_{\ss} (\hat{z}_{b_{k+1}}^{b_k},\chi )=\begin{bmatrix} \delta \alpha _{b_{k+1}}^{b_k}\\ \delta \beta _{b_{k+1}}^{b_k}\\ \delta \theta _{b_{k+1}}^{b_k}\\ \delta b_a\\ \delta b_g \end{bmatrix}=\begin{bmatrix} R_w^{b_k}(p_{b_{k+1}}^w-p_{b_k}^{w}+\frac{1}{2}g^w\Delta t_k^2-v_{b_k}^w\Delta t_k)-\hat{\alpha }_{b_{k+1}}^{b_k}\\ R_w^{b_k}(v_{b_{k+1}}^w+g^w\Delta t_k-v_{b_k}^w)-\hat{\beta }_{b_{k+1}}^{b_k}\\ 2[q_{b_k}^{w^{-1}}\bigotimes q_{b_{k+1}}^w\bigotimes (\hat{\gamma }_{b_{k+1}}^{b_k})^{-1}]_{xyz}\\ b_{a_{b_{k+1}}}-b_{a_{b_k}}\\ b_{w_{b_{k+1}}}-b_{w_{b_k}} \end{bmatrix}$

（1）其中前3项是IMU预积分残差，后边两项分别是加速度计偏差和陀螺仪偏差。

（2） $\large [.]_{xyz}$ 是四元数 $\large q$ 中的向量部分，表示误差状态。

（3） $\large \delta \theta _{b_{k+1}}^{b_k}$ 是四元数3-D误差状态的表示。

（4）IMU测量残差定义对应代码中的IntegrationBase::evaluate函数。

4）视觉测量残差

先说明一下视觉残差是怎样一回事。视觉残差其实就是重投影误差，求解的是观测值和估计值之间的误差。假设世界坐标系中有一个路标点P，当相机第一次观测到该路标点时会在相机的归一化平面中产生该路标点的相机归一化坐标，第 $\large l$ 次相机观测到这个路标点时会在相机归一化平面中产生这个路标点的归一化坐标。那么，认为第 $\large l$ 次相机拍摄到该路标点时产生的归一化坐标为观测值，同时通过旋转平移等计算将第一次的观测值从第一帧的坐标系下转到第 $\large l$ 次的坐标系下，这样就产生了一个估计值。

和传统的针孔相机模型将重投影误差定义在一个通用平面上不同，VINS-Mono中将相机测量残差定义在一个单位球面上。假设第 $\large l$ 个特征第一次被第 $\large i$ 个图像帧观察到，被第 $\large j$ 个图像帧观察到的特征的残差定义如下：

$\large r_C\left ( \hat{z}_l^{c_j},\chi \right )=[b_1,b_2]^T\cdot \left ( \hat{\bar{P}}_l^{c_j}-\frac{P_{l}^{c_j}}{\left \| P_{l}^{c_j} \right \|} \right )$

$\large \hat{\bar{P}}_l^{c_j}=\pi _c^{-1}\left ( \begin{bmatrix} \hat{u}_l^{c_j}\\ \hat{v}_l^{c_j} \end{bmatrix} \right )$

$\large P_l^{c_j}=R_b^c(R_w^{b_j}(R_{b_i}^w(R_c^b\frac{1}{\lambda_l }\pi _c^{-1}(\begin{bmatrix} \hat{u}_l^{c_i}\\ \hat{v}_l^{c_i} \end{bmatrix})+p_c^b)+p_{b_i}^w-p_{b_j}^w)-p_c^b)$

（1） $\large \begin{bmatrix} \hat{u}_l^{c_i}\\ \hat{v}_l^{c_i} \end{bmatrix}$ 是在第 $\large i$ 个图像帧中观测到的第 $\large l$ 个特征的像素坐标， $\large \begin{bmatrix} \hat{u}_l^{c_j}\\ \hat{v}_l^{c_j} \end{bmatrix}$ 是在第 $\large j$ 个图像帧中观测到的同一个特征的像素坐标；

（2） $\large \pi _c^{-1}$ 是反投影函数，它使用相机的内参将像素坐标转成单位向量；

（3）视觉残差的自由度是二，所以将残差向量投影到一个切平面上，如下图所示， $\large b_1$ 和 $\large b_2$ 是在切平面 $\large \hat{\bar{P}}_l^{c_j}$ 上的两个随机选择的正交基。

（4）视觉残差定义对应代码中的ProjectionFactor::Evaluate函数，稍有差别的是代码中是

$\large \frac{P_{l}^{c_j}}{\left \| P_{l}^{c_j} \right \|}-\hat{\bar{P}}_l^{c_j}$ ，不过没关系，最终都要进行平方计算的，所以哪个减哪个无所谓。

5）边缘化

VINS中引用边缘化是为了限制运算复杂度不再一直增加。选择性的从滑动窗口中边缘化掉IMU状态 $\large x_k$ 和特征点 $\large \lambda _l$ ，同时将对应的边缘化的状态转换为先验。这个其实比较好理解，当相机一直在拍摄的时候，就会持续产生一帧一帧的图像进入滑动窗口中，而为了限制系统的计算复杂度而设置了滑动窗口的大小，那么当新的图像帧进入滑动窗口中的时候，为了保持滑动窗口大小的不变，就需要一种策略来删除一些帧。

边缘化的策略如论文中下图所示：

当次新帧是关键帧的时候，就将次新帧保留在滑动窗口中，同时将滑动窗口中最旧的帧和与其相关的视觉和惯导测量值边缘化出去。边缘化的测量值转为先验。

当次新帧不是关键帧的时候，就将该次新帧和其对应的视觉测量移出滑动窗口，并且保留该非关键帧的预积分惯导测量，同时预积分进程继续进行从而测量出下一帧的值。

对应代码为Estimator::optimization函数。

3.代码阅读分析

1）后端优化函数Estimator::optimization代码

/**
 * 进行位姿优化
*/
void Estimator::optimization()
{
    //1.构建最小二乘问题
    ceres::Problem problem;
    //2.创建LossFunction对象，lossfunction用来减小Outlier的影响
    ceres::LossFunction *loss_function;
    //loss_function = new ceres::HuberLoss(1.0);
    //class CauchyLoss is ρ(s)=log(1+s)
    loss_function = new ceres::CauchyLoss(1.0);
    //3.添加优化变量参数块
    //添加要优化的变量：相机位姿、速度、加速度偏差、陀螺仪偏差
    for (int i = 0; i < WINDOW_SIZE + 1; i++)
    {
        ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
        //para_Pose[i]中存放的是滑动窗口中第i帧的位姿，para_Pose[i]的大小为SIZE_POSE(值为7)
        problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);
        //SIZE_SPEEDBIAS值为9
        problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);
    }
    //添加要优化的变量：相机到IMU的外参
    for (int i = 0; i < NUM_OF_CAM; i++)
    {
        ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
        problem.AddParameterBlock(para_Ex_Pose[i], SIZE_POSE, local_parameterization);
        if (!ESTIMATE_EXTRINSIC)
        {
            ROS_DEBUG("fix extinsic param");
            problem.SetParameterBlockConstant(para_Ex_Pose[i]);
        }
        else
            ROS_DEBUG("estimate extinsic param");
    }
    //添加要优化的变量：时间偏差
    if (ESTIMATE_TD)
    {
        problem.AddParameterBlock(para_Td[0], 1);
        //problem.SetParameterBlockConstant(para_Td[0]);
    }

    TicToc t_whole, t_prepare;
    //将要优化的变量转为数组形式
    vector2double();
    //4.添加残差块
    //添加边缘化的先验残差信息，第一次进行优化的时候last_marginalization_info还为空值
    if (last_marginalization_info)
    {
        // construct new marginlization_factor
        /**
         * AddResidualBlock函数中第一个参数是costfunction，第二个参数是lossfunction，第三个参数是参数块
         * */
        MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
        problem.AddResidualBlock(marginalization_factor, NULL,
                                 last_marginalization_parameter_blocks);
    }
    //添加IMU测量值残差
    for (int i = 0; i < WINDOW_SIZE; i++)
    {
        int j = i + 1;
        if (pre_integrations[j]->sum_dt > 10.0)
            continue;
        IMUFactor* imu_factor = new IMUFactor(pre_integrations[j]);
        problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);
    }
    int f_m_cnt = 0;
    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;
        //第一个观测到该特征的帧对应的特征点坐标
        Vector3d pts_i = it_per_id.feature_per_frame[0].point;
        //遍历能观测到该特征的每个帧
        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;
            //ESTIMATE_TD:online estimate time offset between camera and imu
            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());
                    problem.AddResidualBlock(f_td, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]);
                    /*
                    double **para = new double *[5];
                    para[0] = para_Pose[imu_i];
                    para[1] = para_Pose[imu_j];
                    para[2] = para_Ex_Pose[0];
                    para[3] = para_Feature[feature_index];
                    para[4] = para_Td[0];
                    f_td->check(para);
                    */
            }
            else
            {
                ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
                problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]);
            }
            f_m_cnt++;
        }
    }

    ROS_DEBUG("visual measurement count: %d", f_m_cnt);
    ROS_DEBUG("prepare for ceres: %f", t_prepare.toc());
    //重定位残差
    if(relocalization_info)
    {
        //printf("set relocalization factor! \n");
        ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
        problem.AddParameterBlock(relo_Pose, SIZE_POSE, local_parameterization);
        int retrive_feature_index = 0;
        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 start = it_per_id.start_frame;
            if(start <= relo_frame_local_index)
            {  
                while((int)match_points[retrive_feature_index].z() < it_per_id.feature_id)
                {
                    retrive_feature_index++;
                }
                if((int)match_points[retrive_feature_index].z() == it_per_id.feature_id)
                {
                    Vector3d pts_j = Vector3d(match_points[retrive_feature_index].x(), match_points[retrive_feature_index].y(), 1.0);
                    Vector3d pts_i = it_per_id.feature_per_frame[0].point;
                    
                    ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
                    problem.AddResidualBlock(f, loss_function, para_Pose[start], relo_Pose, para_Ex_Pose[0], para_Feature[feature_index]);
                    retrive_feature_index++;
                }     
            }
        }

    }
    //5.创建优化求解器
    ceres::Solver::Options options;
    //6.设定优化器的solver_type类型
    options.linear_solver_type = ceres::DENSE_SCHUR;
    //options.num_threads = 2;
    //7.设定优化使用的算法。这里使用置信域类优化算法中的dogleg方法
    options.trust_region_strategy_type = ceres::DOGLEG;
    //8.设置迭代求解的次数，这个在配置文件中设置
    options.max_num_iterations = NUM_ITERATIONS;
    //options.use_explicit_schur_complement = true;
    //options.minimizer_progress_to_stdout = true;//输出到cout中
    //options.use_nonmonotonic_steps = true;
    if (marginalization_flag == MARGIN_OLD)
        options.max_solver_time_in_seconds = SOLVER_TIME * 4.0 / 5.0;
    else
        options.max_solver_time_in_seconds = SOLVER_TIME;
    TicToc t_solver;
    ceres::Solver::Summary summary;//优化信息
    //9.开始求解
    ceres::Solve(options, &problem, &summary);
    //cout << summary.BriefReport() << endl;
    ROS_DEBUG("Iterations : %d", static_cast<int>(summary.iterations.size()));
    ROS_DEBUG("solver costs: %f", t_solver.toc());
    //求解完成后，将数组转化为向量
    double2vector();

    TicToc t_whole_marginalization;
    //判断边缘化标志
    if (marginalization_flag == MARGIN_OLD)
    {
        MarginalizationInfo *marginalization_info = new MarginalizationInfo();
        vector2double();

        if (last_marginalization_info)
        {
            vector<int> drop_set;
            for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++)
            {
                if (last_marginalization_parameter_blocks[i] == para_Pose[0] ||
                    last_marginalization_parameter_blocks[i] == para_SpeedBias[0])
                    drop_set.push_back(i);
            }
            // construct new marginlization_factor
            MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
            ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(marginalization_factor, NULL,
                                                                           last_marginalization_parameter_blocks,
                                                                           drop_set);

            marginalization_info->addResidualBlockInfo(residual_block_info);
        }

        {
            if (pre_integrations[1]->sum_dt < 10.0)
            {
                IMUFactor* imu_factor = new IMUFactor(pre_integrations[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});
                marginalization_info->addResidualBlockInfo(residual_block_info);
            }
        }

        {
            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;
                if (imu_i != 0)
                    continue;
                //当前特征所在的第一个观测帧中观测的点坐标
                Vector3d pts_i = it_per_id.feature_per_frame[0].point;
                //遍历当前特征的观测帧
                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);
                    }
                }
            }
        }

        TicToc t_pre_margin;
        marginalization_info->preMarginalize();
        ROS_DEBUG("pre marginalization %f ms", t_pre_margin.toc());
        
        TicToc t_margin;
        marginalization_info->marginalize();
        ROS_DEBUG("marginalization %f ms", t_margin.toc());

        std::unordered_map<long, double *> addr_shift;
        for (int i = 1; i <= WINDOW_SIZE; i++)
        {
            addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i - 1];
            addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i - 1];
        }
        for (int i = 0; i < NUM_OF_CAM; i++)
            addr_shift[reinterpret_cast<long>(para_Ex_Pose[i])] = para_Ex_Pose[i];
        if (ESTIMATE_TD)
        {
            addr_shift[reinterpret_cast<long>(para_Td[0])] = para_Td[0];
        }
        vector<double *> parameter_blocks = marginalization_info->getParameterBlocks(addr_shift);

        if (last_marginalization_info)
            delete last_marginalization_info;
        //更新last_marginalization_info
        last_marginalization_info = marginalization_info;
        last_marginalization_parameter_blocks = parameter_blocks;
        
    }//end of if (marginalization_flag == MARGIN_OLD)
    else
    {
        /**
         * 当次新帧不是关键帧时，直接剪切掉次新帧和它的视觉观测边（该帧和路标点之间的关联），而不对次新帧进行marginalize处理
         * 但是要保留次新帧的IMU数据，从而保证IMU预积分的连续性，这样才能积分计算出下一帧的测量值
         * */
        if (last_marginalization_info &&
            std::count(std::begin(last_marginalization_parameter_blocks), std::end(last_marginalization_parameter_blocks), para_Pose[WINDOW_SIZE - 1]))
        {

            MarginalizationInfo *marginalization_info = new MarginalizationInfo();
            vector2double();
            if (last_marginalization_info)
            {
                vector<int> drop_set;
                for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++)
                {
                    ROS_ASSERT(last_marginalization_parameter_blocks[i] != para_SpeedBias[WINDOW_SIZE - 1]);
                    if (last_marginalization_parameter_blocks[i] == para_Pose[WINDOW_SIZE - 1])
                        drop_set.push_back(i);
                }
                // construct new marginlization_factor
                MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
                ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(marginalization_factor, NULL,
                                                                               last_marginalization_parameter_blocks,
                                                                               drop_set);

                marginalization_info->addResidualBlockInfo(residual_block_info);
            }

            TicToc t_pre_margin;
            ROS_DEBUG("begin marginalization");
            //计算每次IMU和视觉观测(cost_function)对应的参数块(parameter_blocks),雅可比矩阵(jacobians),残差值(residuals)
            marginalization_info->preMarginalize();
            ROS_DEBUG("end pre marginalization, %f ms", t_pre_margin.toc());

            TicToc t_margin;
            ROS_DEBUG("begin marginalization");
            //多线程计算整个先验项的参数块,雅可比矩阵和残差值
            marginalization_info->marginalize();
            ROS_DEBUG("end marginalization, %f ms", t_margin.toc());
            
            std::unordered_map<long, double *> addr_shift;
            for (int i = 0; i <= WINDOW_SIZE; i++)
            {
                if (i == WINDOW_SIZE - 1)
                    continue;
                else if (i == WINDOW_SIZE)
                {
                    addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i - 1];
                    addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i - 1];
                }
                else
                {
                    addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i];
                    addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i];
                }
            }
            for (int i = 0; i < NUM_OF_CAM; i++)
                addr_shift[reinterpret_cast<long>(para_Ex_Pose[i])] = para_Ex_Pose[i];
            if (ESTIMATE_TD)
            {
                addr_shift[reinterpret_cast<long>(para_Td[0])] = para_Td[0];
            }
            
            vector<double *> parameter_blocks = marginalization_info->getParameterBlocks(addr_shift);
            if (last_marginalization_info)
                delete last_marginalization_info;
            last_marginalization_info = marginalization_info;
            last_marginalization_parameter_blocks = parameter_blocks;
            
        }
    }
    ROS_DEBUG("whole marginalization costs: %f", t_whole_marginalization.toc());
    
    ROS_DEBUG("whole time for ceres: %f", t_whole.toc());
}

2）IMU残差约束定义函数

/**
     * 定义IMU的残差约束
     * const Eigen::Vector3d &Pi, //第i帧位置
     * const Eigen::Quaterniond &Qi, //第i帧姿态
     * const Eigen::Vector3d &Vi, //第i帧速度
     * const Eigen::Vector3d &Bai, //第i帧加速度偏差
     * const Eigen::Vector3d &Bgi, //第i帧角速度偏差
     * const Eigen::Vector3d &Pj, //第j帧位置
     * const Eigen::Quaterniond &Qj, //第j帧姿态
     * const Eigen::Vector3d &Vj, //第j帧速度
     * const Eigen::Vector3d &Baj, //第j帧加速度偏差
     * const Eigen::Vector3d &Bgj//第j帧角速度偏差
    */
    Eigen::Matrix<double, 15, 1> evaluate(const Eigen::Vector3d &Pi, const Eigen::Quaterniond &Qi, const Eigen::Vector3d &Vi, const Eigen::Vector3d &Bai, const Eigen::Vector3d &Bgi,
                                          const Eigen::Vector3d &Pj, const Eigen::Quaterniond &Qj, const Eigen::Vector3d &Vj, const Eigen::Vector3d &Baj, const Eigen::Vector3d &Bgj)
    {
        Eigen::Matrix<double, 15, 1> residuals;

        Eigen::Matrix3d dp_dba = jacobian.block<3, 3>(O_P, O_BA);
        Eigen::Matrix3d dp_dbg = jacobian.block<3, 3>(O_P, O_BG);

        Eigen::Matrix3d dq_dbg = jacobian.block<3, 3>(O_R, O_BG);

        Eigen::Matrix3d dv_dba = jacobian.block<3, 3>(O_V, O_BA);
        Eigen::Matrix3d dv_dbg = jacobian.block<3, 3>(O_V, O_BG);

        Eigen::Vector3d dba = Bai - linearized_ba;
        Eigen::Vector3d dbg = Bgi - linearized_bg;

        Eigen::Quaterniond corrected_delta_q = delta_q * Utility::deltaQ(dq_dbg * dbg);
        Eigen::Vector3d corrected_delta_v = delta_v + dv_dba * dba + dv_dbg * dbg;
        Eigen::Vector3d corrected_delta_p = delta_p + dp_dba * dba + dp_dbg * dbg;
        /**
         * matrix.block<p,q>(i, j) :<p, q>可理解为一个p行q列的子矩阵，该定义表示从原矩阵中第(i, j)开始，获取一个p行q列的子矩阵，
         * 返回该子矩阵组成的临时矩阵对象，原矩阵的元素不变
         * */
        //位移残差，对应IMU残差公式中的delta alpha_{b_{k+1}}^{b_k}的计算，其中O_P值为0
        residuals.block<3, 1>(O_P, 0) = Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt) - corrected_delta_p;
        //旋转残差，对应IMU残差公式中的delta theta_{b_{k+1}}^{b_k}的计算，其中O_R值为3
        residuals.block<3, 1>(O_R, 0) = 2 * (corrected_delta_q.inverse() * (Qi.inverse() * Qj)).vec();
        //速度残差，对应IMU残差公式中的delta betaO_V值为6
        residuals.block<3, 1>(O_V, 0) = Qi.inverse() * (G * sum_dt + Vj - Vi) - corrected_delta_v;
        //加速度计残差，O_BA值为9
        residuals.block<3, 1>(O_BA, 0) = Baj - Bai;
        //陀螺仪残差，O_BG值为12
        residuals.block<3, 1>(O_BG, 0) = Bgj - Bgi;
        return residuals;
    }

3）视觉残差定义函数

bool ProjectionFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
    TicToc tic_toc;
    Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);
    Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);

    Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]);
    Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]);

    Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]);
    Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);

    double inv_dep_i = parameters[3][0];
    /**
     * 视觉残差计算
    */
    Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i;
    Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;
    Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;
    Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);
    //这里计算出来的pts_camera_j就是P_l^{c_j}
    Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);
    Eigen::Map<Eigen::Vector2d> residual(residuals);
    //球面定义残差
#ifdef UNIT_SPHERE_ERROR 
    residual =  tangent_base * (pts_camera_j.normalized() - pts_j.normalized());
#else
    double dep_j = pts_camera_j.z();
    residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>();
#endif

    residual = sqrt_info * residual;

4）对窗口进行滑动的代码

/**
 * 对窗口进行滑动
*/
void Estimator::slideWindow()
{
    TicToc t_margin;
    //从滑动窗口中删除掉最旧的帧
    if (marginalization_flag == MARGIN_OLD)
    {
        //获取第一帧对应的时间戳
        double t_0 = Headers[0].stamp.toSec();
        //获得滑动窗口中第一帧图像对应的旋转
        back_R0 = Rs[0];
        //获得滑动窗口中第一帧图像对应的位置
        back_P0 = Ps[0];
        if (frame_count == WINDOW_SIZE)
        {
            //交换滑动窗口中的位姿、速度、陀螺仪偏差、加速度buf、角速度buf等的位置
            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]);
            }
            /**
             * 执行完上边的for循环后，滑动窗口中最旧的帧已经被交换到对应状态数组的最后位置了
             * 下面将WINDOW_SIZE-1上的值赋值给WINDOW_SIZE，相当于删掉了最旧的帧的数据
            */
            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];
            //删除掉pre_integrations数组中最后一个元素，相当于删除了最旧帧对应的预积分器
            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();

            if (true || solver_flag == INITIAL)
            {
                map<double, ImageFrame>::iterator it_0;
                //在all_image_frame中找到第一帧图像对应的数据
                it_0 = all_image_frame.find(t_0);
                //删除第一帧图像对应的预积分器
                delete it_0->second.pre_integration;
                it_0->second.pre_integration = nullptr;
                //删除all_image_frame当中第一帧之外的其他帧对应的预积分器
                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这个map中删除掉t_0项元素
                all_image_frame.erase(t_0);

            }
            slideWindowOld();
        }
    }
    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];

                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);
            }
            //用滑动窗口中最新的帧的状态替换掉次新的帧的状态
            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];
            //删除掉滑动窗口中最后一个预积分器
            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();
        }
    }
}

// real marginalization is removed in solve_ceres()
void Estimator::slideWindowNew()
{
    sum_of_front++;
    f_manager.removeFront(frame_count);
}
// real marginalization is removed in solve_ceres()
void Estimator::slideWindowOld()
{
    sum_of_back++;

    bool shift_depth = solver_flag == NON_LINEAR ? true : false;
    if (shift_depth)
    {
        Matrix3d R0, R1;
        Vector3d P0, P1;
        /**
         * back_R0为滑动窗口中第一帧，也是最旧的帧的旋转矩阵
         * ric[0]为相机到IMU之间的旋转
         * 这里计算出来的R0为滑动窗口中未边缘化掉最旧的一帧之前，旧的第一帧图像在IMU坐标系下的旋转
        */
        R0 = back_R0 * ric[0];
        //R1是滑动窗口中边缘化掉最旧的一帧之后，新的第一帧图像在IMU坐标系下的旋转
        R1 = Rs[0] * ric[0];
        //P0为滑动窗口中未边缘化掉最旧的第一帧之前，旧的第一帧图像在IMU坐标系下的位置
        P0 = back_P0 + back_R0 * tic[0];
        //P1为滑动窗口中边缘化掉最旧的第一帧之后，旧的第一帧图像在IMU坐标系下的位置
        P1 = Ps[0] + Rs[0] * tic[0];
        f_manager.removeBackShiftDepth(R0, P0, R1, P1);
    }
    else
        f_manager.removeBack();
}

优化部分是整个系统中比较难理解的一块，笔者学习过程中也参考学习了许多别人分享的笔记。但总觉得还没有把这块完全理解透彻，有问题的地方也请看到的大神指正。后期会不断学习理解这块，有新的理解再进行更新。

VINS-Mono代码阅读笔记（十一）：进入pose_graph节点代码分析