VINS-Mono 代码解析三、后端优化第1部分

最新推荐文章于 2023-12-10 17:02:10 发布

他人是一面镜子，保持谦虚的态度

最新推荐文章于 2023-12-10 17:02:10 发布

阅读量2k

点赞数 4

分类专栏： VINS-Mono and Fusion 程序解读文章标签：自动驾驶深度学习机器学习

本文链接：https://blog.csdn.net/hltt3838/article/details/109443231

版权

VINS-Mono and Fusion 程序解读专栏收录该内容

20 篇文章 104 订阅

订阅专栏

【5】后端优化

一、优化部分

前言：IMU预积分

1. 当前帧的位置、速度、旋转:

2. 相邻两帧之间的位置、速度、旋转的变化量（IMU预积分）

3. IMU预积分的误差分析

4. IMU预积分误差的协方差和Jacobian计算

注意：IMU积分只要给定初始值，然后机械编排就行了，得到的值是相对于初始位置的距离，速度，姿态；

而预积分仅仅是相邻两帧之间的位置、速度、旋转的变化量，可以看作一个独立的部分，只要有加速度和陀螺仪数据就能得到相邻两帧预积分的值！

正式部分：

1、需要优化的状态向量
状态向量共包括滑动窗口内的 n+1 个所有相机的状态(包括位置、朝向、速度、加速度计 bias 和陀螺仪 bias)、Camera 到 IMU 的外参、m+1 个 3D 点的逆深度:

2、要优化的目标函数

其中三个残差项即误差项分别为 边缘化的先验信息、IMU 测量残差、视觉的重投影残差，三种残差都是用马氏距离表示；对于边缘化的先验信息 可能有点模糊，有面我们在仔细的分析！

用下面的经典的图去理解 视觉 IMU的约束关系，应仔细体会和理解！

和下面这个图结合着看吧！

理解：上图中，为什么b1 到 b2 标注的是 IMU Measurements; 而 b2 到 b3标注的是 Pre-integrated IMU Measurements ?

假如b1是初始位置，IMU 只要有一个初始值，便可以根据机械编排得到当前时刻的位置、速度、和姿态（这个我们应该很清楚的），这便是b1 到 b2 标注的是 IMU Measurements的原因；

而预积分是一个变化量，和初始位置没有关系，故b2到b3的值可以用预积分方法获得，也可以用b3时刻的IMU积分值减去 b3时刻的IMU积分值获得，两者的差理想情况下等于0；

这个便是IMU的一种约束,后面的方程可以让我们可以更加的明白！

根据《十四讲》中高斯-牛顿法,若要计算目标函数的最小值,可以理解为,当优化变量有一个增量后,目标函数值最小, 以 IMU 残差 为例,可写成如下所示:

3、IMU约束（预测值-IMU预积分值）

1)残差:两帧之间的 PVQ 和 bias 的变化量的差 (理想情况下这个值等于0，但是有噪声)

其中各增量关于 bias 的 Jacobian 可从相应位置获得，上面与代码中 IntegrationBase::evaluate()对应。

2)优化变量:

3)Jacobian:

4)协方差

上面提到的 IMU 协方差 P 为推导的 IMU 预积分中迭代出来的 IMU 增量误差的协方差。

4、视觉约束

1) 残差
视觉残差是重投影误差,对于第 l 个路标点 P,将 P 从第一次观看到它的第 i 个相机坐标系,转换到当前的第 j 个相机坐标系下的像素坐标,可定义视觉误差项为:

2) 优化变量

3) Jacobian

根据视觉残差公式,我们可以得到相对于各优化变量的 Jacobian

4)协方差

视觉约束的噪声协方差与标定相机内参时的重投影误差,也就是偏离几个像素有关,代码对应为 ProjectionTdFactor::sqrt_info,

这里取的 1.5 个像素,对应到归一化相机平面上的协方差矩阵需除以焦距 f,则信息矩阵等于协方差矩阵的逆,为:

5) 时间戳偏移量 td

如果考虑 Cam 和 IMU 的时间戳偏移量 td,则需要增加优化变量,后续再补

二、代码部分

在 void Estimator::processImage() 程序里面，有

if (solver_flag == INITIAL) 联合初始化

else 后端优化

具体代码如下：

 else//【5】紧耦合的非线性优化，这是一个重要模块。一般非线性优化是稳定状态下VINS系统一直要循环的部分
    {
        TicToc t_solve;
        solveOdometry();// 5.1 非线性化求解里程计
        ROS_DEBUG("solver costs: %fms", t_solve.toc());

         //5.2 故障检测与恢复,一旦检测到故障，系统将切换回初始化阶段
        if (failureDetection())
        {
            ROS_WARN("failure detection!");
            failure_occur = 1;//失败标志位=1
            clearState();     // 清空状态
            setParameter();   // 重设参数
            ROS_WARN("system reboot!");
            return;
        }

        TicToc t_margin;
        slideWindow();             //5.3 滑动窗口
        f_manager.removeFailures();//5.4 去除估计失败的点并发布关键点位置/
        ROS_DEBUG("marginalization costs: %fms", t_margin.toc());
        
        // prepare output of VINS
        key_poses.clear();
        for (int i = 0; i <= WINDOW_SIZE; i++)
            key_poses.push_back(Ps[i]);

        last_R = Rs[WINDOW_SIZE];  //用于failure dectection
        last_P = Ps[WINDOW_SIZE];
        last_R0 = Rs[0];
        last_P0 = Ps[0];
        
        //而optimization()恰好在solveOdometry()里，所以可以发现，代码是先优化，后marg
    }
}

上面这段程序，主要的2部分是：solveOdometry() -非线性化求解里程计和 slideWindow()-滑动窗口

第一部分： solveOdometry() - 非线性化求解里程计，程序如下：

void Estimator::solveOdometry()
{
    //如果滑窗内视觉帧的个数小于设定大小，则返回
    if (frame_count < WINDOW_SIZE)
        return;
    
    if (solver_flag == NON_LINEAR)
    {
        TicToc t_tri;
        //Ps是唯一，tic和ric是相机到IMU的外参
        //三角化还没有得到深度的Features
        f_manager.triangulate(Ps, tic, ric);
        ROS_DEBUG("triangulation costs %f", t_tri.toc());
        //进入优化阶段
        optimization();
    }
}

第一件事就是根据当前的位姿三角化特征点，然后是 optimization() 函数，这一部分是主要的内容！

子程序 optimization() // 应该熟悉 ceres 用法，可以参考我的博客：Ceres Solver中文详细易懂版学习笔记_卫少东个人博客-CSDN博客

Ceres 需要添加 优化的变量、优化的残差函数 两个部分

(1)非线性优化
a. 声明和引入鲁棒核函数

void Estimator::optimization()
{//创建一个ceres Problem实例, loss_function定义为CauchyLoss.柯西核函数
    ceres::Problem problem;//一种类
    ceres::LossFunction *loss_function;//1.引入鲁棒核函数
    //loss_function = new ceres::HuberLoss(1.0);
    loss_function = new ceres::CauchyLoss(1.0);//柯西核函数

b. 添加各种待优化量X——位姿优化量

先添加优化参数量, ceres中参数用ParameterBlock来表示, 类似于g2o中的vertex, 这里的参数块有sliding windows中所有帧的para_Pose(7维) 和 para_SpeedBias(9维).
Ps、Rs转变成para_Pose, 6自由度7DOF（x,y,z,qx,qy,qz,w），Vs、Bas、Bgs转变成para_SpeedBias,9自由度9DOF(vx,vy,vz,bax,bay,baz,bgx,bgy,bgz).

    for (int i = 0; i < WINDOW_SIZE + 1; i++)// 遍历滑动窗数量，还包括最新的第11帧
    {
        ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
        problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);
        problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);
        /*这块出现了新数据结构para_Pose[i]和para_SpeedBias[i]，这是因为ceres传入的都是double类型的，在vector2double()里初始化的*/
    }

添加各种待优化量X——相机外参

    for (int i = 0; i < NUM_OF_CAM; i++)//7维、相机IMU外参
    {
        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]);//这个变量固定为constant
        }
        else
            ROS_DEBUG("estimate extinsic param");
    }

添加各种待优化量X——IMU-image时间同步误差

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

疑问：怎么没有逆深度参与优化? 答：-实际上优化了逆深度，在ProjectionFactor里面

c.给ParameterBlock赋值
vector2double();

vector2double做类型装换，把原来的Ps Vs Bgs Bas转到para_Pose para_SpeedBias下

d. 添加各种残差——先验信残差

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

关于这一部分的理解，请看heyijia大神的BLOG:http://blog.csdn.net/heyijia0327/article/details/53707261,可能是说的最清楚的一个了，这里可以这样理解，下面会添加对IMU和视觉的残差，但是，这些对应的变量实际上跟之前被margin掉的变量是有约束的，这里的last_marginalization_parameter_blocks就是保存的和margin掉变量有关系的变量，也就是heyijia博客中对应的Xb变量，last_marginalization_info中对应的是Xb对应的测量Zb，这里用先验来表示这个约束，整个margin部分实际上就是在维护这个结构：

这块出现了2个新的数据结构。在第一次执行这段代码的时候，没有先验信息，所以这段肯定是跳过的。当第二次执行的时候就有了。last_marginalization_info的赋值出现在后面的代码里。数据结构: last_marginalization_info，这个数据结构定义在marginalization_factor.cpp里面，比较复杂；另一个数据结构就是last_marginalization_parameter_blocks。它指的是先验矩阵对应的状态量X

添加各种残差——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]);
         //注意在添加残差的组成部分，由前后两帧的[p,q,v,b]组成，在计算雅克比的时候[p,q](7),[v,b](9)分开计算.
        problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);
    }

添加各种残差——重投影残差

注意：IMU的残差是相邻两帧，但是视觉不是的, 上面的理论部分我们是可以知道的，但是你们有没有想过为什么不是相邻的？

自己的理解：相比于IMU，相邻两帧的误差比较小，不是不能选择相邻的，而是选择相距一定的帧可能效果是一样的，而且计算量变小了

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

添加各种残差——回环检测

if(relocalization_info)
    {
        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++;
                }     
            }
        }

    }

e. 求解

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

    double2vector();

(2)边缘化

..........

请看下集

VINS-Mono 代码解析三、后端优化第2部分_努力努力努力-CSDN博客

他人是一面镜子，保持谦虚的态度

关注

4
点赞
踩
23

收藏

觉得还不错? 一键收藏
打赏
0
评论
VINS-Mono 代码解析三、后端优化第1部分

【5】后端优化一、理论部分1、需要优化的状态向量状态向量共包括滑动窗口内的 n+1 个所有相机的状态(包括位置、朝向、速度、加速度计 bias 和陀螺仪 bias)、Camera 到 IMU 的外参、m+1 个 3D 点的逆深度:2、要优化的目标函数其中三个残差项即误差项分别为边缘化的先验信息、IMU 测量残差、视觉的重投影残差，三种残差都是用马氏距离表示；对于边缘化的先验信息可能有点模糊，有面我们在仔细的分析！用下面的一个经典的图去理解视觉 IMU的约束...
复制链接

扫一扫