VINS-Mono代码学习记录（九）--processImage()(滑窗优化边缘化部分）

逃离我推掉我的手 2023-06-17 06:55 9阅读 0赞

上一节结束了初始化的内容，根据这张流程图：  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl8zODUwNDI3Ng_size_16_color_FFFFFF_t_70]接下来继续学习其中的solveOdometry( )函数，这一块就开始优化啦，VINS-Mono采用的是ceres来进行优化的。  
在solveOdometry( )函数中，首先需要了解的是三角化triangulate(Vector3d Ps\[\], Vector3d tic\[\], Matrix3d ric\[\])，这一部分需要先看理论知识，不然就会一头雾水，在这里，直接贴上我查找的对于三角化的比较好理解的解释：  
参考链接： https://blog.csdn.net/qq\_37611824/article/details/93210012  
参考链接2： https://blog.csdn.net/u012101603/article/details/79714332

代码部分为：

void FeatureManager::triangulate(Vector3d Ps[], Vector3d tic[], Matrix3d ric[])
    {
        for (auto &it_per_id : 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;
    
            if (it_per_id.estimated_depth > 0)
                continue;
            int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;
    
            ROS_ASSERT(NUM_OF_CAM == 1);
            Eigen::MatrixXd svd_A(2 * it_per_id.feature_per_frame.size(), 4);//每一帧可以产生两个约束
            int svd_idx = 0;
    
            Eigen::Matrix<double, 3, 4> P0;
            Eigen::Vector3d t0 = Ps[imu_i] + Rs[imu_i] * tic[0];//这个坐标变换有点搞不清楚
            Eigen::Matrix3d R0 = Rs[imu_i] * ric[0];
            P0.leftCols<3>() = Eigen::Matrix3d::Identity();
            P0.rightCols<1>() = Eigen::Vector3d::Zero();
    
            for (auto &it_per_frame : it_per_id.feature_per_frame)
            {
                imu_j++;
    
                Eigen::Vector3d t1 = Ps[imu_j] + Rs[imu_j] * tic[0];
                Eigen::Matrix3d R1 = Rs[imu_j] * ric[0];
                Eigen::Vector3d t = R0.transpose() * (t1 - t0);
                Eigen::Matrix3d R = R0.transpose() * R1;
                Eigen::Matrix<double, 3, 4> P;
                P.leftCols<3>() = R.transpose();
                P.rightCols<1>() = -R.transpose() * t;
                Eigen::Vector3d f = it_per_frame.point.normalized();//(u,v,1)坐标
                svd_A.row(svd_idx++) = f[0] * P.row(2) - f[2] * P.row(0);//构建svd_A每帧的第一个约束
                svd_A.row(svd_idx++) = f[1] * P.row(2) - f[2] * P.row(1);//构建svd_A每帧的第二个约束
    
                if (imu_i == imu_j)
                    continue;
            }
            ROS_ASSERT(svd_idx == svd_A.rows());
            Eigen::Vector4d svd_V = Eigen::JacobiSVD<Eigen::MatrixXd>(svd_A, Eigen::ComputeThinV).matrixV().rightCols<1>();
            double svd_method = svd_V[2] / svd_V[3];
            //it_per_id->estimated_depth = -b / A;
            //it_per_id->estimated_depth = svd_V[2] / svd_V[3];
    
            it_per_id.estimated_depth = svd_method;
            //it_per_id->estimated_depth = INIT_DEPTH;
    
            if (it_per_id.estimated_depth < 0.1)
            {
                it_per_id.estimated_depth = INIT_DEPTH;
            }
    
        }
    }

看到一个对于理论推导的介绍，  
参考链接： https://blog.csdn.net/qq\_41839222/article/details/93593844  
参考链接：https://blog.csdn.net/q597967420/article/details/76099443  
optimization()中部分注释如下：

void Estimator::optimization()
    {
        //[1]创建一个ceres Problem实例, loss_function定义为CauchyLoss.
        ceres::Problem problem;
        ceres::LossFunction *loss_function;
        //loss_function = new ceres::HuberLoss(1.0);
        loss_function = new ceres::CauchyLoss(1.0);
    
        //[2]添加优化参数量, ceres中参数用ParameterBlock来表示,类似于g2o中的vertex,
        // 这里的参数块有sliding windows中所有帧的para_Pose(7维) 和 para_SpeedBias(9维)
        for (int i = 0; i < WINDOW_SIZE + 1; i++)
        {
            ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
            problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);//SIZE_POSE=7
            problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);//SIZE_SPEEDBIAS=9
        }
        /*add vertex of: camera extrinsic */
        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]);
        }
    
    
        //[3]添加残差，依次加入margin项,IMU项和视觉feature项. 每一项都是一个factor, 这是ceres的使用方法,
        // 创建一个类继承ceres::CostFunction类, 重写Evaluate()函数定义residual的计算形式.
        // 分别对应marginalization_factor.h, imu_factor.h, projection_factor.h中的MarginalizationInfo, IMUFactor, ProjectionFactor三个类
    
        TicToc t_whole, t_prepare;
        vector2double();
    
        //先验
        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);//添加先验残差
        }

这里可以去了解一下marginalization\_factor的头文件和源文件，  
头文件中定义了两个struct,两个类， 创建一个类MarginalizationFactor继承ceres::CostFunction类, 重写Evaluate()函数定义residual的计算形式。  
边缘化知识参考： https://blog.csdn.net/weixin\_44580210/article/details/95748091

//imu residual
        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的残差
        }
    
        //视觉残差
        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;
                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++;
                    }     
                }
            }
    
        }
    
        //[4]创建一个求解配置参数Option, 定义成DENSE_SCHUR,
        // 优化算法用的”dog leg”, 设置最大迭代次数和最大求解时间. 创建一个求解描述Summary, 调用ceres::Solve()进行求解
        ceres::Solver::Options options;
    
        options.linear_solver_type = ceres::DENSE_SCHUR;
        //options.num_threads = 2;
        options.trust_region_strategy_type = ceres::DOGLEG;
        options.max_num_iterations = NUM_ITERATIONS;
        //options.use_explicit_schur_complement = true;
        //options.minimizer_progress_to_stdout = true;
        //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;
        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();

在这个函数中需要去延伸的是factor文件，像是imu\_factor.h和intergration\_base.h下定义imu的模型，中值积分，残差，信息矩阵，雅克比等，具体的注释，就不给出了。  
optimization( )中优化结束之后边缘化部分的代码注释如下：  
这个一定要参考 https://blog.csdn.net/weixin\_44580210/article/details/95748091 写得非常详细。。  
当marg最老帧时，具体操作步骤：

TicToc t_whole_marginalization;
        if (marginalization_flag == MARGIN_OLD)//marg最老帧
        {
            MarginalizationInfo *marginalization_info = new MarginalizationInfo();
            vector2double();//ceres中变量必须用数组类型，所以需要进行数据类型转换成我之前一直迷惑的二维数组。。。哈哈哈
            //para_Pose（6维，相机位姿）、
            //para_SpeedBias（9维，相机速度、加速度偏置、角速度偏置）、
            //para_Ex_Pose（6维、相机IMU外参）、
            //para_Feature（1维，特征点深度）、
            //para_Td（1维，标定同步时间）
    
            //先验误差会一直保存，而不是只使用一次
            // 如果上一次边缘化的信息存在
            //要边缘化的参数块是 para_Pose[0] para_SpeedBias[0] 以及 para_Feature[feature_index](滑窗内的第feature_index个点的逆深度)
    
            //添加上一次的先验残差
            if (last_marginalization_info)
            {
                vector<int> drop_set;//待marg的优化变量id
                for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++)//last_marginalization_parameter_blocks是上一轮留下来的残差块
                {
                    if (last_marginalization_parameter_blocks[i] == para_Pose[0] ||
                        last_marginalization_parameter_blocks[i] == para_SpeedBias[0])//需要marg掉的优化变量，也就是滑窗内第一个变量
                        drop_set.push_back(i);
                }
                // 构造边缘化的的Factor
                // 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);
            }
    
    
            {
                //添加IMU的先验，只包含边缘化帧的IMU测量残差
                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});//其待边缘化变量是para_Pose[0], para_SpeedBias[0]，也是第一政相关的变量都作为边缘化的对象
                    marginalization_info->addResidualBlockInfo(residual_block_info);
                }
            }
    
            {
                //添加视觉的先验，只添加起始帧是旧帧且观测次数大于2的Features
                int feature_index = -1;
                for (auto &it_per_id : f_manager.feature)// 遍历滑窗内所有的Features
                {
                    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)) //Feature的观测次数不小于2次，且起始帧不属于最后两帧
                        continue;
    
                    ++feature_index;
    
                    int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;
                    if (imu_i != 0)//只选择被边缘化的帧的Features，在这里是marg最老帧就是滑窗内的第0帧，当这个特征点被观测的起始帧不是第0帧，就不marg.
                        continue;
    
                    Vector3d pts_i = it_per_id.feature_per_frame[0].point;// 得到该Feature在起始帧下的归一化坐标
    
                    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});//其待边缘化变量是para_Pose[imu_i]和para_Feature[feature_index]位姿和相关的特征点
                            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);
                        }
                    }
                }
            }
    
            //将三个ResidualBlockInfo中的参数块综合到marginalization_info中
            // 计算所有ResidualBlock(残差项)的残差和雅克比,parameter_block_data是参数块的容器
            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 = marginalization_info;
            last_marginalization_parameter_blocks = parameter_blocks;
            
        }

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl8zODUwNDI3Ng_size_16_color_FFFFFF_t_70]: https://img-blog.csdnimg.cn/20191127214634570.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl8zODUwNDI3Ng==,size_16,color_FFFFFF,t_70