VINS-Mono源码分析4— vins_estimator3(非线性优化)

最新推荐文章于 2023-11-28 15:52:45 发布

闭关修炼到走火入魔

最新推荐文章于 2023-11-28 15:52:45 发布

阅读量759

点赞数 2

分类专栏：笔记文章标签： slam

本文链接：https://blog.csdn.net/yys2324826380/article/details/105329910

版权

VINS-Mono源码分析4— vins_estimator3

初始化成功完成后，程序就进入了如下所示的代码中，

if(result)
{
   
    solver_flag = NON_LINEAR;
    solveOdometry();
    slideWindow();
    f_manager.removeFailures();
    ROS_INFO("Initialization finish!");
    last_R = Rs[WINDOW_SIZE];
    last_P = Ps[WINDOW_SIZE];
    last_R0 = Rs[0];
    last_P0 = Ps[0];
    
}

将solver_flag的状态置为NON_LINEAR后，就会执行后端非线性优化solveOdometry()函数和滑窗处理slideWindow()函数，并且删除特征点管理器f_manager中深度为负值的特征点，最后记录last_R、last_P、last_R0和last_P0。

接下来详细分析一下solveOdometry()函数，

void Estimator::solveOdometry()
{
   
    if (frame_count < WINDOW_SIZE)
        return;
    if (solver_flag == NON_LINEAR)
    {
   
        TicToc t_tri;
        f_manager.triangulate(Ps, tic, ric);
        ROS_DEBUG("triangulation costs %f", t_tri.toc());
        optimization();
    }
}

triangulate()函数在《VINS-Mono源码分析3— vins_estimator2》中已经分析过了，这里不再赘述了，不过要强调一下，这里再次调用此函数的目的是更新特征点的深度到世界坐标系下的图像帧中。接下来分析optimization()函数，

ceres::Problem problem;
ceres::LossFunction *loss_function;
//loss_function = new ceres::HuberLoss(1.0);
loss_function = new ceres::CauchyLoss(1.0);

构建ceres库优化问题，定义损失函数为柯西核函数，

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

添加参数块，包括滑窗内所有帧的位姿、IMU的速度和加计陀螺偏置信息以及IMU-相机间的外参位姿，并将IMU-相机间的外参位姿设为固定参数块。

if (ESTIMATE_TD)
{
   
    problem.AddParameterBlock(para_Td[0], 1);
}

这段代码默认不执行。

vector2double();

void Estimator::vector2double()
{
   
    ......
    VectorXd dep = f_manager.getDepthVector();
    for (int i = 0; i < f_manager.getFeatureCount(); i++)
        para_Feature[i][0] = dep(i);
    if (ESTIMATE_TD)
        para_Td[0][0] = td;
}

这段代码就是给para_Pose、para_SpeedBias和para_Ex_Pose赋值，最重要的是得到逆深度para_Feature。其实这里我一直不太明白，按理说应该先给para_Pose、para_SpeedBias和para_Ex_Pose赋值，然后再添加参数块才对，但不知为何这样写程序也能正常执行？

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

定义边缘化残差信息，添加边缘化残差块，注意边缘化在优化过程中只是起到了一个约束的作用。关于边缘化这部分专门写了一篇博客《VINS-Mono源码分析5— vins_estimator4》来介绍，这里就不多讲了。

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

定义IMU残差信息，添加IMU残差块。这部分具体实现代码在factor文件夹下的IMU_factor.h文件中，

virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
   
    //传参
    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 Vi(parameters[1][0], parameters[1][1], parameters[1][2]);
    Eigen::Vector3d Bai(parameters[1][3], parameters[1][4], parameters[1][5]);
    Eigen::Vector3d Bgi(parameters[1][6], parameters