vins-mono无ROS版本阅读（8）后端优化

参考：【SLAM】VINS-MONO解析——后端优化(代码部分) - 古月居

VINS-Mono 代码详细解读——基于滑动窗口的紧耦合后端非线性优化 optimization()+滑动窗slideWindow()_try_again_later的博客-CSDN博客

 VINS-Mono代码精简版代码详解-后端非线性优化(三)_jiweinanyi的博客-CSDN博客

   else//solver_flag = NON_LINEAR,进行非线性优化
    {
        solveOdometry();  //<--1
        //边界判断：检测系统运行是否失败，若失败则重置估计器
        if (failureDetection()) //<--2
        {
            ROS_WARN("failure detection!");
            failure_occur = 1;
            clearState();
            setParameter();
            return;
        }

        slideWindow();//执行窗口滑动函数slideWindow();//<--3
        f_manager.removeFailures();//去除估计失败的点并发布关键点位置//<--4
        // prepare output of VINS
        key_poses.clear();//<--5
        for (int i = 0; i <= WINDOW_SIZE; i++)
            key_poses.push_back(Ps[i]);

        last_R = Rs[WINDOW_SIZE]; //<--6
        last_P = Ps[WINDOW_SIZE];
        last_R0 = Rs[0];
        last_P0 = Ps[0];
    }
}

最重要的两个函数：solveOdometry()和slideWindow()。

1. solveOdometry()

void Estimator::solveOdometry()
{
    if (frame_count < WINDOW_SIZE)
        return;
    if (solver_flag == NON_LINEAR)
    {
        f_manager.triangulate(Ps, tic, ric);//获得最新特征的深度//<--1
        optimization();//vins-mono用的优化函数
        // backendOptimization();//贺大佬改后用的函数
    }
}

首先是根据当前的位姿对特征点进行三角化。然后先分析optimization函数。

optimization（）函数

0 ceres库简介

优化库——ceres（一）快速概览_雨luo凡城的博客-CSDN博客

ceres库主要用来求解有界约束的非线性最小二乘问题：

 

1. 添加ceres参数块,待优化变量 AddParameterBlock

a. 添加ceres参数块

    ceres::Problem problem;//构建优化问题
    ceres::LossFunction *loss_function;// 构建损失函数，引入鲁棒核函数
    loss_function = new ceres::CauchyLoss(1.0); // 柯西核函数

b. 添加待优化变量：相机状态（位姿、传感器偏差）

for (int i = 0; i < WINDOW_SIZE + 1; i++) // 遍历滑动窗数量，包括最新的一帧
    {
        ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
        problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);
        problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);
    }

先添加优化参数量, ceres中参数用ParameterBlock来表示,类似于g2o中的vertex, 这里的参数块有sliding windows中所有帧的para_Pose(7维) 和 para_SpeedBias(9维).

这块出现了新数据结构para_Pose[i]和para_SpeedBias[i]，这是因为ceres传入的都是double类型的，在vector2double()里初始化的。

c 添加待优化变量：相机外参

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)//如果IMU-相机外参不需要标定
        {
            ROS_DEBUG("fix extinsic param");
            problem.SetParameterBlockConstant(para_Ex_Pose[i]);//将这个值固定为常量
        }
        else
            ROS_DEBUG("estimate extinsic param");
    }

d. 添加待优化变量：imu-image时间同步误差

if (ESTIMATE_TD) //一维，标定同步时间
    {
        problem.AddParameterBlock(para_Td[0], 1);
    }

为什么没有特征点逆深度，在e中用para_Feature表示，但是没有添加到ParameterBlock中？？                        e. vector2double() 给ParameterBlock赋值

• 从Ps、Rs、Vs、Bas、Bgs转化为para_Pose（6维，相机位姿）和para_SpeedBias（9维，相机速度、加速度偏置、角速度偏置）
• 从tic和q转化为para_Ex_Pose （6维，Cam到IMU外参）
• 从dep到para_Feature（1维，特征点深度）
• 从td转化为para_Td（1维，标定同步时间）

void Estimator::vector2double()
{
    // 1.遍历滑动窗，IMU的15个自由度的优化变量
    for (int i = 0; i <= WINDOW_SIZE; i++)
    {
        // P、R
        para_Pose[i][0] = Ps[i].x();
        para_Pose[i][1] = Ps[i].y();
        para_Pose[i][2] = Ps[i].z();
        Quaterniond q{Rs[i]};
        para_Pose[i][3] = q.x();
        para_Pose[i][4] = q.y();
        para_Pose[i][5] = q.z();
        para_Pose[i][6] = q.w();
        // V、Ba、Bg
        para_SpeedBias[i][0] = Vs[i].x();
        para_SpeedBias[i][1] = Vs[i].y();
        para_SpeedBias[i][2] = Vs[i].z();
 
        para_SpeedBias[i][3] = Bas[i].x();
        para_SpeedBias[i][4] = Bas[i].y();
        para_SpeedBias[i][5] = Bas[i].z();
 
        para_SpeedBias[i][6] = Bgs[i].x();
        para_SpeedBias[i][7] = Bgs[i].y();
        para_SpeedBias[i][8] = Bgs[i].z();
    }
    // 2.Cam到IMU的外参，6自由度由7个参数表示
    for (int i = 0; i < NUM_OF_CAM; i++)
    {
        para_Ex_Pose[i][0] = tic[i].x();
        para_Ex_Pose[i][1] = tic[i].y();
        para_Ex_Pose[i][2] = tic[i].z();
        Quaterniond q{ric[i]};
        para_Ex_Pose[i][3] = q.x();
        para_Ex_Pose[i][4] = q.y();
        para_Ex_Pose[i][5] = q.z();
        para_Ex_Pose[i][6] = q.w();
    }
    
    VectorXd dep = f_manager.getDepthVector();
    for (int i = 0; i < f_manager.getFeatureCount(); i++)
        para_Feature[i][0] = dep(i);
        
    // 3、保证传感器同步的时钟差
    if (ESTIMATE_TD)
        para_Td[0][0] = td;
}

2. 添加残差块 AddResidualBlock

也就是目标函数中的三个残差项，包括边缘化的先验信息、imu测量残差和视觉的重投影误差。

每一个残差项都是一个factor, 这是ceres的使用方法：MarginalizationInfo, IMUFactor, ProjectionFactor

创建一个类继承ceres::CostFunction类, 重写Evaluate()函数定义residual的计算形式.

首先会调用addResidualBlockInfo()函数将各个残差以及残差涉及的优化变量添加入上面所述的优化变量中。

a. 添加边缘化残差

    if (last_marginalization_info)
    {
        // construct new marginlization_factor
        MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
        problem.AddResidualBlock(marginalization_factor, NULL,
                                 last_marginalization_parameter_blocks);
    }

这块出现了2个新的数据结构。在第一次执行这段代码的时候，没有先验信息，所以这段肯定是跳过的。当第二次执行的时候就有了。last_marginalization_info的赋值出现在后面的代码里。

数据结构: last_marginalization_info 这个数据结构定义在marginalization_factor.cpp里面，比较复杂。

数据结构：ast_marginalization_parameter_blocks。它指的是先验矩阵对应的状态量X。

class MarginalizationInfo
{
  public:
    ~MarginalizationInfo();
    int localSize(int size) const;
    int globalSize(int size) const;
    void addResidualBlockInfo(ResidualBlockInfo *residual_block_info);//添加参差块相关信息（优化变量，待marg的变量）
    void preMarginalize();//计算每个残差对应的雅克比，并更新parameter_block_data
    void marginalize();
    std::vector<double *> getParameterBlocks(std::unordered_map<long, double *> &addr_shift);

    std::vector<ResidualBlockInfo *> factors;//所有观测项
    int m, n;//m为要边缘化的变量个数，n为要保留下来的变量个数 m表示需marg变量/parameter_block_idx的总localSize n表示需保留变量的总localSize
    std::unordered_map<long, int> parameter_block_size; //global size//<优化变量内存地址,localSize>//为每个优化变量块的变量的大小，以IMU为例，是[7,9,7,9]
    int sum_block_size;
    std::unordered_map<long, int> parameter_block_idx; //local size //<优化变量内存地址,在矩阵中的id>//被merge掉的变量
    std::unordered_map<long, double *> parameter_block_data;//<优化变量内存地址,数据>//每个优化块数据
    //他们的key都同一是long类型的内存地址，而value分别是，各个优化变量的长度，各个优化变量在id以各个优化变量对应的double指针类型的数据。
    std::vector<int> keep_block_size; //global size
    std::vector<int> keep_block_idx;  //local size
    std::vector<double *> keep_block_data;
    //他们是进行边缘化之后保留下来的各个优化变量的长度，各个优化变量在id以各个优化变量对应的double指针类型的数据
    Eigen::MatrixXd linearized_jacobians;//分别指的是边缘化之后从信息矩阵恢复出来雅克比矩阵和残差向量
    Eigen::VectorXd linearized_residuals;
    const double eps = 1e-8;
}; 

b 添加imu残差 IMUFactor

    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;

 c 添加重投影残差

这里需要再次注意一点，IMU的残差是相邻两帧，但是视觉不是的，它加入的2帧，这两帧是观测到同一特征的最近两帧。  

 假如，特征点M在帧1、2、3、4、5中都能看到，循环到第2帧时，记录下重投影误差，然后还会在到第3、4、5帧嘛？

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;//必须满足出现2次以上且在倒数第二帧之前出现过,不满足的话，直接跳出本次循环，找下个特征点
    ++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;//！得到第二个特征点
        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, t_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]);
        }
        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++;
    }
}

continue 只管for、while，不看if，不管多少if都直接无视,continue 表示终止本次（本轮）循环。当代码执行到continue时，本轮循环终止，进入下一轮循环，具有过滤功能。

d 添加闭环检测残差，计算滑动窗口中与每一个闭环关键帧的相对位姿，这个相对位置是为后面的图优化准备

3. ceres求解Solver

a. 配置求解器

    ceres::Solver::Options options; 
    //配置求解器
    options.linear_solver_type = ceres::DENSE_SCHUR;
    options.trust_region_strategy_type = ceres::DOGLEG;
    options.max_num_iterations = NUM_ITERATIONS;
    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;
    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());

b. double2vector()

Vector3d origin_R0 = Utility::R2ypr(Rs[0]);
Vector3d origin_P0 = Ps[0];

    这块固定了先验信息。

边缘化之后再看吧。然后看无ROS版本的非线性优化是如何处理的。

2.  backOptimization()后端优化部分

2.1 系统数据类型转化

vector2double()；//由Ps,Rs转换为para_Pose，Vs,Bas,Bgs转换为para_SpeedBias，tic,ric转换为para_Ex_Pose。

 2.2 添加残差

包括边缘化的先验残差，IMU测量残差，camera测量残差。

注意这里IMU项和camera项之间是有一个系数，这个系数就是他们各自的协方差矩阵：IMU的协方差是预积分的协方差(IMUFactor::Evaluate，中添加IMU协方差，求解jacobian矩阵)，而camera的测量残差则是一个固定的系数。

problemSolve();//构建求解器

a.构建problem

//构建损失函数
backend::LossFunction *lossfunction;
lossfunction = new backedn::CauchyLoss(1.0);

//构建problem
backend::Problem problem(backend::Problem::ProblemType::SLAM_PROBLEM);
vector<shared_ptr<backend::VertexPose>> vertexCams_vec;
vector<shared_ptr<backend::VertexSpeedBias>> vertexVB_vec;
int pose_dim = 0;

b. 添加外参数节点 problem.AddVertex

c.添加IMU位姿和IMU速度以及偏置节点

d.添加IMU预积分对应的约束边 problem.Edge

e. 添加视觉点

首先遍历所有特征点，特征点需要满足至少出现过两次且在次新帧中也出现。

添加特征点的逆深度；然后遍历所有观察到该特征点的帧，构建所有观测到同一特征的帧之间的约束边，包括的信息：逆深度，边的顶点对应的IMU全局位姿，外参数，信息矩阵，lossfunction核函数

f. 添加边缘化得到的先验信息

g 非线性优化求解

problem.Solve(10);

h 位姿更新

优化求解完毕后，对窗口的IMU位姿进行更新，以及速度及偏置向量进行更新;对特征的逆深度进行更新。

2.3 

double2vector()；

2.4 后边是边缘化