VINS-FUSION 前端后端代码全详解（二）

4.3.2 双目+IMU初始化

// stereo + IMU initilization
if(STEREO && USE_IMU)

求解深度

// 双目三角化
// 结果放入了feature的estimated_depth中
void FeatureManager::triangulate(int frameCnt, Vector3d Ps[], Matrix3d Rs[], Vector3d tic[], Matrix3d ric[])


// 利用svd方法对双目进行三角化
void FeatureManager::triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1,
                        Eigen::Vector2d &point0, Eigen::Vector2d &point1, Eigen::Vector3d &point_3d)

有了深度之后就可以进行PnP求解

// 有了深度，当下一帧照片来到以后就可以利用pnp求解位姿了
void FeatureManager::initFramePoseByPnP(int frameCnt, Vector3d Ps[], Matrix3d Rs[], Vector3d tic[], Matrix3d ric[])

之后求解陀螺仪的偏执,并对IMU预积分值进行重新传播

solveGyroscopeBias(all_image_frame, Bgs);

// 对之前预积分得到的结果进行更新。
// 预积分的好处查看就在于你得到新的Bgs，不需要又重新再积分一遍，可以通过Bgs对位姿，速度的一阶导数，进行线性近似，得到新的Bgs求解出MU的最终结果。
for (int i = 0; i <= WINDOW_SIZE; i++)
{
    pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);
}

进行优化

optimization();
updateLatestStates();
solver_flag = NON_LINEAR;
slideWindow();

其中的optimization()、 updateLatestStates()、 slideWindow()在下一篇博客进行讲解

4.3.3双目初始化

// stereo only initilization
if(STEREO && !USE_IMU)
{
    f_manager.initFramePoseByPnP(frame_count, Ps, Rs, tic, ric);
    f_manager.triangulate(frame_count, Ps, Rs, tic, ric);
    optimization();

    if(frame_count == WINDOW_SIZE)
    {
        optimization();
        updateLatestStates();
        solver_flag = NON_LINEAR;
        slideWindow();
        ROS_INFO("Initialization finish!");
    }
}

5. 回环检测pose_graph_node.cpp

本节主要讲loop_fusion包的程序结构，loop_fusion主要作用：利用词袋模型进行图像的回环检测。在vinsmono中，该程序包处于pose_graph包内。vins_fusion与vins_mono一个差别在于，回环检测的点云数据在mono中有回调供给VIO进行非线性优化，而在fusion中，VIO估计完全独立于回环检测的结果。即回环检测的全局估计会受到VIO的影响，但是VIO不受全局估计的影响。（这个意思是fusion我们可以单独使用VIO部分）

5.1 程序入口

5.1读取配置文件

通过fsSetting进行相应的参数配置，其中比较重要的是读入了vocabulary_file,即在support_files里面的brief_k10L6.bin。以及BRIEF_PATTERN_FILE。通过posegraph.loadVocabulary为posegraph的类成员 BriefDatabase db设置属性以及BriefVocabulary voc赋值。以及为BRIEF_PATTERN_FILE赋值。为后期keyframe的构建创造一个基础。

    cv::FileStorage fsSettings(config_file, cv::FileStorage::READ);
    if(!fsSettings.isOpened())
    {
        std::cerr << "ERROR: Wrong path to settings" << std::endl;
    }

    cameraposevisual.setScale(0.1);
    cameraposevisual.setLineWidth(0.01);

    std::string IMAGE_TOPIC;
    int LOAD_PREVIOUS_POSE_GRAPH;
    ROW = fsSettings["image_height"];
    COL = fsSettings["image_width"];
    std::string pkg_path = ros::package::getPath("loop_fusion");
    string vocabulary_file = pkg_path + "/../support_files/brief_k10L6.bin";
    cout << "vocabulary_file" << vocabulary_file << endl;
    posegraph.loadVocabulary(vocabulary_file);

    BRIEF_PATTERN_FILE = pkg_path + "/../support_files/brief_pattern.yml";
    cout << "BRIEF_PATTERN_FILE" << BRIEF_PATTERN_FILE << endl;

5.1.2LOAD_PREVIOUS_POSE_GRAPH

即是否要加载原有的地图信息，如果加载了之前的信息，则posegraph.loadPoseGraph，之前所有的关键帧的序号sequence都设置为0，base_sequence也是0。不过不加载之前的信息，base_sequence=1。

    if (LOAD_PREVIOUS_POSE_GRAPH)
    {
        printf("load pose graph\n");
        m_process.lock();
        posegraph.loadPoseGraph();
        m_process.unlock();
        printf("load pose graph finish\n");
        load_flag = 1;
    }
    else
    {
        printf("no previous pose graph\n");
        load_flag = 1;
    }

5.1.3 订阅话题，发布话题

// 订阅里程计
    ros::Subscriber sub_vio = n.subscribe("/vins_estimator/odometry", 2000, vio_callback);
    // 订阅图像
    ros::Subscriber sub_image = n.subscribe(IMAGE_TOPIC, 2000, image_callback);
    // 订阅关键帧位姿
    ros::Subscriber sub_pose = n.subscribe("/vins_estimator/keyframe_pose", 2000, pose_callback);
    // 订阅外参
    ros::Subscriber sub_extrinsic = n.subscribe("/vins_estimator/extrinsic", 2000, extrinsic_callback);
    // 订阅关键帧特征点
    ros::Subscriber sub_point = n.subscribe("/vins_estimator/keyframe_point", 2000, point_callback);
    // 
    ros::Subscriber sub_margin_point = n.subscribe("/vins_estimator/margin_cloud", 2000, margin_point_callback);

    pub_match_img = n.advertise<sensor_msgs::Image>("match_image", 1000);
    pub_camera_pose_visual = n.advertise<visualization_msgs::MarkerArray>("camera_pose_visual", 1000);
    pub_point_cloud = n.advertise<sensor_msgs::PointCloud>("point_cloud_loop_rect", 1000);
    pub_margin_cloud = n.advertise<sensor_msgs::PointCloud>("margin_cloud_loop_rect", 1000);
    pub_odometry_rect = n.advertise<nav_msgs::Odometry>("odometry_rect", 1000);

process && command

process主要的作用是开启一个新线程，这一块为程序的主要执行部分，我们下一节详细阐述。而command是开启一个控制台键盘控制线程。键盘控制输入，有提供两种选择，在控制台上写入：‘s’：把当前Posegraph存储起来，并且把整个loop_fusion包的进程关掉。‘n’：图像更新序列,new_sequence()。

void command()
{
    while(1)
    {
        char c = getchar();
        if (c == 's')
        {
            m_process.lock();
            posegraph.savePoseGraph();
            m_process.unlock();
            printf("save pose graph finish\nyou can set 'load_previous_pose_graph' to 1 in the config file to reuse it next time\n");
            printf("program shutting down...\n");
            ros::shutdown();
        }
        if (c == 'n')
            new_sequence();

        std::chrono::milliseconds dura(5);
        std::this_thread::sleep_for(dura);
    }
}

5.2 process

5.2.1 数据输入

首先process需要先判断image_buf,pose_buf,point_buf三个buff都不为空的时候，才进行运行，否则程序就休息5毫秒，继续监测。

// find out the messages with same time stamp
// 时间戳对齐
m_buf.lock();
if(!image_buf.empty() && !point_buf.empty() && !pose_buf.empty())

其中：
（1）image_buf，存放image_callback()图像回调函数的信息，接收相机话题的原始图片。并且当两张图片的时间戳相差1秒，或者说当图像的时间戳小于上一帧的时间戳，可以认为图像出现新序列，因此new_sequence()
（2）point_buf,存放point_callback()关键帧的三维特征点信息的回调。并且在这个回调函数中，同时利用point_cloud标准数据结构发布特征点三维点云以供可视化操作。发布的话题为：point_cloud_loop_rect。注意，此时的点云信息不是单纯vio得到的点云信息，而是在pose_graph参考坐标系下，关键帧点云的位置。因为vio自己有一个参考坐标系，而pose_graph求解出来的位姿图也有自己的坐标系，这两个坐标系有一个变换关系，写在了pose_graph.r_drift,pose_graph.t_drift里面。
（3）pose_buf，存放pose_callback()回调函数的信息。订阅的是关键帧的vio得到的关键帧的位姿信息。

从上面订阅的信息我们可以看出，只有当相机有关键帧的时候（vins_estimator中有对应的规则判断该帧是不是关键帧），才会进行进一步处理，整个loop_fusion处理的信息不是原始图像，而是一帧帧关键帧。

接着程序开始找一帧keyframe 对应的image_buf，point_buf，pose_buf三者的数据。即point_buf和pose_buf这个存储的点云和位姿是对应（image_buf里面的）哪一帧关键帧。最终的结果
（1）image_msg存放关键帧的图像原始信息
（2）point_msg存放了该关键帧里面的3D点云信息，由VIO以及pose_graph.r_drift, pose_graph.t_drift 解算得到。
（3）pose_msg存放了该关键帧的位姿信息。

if(!image_buf.empty() && !point_buf.empty() && !pose_buf.empty())
{
    if (image_buf.front()->header.stamp.toSec() > pose_buf.front()->header.stamp.toSec())
    {
        pose_buf.pop();
        printf("throw pose at beginning\n");
    }
    else if (image_buf.front()->header.stamp.toSec() > point_buf.front()->header.stamp.toSec())
    {
        point_buf.pop();
        printf("throw point at beginning\n");
    }
    else if (image_buf.back()->header.stamp.toSec() >= pose_buf.front()->header.stamp.toSec() 
        && point_buf.back()->header.stamp.toSec() >= pose_buf.front()->header.stamp.toSec())
    {
        pose_msg = pose_buf.front();
        pose_buf.pop();
        while (!pose_buf.empty())
            pose_buf.pop();
        while (image_buf.front()->header.stamp.toSec() < pose_msg->header.stamp.toSec())
            image_buf.pop();
        image_msg = image_buf.front();
        image_buf.pop();

        while (point_buf.front()->header.stamp.toSec() < pose_msg->header.stamp.toSec())
            point_buf.pop();
        point_msg = point_buf.front();
        point_buf.pop();
    }
}

5.2.2 关键帧

根据上面得到的三个信息，来构造关键帧！！用数据结构KeyFrame来表示，这里用了十个变量的KeyFrame构造函数来构造这个关键帧，十个变量的KeyFrame来构造函数默认没有brief_descriptor，因为我们VIO前端提取的并不需要特征点的描述子。在构造关键帧的时候，函数同时又增加了阈值大于20的FAST角点，来增加关键帧特征点的数量，同时，对这些特征点用相应的描述子来进行表述。
构造出一个向量容器，用来存放所有特征点的描述子，即KeyFrame类成员 vector<BRIEF::bitset> brief_descriptors（在keyframe.cpp文件中）
注意点：只有当两个关键帧之间的位移量(T - last_t).norm() > SKIP_DIS时候，才会构造新的关键帧。但是程序默认的SKIP_DIS为0。就是除非T和last_t完全相等，否则就会进入构造函数。
构造好的keyframe，通过posegraph.addKeyFrame(keyframe,1)，加入到全局姿态图当中去，第二个参数代表是需要回环检测detect_loop，这里直接默认设置需要。

// 位姿平移量间隔（关键帧）
 if((T - last_t).norm() > SKIP_DIS)
 {
     vector<cv::Point3f> point_3d; 
     vector<cv::Point2f> point_2d_uv; 
     vector<cv::Point2f> point_2d_normal;
     vector<double> point_id;

     // 特征点
     for (unsigned int i = 0; i < point_msg->points.size(); i++)
     {
         cv::Point3f p_3d;
         p_3d.x = point_msg->points[i].x;
         p_3d.y = point_msg->points[i].y;
         p_3d.z = point_msg->points[i].z;
         // 世界坐标
         point_3d.push_back(p_3d);

         cv::Point2f p_2d_uv, p_2d_normal;
         double p_id;
         p_2d_normal.x = point_msg->channels[i].values[0];
         p_2d_normal.y = point_msg->channels[i].values[1];
         p_2d_uv.x = point_msg->channels[i].values[2];
         p_2d_uv.y = point_msg->channels[i].values[3];
         p_id = point_msg->channels[i].values[4];
         // 归一化相机平面坐标
         point_2d_normal.push_back(p_2d_normal);
         // 像素坐标
         point_2d_uv.push_back(p_2d_uv);
         // 特征点id
         point_id.push_back(p_id);

         //printf("u %f, v %f \n", p_2d_uv.x, p_2d_uv.y);
     }
     // 构造关键帧
     KeyFrame* keyframe = new KeyFrame(pose_msg->header.stamp.toSec(), frame_index, T, R, image,
                        point_3d, point_2d_uv, point_2d_normal, point_id, sequence);   
     m_process.lock();
     start_flag = 1;
     // 添加关键帧
     posegraph.addKeyFrame(keyframe, 1);
     m_process.unlock();
     frame_index++;
     last_t = T;

5.3 PoseGraph

PoseGraph作为loop_fusion整个程序里面最重量级的类。整个程序都是维护这个main函数里面定义的全局变量PoseGraph posegraph来进行的。通过PoseGraph 的类函数，addKeyFrame来进行关键帧的添加。
list<KeyFrame*>keyframelist 作为最重要的类成员，整个回环检测的过程就是用来维护这个关键帧链表list，当检测到回环的时候，更新这个链表的数据，即关键帧的位姿。
BriefDatabase db 图像数据库信息，通过不断更新这个数据库，可以用来查询，即回环检测有没有回到之前曾经来过的地方。

5.3.1 addKeyFrame

检查该关键帧的序号是否有跳变，在整个loop_fusion里面，每当出现new_sequence()时候，关键帧的sequence会自增1，为什么一直需要关心一个关键帧的sequence呢？因为这里一个关键帧的位姿的参考系是建立在某一个sequence下面的，不同的sequence对应了不同w_t_vio,w_r_vio，就是该sequence 和base_sequence之间坐标系的转换关系。
posegraph坐标系是以world frame作为坐标系的，world frame为base sequence(载入原来的图像数据库)或者（没有载入图像数据库的话）用sequence=1的序列作为坐标系。PoesGraph里面的两个类成员 w_t_vio,w_r_vio描述的就是当前序列的第一帧，与世界坐标系之间的转换关系。

//shift to base frame
Vector3d vio_P_cur;
Matrix3d vio_R_cur;
// 新加入一段
if (sequence_cnt != cur_kf->sequence)
{
    sequence_cnt++;
    sequence_loop.push_back(0);
    w_t_vio = Eigen::Vector3d(0, 0, 0);
    w_r_vio = Eigen::Matrix3d::Identity();
    m_drift.lock();
    t_drift = Eigen::Vector3d(0, 0, 0);
    r_drift = Eigen::Matrix3d::Identity();
    m_drift.unlock();
}

// 转换成世界坐标
cur_kf->getVioPose(vio_P_cur, vio_R_cur);
vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;
vio_R_cur = w_r_vio *  vio_R_cur;
cur_kf->updateVioPose(vio_P_cur, vio_R_cur);
cur_kf->index = global_index;
global_index++;
int loop_index = -1;

5.3.2 detectLoop

通过detectLoop()，查找到一个最合适得分最高的闭环候选帧（如果没找到，loop_index=-1）,此时本质是通过bag of word中的db.query()来进行查找的。并且无论有没有查找到，都用db.add()把当前关键帧的描述子添加到图像的数据库db中去。

/**
 * 闭环检测
 * 1、在db中查找当前描述子匹配帧，返回4帧候选帧，要求是当前帧50帧之前的帧
 * 2、添加当前帧描述子到db
 * 3、计算候选帧匹配得分，最大得分超过0.05，其他得分超过最大得分的0.3倍，认为找到闭环
 * 4、返回最早的闭环帧idx
*/
int PoseGraph::detectLoop(KeyFrame* keyframe, int frame_index)
{
    // put image into image_pool; for visualization
    // 图像添加文字，展示特征点数量，加入集合，用于展示
    cv::Mat compressed_image;
    if (DEBUG_IMAGE)
    {
        int feature_num = keyframe->keypoints.size();
        cv::resize(keyframe->image, compressed_image, cv::Size(376, 240));
        putText(compressed_image, "feature_num:" + to_string(feature_num), cv::Point2f(10, 10), cv::FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255));
        image_pool[frame_index] = compressed_image;
    }
    TicToc tmp_t;
    //first query; then add this frame into database!
    QueryResults ret;
    TicToc t_query;
    // 在db中查找当前描述子匹配帧，返回4帧，要求是当前帧50帧之前的帧
    db.query(keyframe->brief_descriptors, ret, 4, frame_index - 50);
    //printf("query time: %f", t_query.toc());
    //cout << "Searching for Image " << frame_index << ". " << ret << endl;

    TicToc t_add;
    // 添加当前帧描述子到db
    db.add(keyframe->brief_descriptors);
    //printf("add feature time: %f", t_add.toc());
    // ret[0] is the nearest neighbour's score. threshold change with neighour score
    bool find_loop = false;
    cv::Mat loop_result;
    if (DEBUG_IMAGE)
    {
        // 最接近的一帧图像的得分
        loop_result = compressed_image.clone();
        if (ret.size() > 0)
            putText(loop_result, "neighbour score:" + to_string(ret[0].Score), cv::Point2f(10, 50), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255));
    }
    // visual loop result 
    if (DEBUG_IMAGE)
    {
        // 候选闭环帧，加上得分
        for (unsigned int i = 0; i < ret.size(); i++)
        {
            int tmp_index = ret[i].Id;
            auto it = image_pool.find(tmp_index);
            cv::Mat tmp_image = (it->second).clone();
            putText(tmp_image, "index:  " + to_string(tmp_index) + "loop score:" + to_string(ret[i].Score), cv::Point2f(10, 50), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255));
            cv::hconcat(loop_result, tmp_image, loop_result);
        }
    }
    // a good match with its nerghbour
    // 最大得分超过0.05，其他得分超过最大得分的0.3倍，认为找到闭环
    if (ret.size() >= 1 &&ret[0].Score > 0.05)
        for (unsigned int i = 1; i < ret.size(); i++)
        {
            //if (ret[i].Score > ret[0].Score * 0.3)
            if (ret[i].Score > 0.015)
            {          
                find_loop = true;
                int tmp_index = ret[i].Id;
                if (DEBUG_IMAGE && 0)
                {
                    auto it = image_pool.find(tmp_index);
                    cv::Mat tmp_image = (it->second).clone();
                    putText(tmp_image, "loop score:" + to_string(ret[i].Score), cv::Point2f(10, 50), cv::FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255));
                    cv::hconcat(loop_result, tmp_image, loop_result);
                }
            }

        }
/*
    if (DEBUG_IMAGE)
    {
        cv::imshow("loop_result", loop_result);
        cv::waitKey(20);
    }
*/
    // 返回最早的闭环帧idx
    if (find_loop && frame_index > 50)
    {
        int min_index = -1;
        for (unsigned int i = 0; i < ret.size(); i++)
        {
            if (min_index == -1 || (ret[i].Id < min_index && ret[i].Score > 0.015))
                min_index = ret[i].Id;
        }
        return min_index;
    }
    else
        return -1;

}

5.3.2 findConnection

若detectLoop()能够找到相似的一帧，此时，通过findConnection() 判断新旧两帧关联，匹配点大于25对，相对的偏航角位移小于30度并且相对位移少于20m，则返回true,其余情况均返回false。在true的条件下，更新参考系转化的变量。并且把当前帧的序号加入到optimize_buf中去。

// 检测到闭环
if (loop_index != -1)
{
     //printf(" %d detect loop with %d \n", cur_kf->index, loop_index);
     // 闭环帧
     KeyFrame* old_kf = getKeyFrame(loop_index);

     /**
      * 计算当前帧与闭环帧之间的位姿差，判断是否闭环
      * 1、通过brief描述子，计算当前帧与闭环匹配帧之间的匹配点，要求大于25个
      * 2、3d-2d Pnp计算匹配闭环帧的位姿
      * 3、闭环帧与当前帧之间的位姿误差不能太大，才认为闭环
     */
     if (cur_kf->findConnection(old_kf))
     {
         // 记录历史上最早的闭环帧
         if (earliest_loop_index > loop_index || earliest_loop_index == -1)
             earliest_loop_index = loop_index;

         Vector3d w_P_old, w_P_cur, vio_P_cur;
         Matrix3d w_R_old, w_R_cur, vio_R_cur;
         // 闭环帧位姿
         old_kf->getVioPose(w_P_old, w_R_old);
         // 当前帧位姿
         cur_kf->getVioPose(vio_P_cur, vio_R_cur);

         Vector3d relative_t;
         Quaterniond relative_q;
         // 当前帧与闭环帧的位姿差量，根据特征点匹配算出来的
         relative_t = cur_kf->getLoopRelativeT();
         relative_q = (cur_kf->getLoopRelativeQ()).toRotationMatrix();
         // 以闭环帧位姿为基础，加上位姿差量，得到当前帧位姿
         w_P_cur = w_R_old * relative_t + w_P_old;
         w_R_cur = w_R_old * relative_q;
         double shift_yaw;
         Matrix3d shift_r;
         Vector3d shift_t; 
         if(use_imu)
         {
             // 当前帧由闭环匹配算出来的位姿，与里程计位姿，计算偏移量，认为前者是更精确的
             shift_yaw = Utility::R2ypr(w_R_cur).x() - Utility::R2ypr(vio_R_cur).x();
             shift_r = Utility::ypr2R(Vector3d(shift_yaw, 0, 0));
         }
         else
             shift_r = w_R_cur * vio_R_cur.transpose();
         shift_t = w_P_cur - w_R_cur * vio_R_cur.transpose() * vio_P_cur; 
         // shift vio pose of whole sequence to the world frame
         // todo
         if (old_kf->sequence != cur_kf->sequence && sequence_loop[cur_kf->sequence] == 0)
         {  
             w_r_vio = shift_r;
             w_t_vio = shift_t;
             // 当前帧加上闭环偏移量之后的位姿
             vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;
             vio_R_cur = w_r_vio *  vio_R_cur;
             cur_kf->updateVioPose(vio_P_cur, vio_R_cur);
             list<KeyFrame*>::iterator it = keyframelist.begin();
             for (; it != keyframelist.end(); it++)   
             {
                 // 加上闭环偏移量
                 if((*it)->sequence == cur_kf->sequence)
                 {
                     Vector3d vio_P_cur;
                     Matrix3d vio_R_cur;
                     (*it)->getVioPose(vio_P_cur, vio_R_cur);
                     vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;
                     vio_R_cur = w_r_vio *  vio_R_cur;
                     (*it)->updateVioPose(vio_P_cur, vio_R_cur);
                 }
             }
             sequence_loop[cur_kf->sequence] = 1;
         }
         m_optimize_buf.lock();
         // 加入优化队列
         optimize_buf.push(cur_kf->index);
         m_optimize_buf.unlock();
     }

添加到keyframelist

最终，把该帧keyframe 添加到keyframelist中去。从此，keyframelist又增添了一个关键帧元素

m_keyframelist.lock();
Vector3d P;
Matrix3d R;
cur_kf->getVioPose(P, R);
P = r_drift * P + t_drift;
R = r_drift * R;
cur_kf->updatePose(P, R);
Quaterniond Q{R};
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header.stamp = ros::Time(cur_kf->time_stamp);
pose_stamped.header.frame_id = "world";
pose_stamped.pose.position.x = P.x() + VISUALIZATION_SHIFT_X;
pose_stamped.pose.position.y = P.y() + VISUALIZATION_SHIFT_Y;
pose_stamped.pose.position.z = P.z();
pose_stamped.pose.orientation.x = Q.x();
pose_stamped.pose.orientation.y = Q.y();
pose_stamped.pose.orientation.z = Q.z();
pose_stamped.pose.orientation.w = Q.w();
path[sequence_cnt].poses.push_back(pose_stamped);
path[sequence_cnt].header = pose_stamped.header;

if (SAVE_LOOP_PATH)
{
    ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
    loop_path_file.setf(ios::fixed, ios::floatfield);
    loop_path_file.precision(0);
    loop_path_file << cur_kf->time_stamp * 1e9 << ",";
    loop_path_file.precision(5);
    loop_path_file  << P.x() << ","
          << P.y() << ","
          << P.z() << ","
          << Q.w() << ","
          << Q.x() << ","
          << Q.y() << ","
          << Q.z() << ","
          << endl;
    loop_path_file.close();
}
//draw local connection
if (SHOW_S_EDGE)
{
    // 相邻边
    list<KeyFrame*>::reverse_iterator rit = keyframelist.rbegin();
    for (int i = 0; i < 4; i++)
    {
        if (rit == keyframelist.rend())
            break;
        Vector3d conncected_P;
        Matrix3d connected_R;
        if((*rit)->sequence == cur_kf->sequence)
        {
            (*rit)->getPose(conncected_P, connected_R);
            posegraph_visualization->add_edge(P, conncected_P);
        }
        rit++;
    }
}
if (SHOW_L_EDGE)
{
    // 闭环边
    if (cur_kf->has_loop)
    {
        //printf("has loop \n");
        KeyFrame* connected_KF = getKeyFrame(cur_kf->loop_index);
        Vector3d connected_P,P0;
        Matrix3d connected_R,R0;
        connected_KF->getPose(connected_P, connected_R);
        //cur_kf->getVioPose(P0, R0);
        cur_kf->getPose(P0, R0);
        if(cur_kf->sequence > 0)
        {
            //printf("add loop into visual \n");
            posegraph_visualization->add_loopedge(P0, connected_P + Vector3d(VISUALIZATION_SHIFT_X, VISUALIZATION_SHIFT_Y, 0));
        }
        
    }
}
//posegraph_visualization->add_pose(P + Vector3d(VISUALIZATION_SHIFT_X, VISUALIZATION_SHIFT_Y, 0), Q);

keyframelist.push_back(cur_kf);
publish();
m_keyframelist.unlock();

optimize4DoF

optimize_buf一有东西，意味着该帧已经被检测出回环了，因此就开始优化，优化的对象就是keyframelist中每个关键帧的四个自由度，包括x,y,z,yaw。同样是ceres问题求解

/**
 * 构建图优化，优化位姿，（x,y,z,yaw）
*/
void PoseGraph::optimize4DoF()
{
    while(true)
    {
        int cur_index = -1;
        int first_looped_index = -1;
        m_optimize_buf.lock();
        while(!optimize_buf.empty())
        {
            cur_index = optimize_buf.front();
            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();
            // 当前帧
            KeyFrame* cur_kf = getKeyFrame(cur_index);

            int max_length = cur_index + 1;

            // w^t_i   w^q_i
            double t_array[max_length][3];
            Quaterniond q_array[max_length];
            double euler_array[max_length][3];
            double sequence_array[max_length];

            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);
            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;
                Quaterniond tmp_q;
                Matrix3d tmp_r;
                Vector3d tmp_t;
                (*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
                // 添加边，每一帧与前面4帧建立边
                for (int j = 1; j < 5; j++)
                {
                  if (i - j >= 0 && sequence_array[i] == sequence_array[i-j])
                  {
                    Vector3d euler_conncected = Utility::R2ypr(q_array[i-j].toRotationMatrix());
                    Vector3d relative_t(t_array[i][0] - t_array[i-j][0], t_array[i][1] - t_array[i-j][1], t_array[i][2] - t_array[i-j][2]);
                    relative_t = q_array[i-j].inverse() * relative_t;
                    double relative_yaw = euler_array[i][0] - euler_array[i-j][0];
                    ceres::CostFunction* cost_function = FourDOFError::Create( relative_t.x(), relative_t.y(), relative_t.z(),
                                                   relative_yaw, euler_conncected.y(), euler_conncected.z());
                    problem.AddResidualBlock(cost_function, NULL, euler_array[i-j], 
                                            t_array[i-j], 
                                            euler_array[i], 
                                            t_array[i]);
                  }
                }

                //add loop edge
                // 添加闭环边，与闭环帧建立边
                if((*it)->has_loop)
                {
                    assert((*it)->loop_index >= first_looped_index);
                    int connected_index = getKeyFrame((*it)->loop_index)->local_index;
                    Vector3d euler_conncected = Utility::R2ypr(q_array[connected_index].toRotationMatrix());
                    Vector3d relative_t;
                    relative_t = (*it)->getLoopRelativeT();
                    double relative_yaw = (*it)->getLoopRelativeYaw();
                    ceres::CostFunction* cost_function = FourDOFWeightError::Create( relative_t.x(), relative_t.y(), relative_t.z(),
                                                                               relative_yaw, euler_conncected.y(), euler_conncected.z());
                    problem.AddResidualBlock(cost_function, loss_function, euler_array[connected_index], 
                                                                  t_array[connected_index], 
                                                                  euler_array[i], 
                                                                  t_array[i]);
                    
                }
                
                if ((*it)->index == cur_index)
                    break;
                i++;
            }
            m_keyframelist.unlock();

            ceres::Solve(options, &problem, &summary);
            //std::cout << summary.BriefReport() << "\n";
            
            //printf("pose optimization time: %f \n", tmp_t.toc());
            /*
            for (int j = 0 ; j < i; j++)
            {
                printf("optimize i: %d p: %f, %f, %f\n", j, t_array[j][0], t_array[j][1], t_array[j][2] );
            }
            */
            m_keyframelist.lock();
            i = 0;
            // 更新图中顶点位姿，闭环帧到当前帧之间（不包括当前帧）
            for (it = keyframelist.begin(); it != keyframelist.end(); it++)
            {
                if ((*it)->index < first_looped_index)
                    continue;
                Quaterniond tmp_q;
                tmp_q = Utility::ypr2R(Vector3d(euler_array[i][0], euler_array[i][1], euler_array[i][2]));
                Vector3d tmp_t = Vector3d(t_array[i][0], t_array[i][1], t_array[i][2]);
                Matrix3d tmp_r = tmp_q.toRotationMatrix();
                (*it)-> updatePose(tmp_t, tmp_r);

                if ((*it)->index == cur_index)
                    break;
                i++;
            }

            Vector3d cur_t, vio_t;
            Matrix3d cur_r, vio_r;
            // 当前帧优化后位姿 todo
            cur_kf->getPose(cur_t, cur_r);
            // 当前帧优化前位姿
            cur_kf->getVioPose(vio_t, vio_r);
            m_drift.lock();
            // 计算优化前后位姿差量
            yaw_drift = Utility::R2ypr(cur_r).x() - Utility::R2ypr(vio_r).x();
            r_drift = Utility::ypr2R(Vector3d(yaw_drift, 0, 0));
            t_drift = cur_t - r_drift * vio_t;
            m_drift.unlock();
            //cout << "t_drift " << t_drift.transpose() << endl;
            //cout << "r_drift " << Utility::R2ypr(r_drift).transpose() << endl;
            //cout << "yaw drift " << yaw_drift << endl;

            it++;
            // 更新当前帧之后的关键帧位姿
            for (; it != keyframelist.end(); it++)
            {
                Vector3d P;
                Matrix3d R;
                (*it)->getVioPose(P, R);
                P = r_drift * P + t_drift;
                R = r_drift * R;
                (*it)->updatePose(P, R);
            }
            m_keyframelist.unlock();
            updatePath();
        }

        std::chrono::milliseconds dura(2000);
        std::this_thread::sleep_for(dura);
    }
    return;
}

6.全局融合globalOptNode.cpp

6.1 globalEstimator

…详情请参照古月居