VINS-Mono源码分析7— pose_graph2(四自由度位姿图优化)

PoseGraph::PoseGraph()
{
   
    ......
	t_optimization = std::thread(&PoseGraph::optimize4DoF, this);
    ......
}

在pose_graph_node.cpp中，定义posegraph变量时，就开启了t_optimization线程，在这个线程中执行optimize4DoF函数，

void PoseGraph::optimize4DoF()
{
   
    while(true)
    {
   
        int cur_index = -1;
        int first_looped_index = -1;
        m_optimize_buf.lock();
        while(!optimize_buf.empty())
        {
   
            //得到 和回环帧匹配成功的 当前关键帧的索引，
            //即optimize_buf的最后一个元素，因为如果不遍历到最后，跳不出循环
            cur_index = optimize_buf.front();
            //得到时间上最早的回环帧的索引，它和上面的cur_index是匹配的
            first_looped_index = earliest_loop_index;
            optimize_buf.pop();
        }
        m_optimize_buf.unlock();
        if (cur_index != -1)
        {
   
            printf("optimize pose graph \n");
            TicToc tmp_t;
            m_keyframelist.lock();
            //得到cur_index索引对应的关键帧
            KeyFrame* cur_kf = getKeyFrame(cur_index);
            //等待优化的关键帧的帧数上限，极端情况下优化所有关键帧
            int max_length = cur_index + 1;

            // 定义ceres库优化时会用到的变量
            double t_array[max_length][3];
            Quaterniond q_array[max_length];
            double euler_array[max_length][3];
            double sequence_array[max_length];
            //构建ceres问题，ceres库优化前的一些参数配置
            ceres::Problem problem;
            ceres::Solver::Options options;
            options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
            //options.minimizer_progress_to_stdout = true;
            //options.max_solver_time_in_seconds = SOLVER_TIME * 3;
            options.max_num_iterations = 5;
            ceres::Solver::Summary summary;
            ceres::LossFunction *loss_function;
            loss_function = new ceres::HuberLoss(0.1);
            //loss_function = new ceres::CauchyLoss(1.0);
            //AngleLocalParameterization类指定yaw角优化变量是如何进行迭代更新的
            ceres::LocalParameterization* angle_local_parameterization =
                AngleLocalParameterization::Create();

            list<KeyFrame*>::iterator it;

            int i = 0;
            for (it = keyframelist.begin(); it != keyframelist.end(); it++)
            {
   
                //回环帧之前的关键帧不优化，跳过
                if ((*it)->index < first_looped_index)
                    continue;
                //待优化关键帧的局部索引
                (*it)->local_index = i;
                //给ceres优化时用到的变量赋值
                Quaterniond tmp_q;
                Matrix3d tmp_r;
                Vector3d tmp_t;
                //注意getVioPose()函数与getPose()函数的区别
                (*it)->getVioPose(tmp_t, tmp_r);
                tmp_q = tmp_r;
                t_array[i][0] = tmp_t(0);
                t_array[i][1] = tmp_t(1);
                t_array[i][2] = tmp_t(2);
                q_array[i] = tmp_q;

                Vector3d euler_angle = Utility::R2ypr(tmp_q.toRotationMatrix());
                euler_array[i][0] = euler_angle.x();
                euler_array[i][1] = euler_angle.y();
                euler_array[i][2] = euler_angle.z();

                sequence_array[i] = (*it)->sequence;
                //添加参数块
                problem.AddParameterBlock(euler_array[i], 1, angle_local_parameterization);
                problem.AddParameterBlock(t_array[i], 3);

                if ((*it)->index == first_looped_index || (*it)->sequence == 0)
                {
      //固定回环帧的欧拉角和位置
                    problem.SetParameterBlockConstant(euler_array[i]);
                    problem.SetParameterBlockConstant(t_array[i]);
                }

                //add edge
                //添加序列边的目的就是缩小回环帧－当前帧中间的那些关键帧的位姿误差
                for (int j =