VINS-Mono代码阅读笔记（十一）：进入pose_graph节点代码分析

本篇笔记紧接着上一篇VINS-Mono代码阅读笔记（十）：vins_estimator中的非线性优化，来接着学习VINS-Mono系统中重定位和全局优化部部分的代码。这部分代码在pose_graph节点中，首先看一下在第一篇笔记中贴出来的下面这张系统运行时的Node graph图。

从这张图中可以看到pose_graph在系统节点中所处的位置以及它要订阅和发布的topic有哪些，rviz就是根据posegraph节点中发布的topic来进行可视化显示的。那么下面就根据这张图，结合代码和论文来学习这部分代码。

1.入口main函数

pose_graph节点的main函数在pose_graph_node.cpp中，启动pose_graph节点的启动代码在euroc.launch（vin_estimator/launch下的所有launch文件）文件当中，代码如下：

<launch>
    <arg name="config_path" default = "$(find feature_tracker)/../config/euroc/euroc_config.yaml" />
	  <arg name="vins_path" default = "$(find feature_tracker)/../config/../" />
    
    <node name="feature_tracker" pkg="feature_tracker" type="feature_tracker" output="log">
        <param name="config_file" type="string" value="$(arg config_path)" />
        <param name="vins_folder" type="string" value="$(arg vins_path)" />
    </node>

    <node name="vins_estimator" pkg="vins_estimator" type="vins_estimator" output="screen">
       <param name="config_file" type="string" value="$(arg config_path)" />
       <param name="vins_folder" type="string" value="$(arg vins_path)" />
    </node>

    <node name="pose_graph" pkg="pose_graph" type="pose_graph" output="screen">
        <param name="config_file" type="string" value="$(arg config_path)" />
        <param name="visualization_shift_x" type="int" value="0" />
        <param name="visualization_shift_y" type="int" value="0" />
        <param name="skip_cnt" type="int" value="0" />
        <param name="skip_dis" type="double" value="0" />
    </node>

</launch>

这个launch文件里配置了所有节点运行时需要的参数，这里我们需要留意一下pose_graph下的几个参数，在后边的代码流程分析中会看到它们的身影。

pose_graph中main函数代码如下：

int main(int argc, char **argv)
{
    //1.ROS相关初始化
    ros::init(argc, argv, "pose_graph");
    ros::NodeHandle n("~");
    //2.注册topic的发布
    posegraph.registerPub(n);

    // read param
    //3.读取相关参数
    n.getParam("visualization_shift_x", VISUALIZATION_SHIFT_X);//在euroc.launch中为0
    n.getParam("visualization_shift_y", VISUALIZATION_SHIFT_Y);//在euroc.launch中为0
    n.getParam("skip_cnt", SKIP_CNT);//在euroc.launch中为0
    n.getParam("skip_dis", SKIP_DIS);//在euroc.launch中为0
    std::string config_file;
    n.getParam("config_file", config_file);
    cv::FileStorage fsSettings(config_file, cv::FileStorage::READ);
    if(!fsSettings.isOpened())
    {
        std::cerr << "ERROR: Wrong path to settings" << std::endl;
    }

    double camera_visual_size = fsSettings["visualize_camera_size"];
    cameraposevisual.setScale(camera_visual_size);
    cameraposevisual.setLineWidth(camera_visual_size / 10.0);

    //配置文件中该值为1
    LOOP_CLOSURE = fsSettings["loop_closure"];
    std::string IMAGE_TOPIC;
    int LOAD_PREVIOUS_POSE_GRAPH;
    //闭环检测的判断
    if (LOOP_CLOSURE)
    {
        //闭环情况下相关参数的读取
        ROW = fsSettings["image_height"];
        COL = fsSettings["image_width"];
        std::string pkg_path = ros::package::getPath("pose_graph");
        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;
        m_camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile(config_file.c_str());

        fsSettings["image_topic"] >> IMAGE_TOPIC;        
        fsSettings["pose_graph_save_path"] >> POSE_GRAPH_SAVE_PATH;
        fsSettings["output_path"] >> VINS_RESULT_PATH;
        fsSettings["save_image"] >> DEBUG_IMAGE;

        // create folder if not exists
        FileSystemHelper::createDirectoryIfNotExists(POSE_GRAPH_SAVE_PATH.c_str());
        FileSystemHelper::createDirectoryIfNotExists(VINS_RESULT_PATH.c_str());

        VISUALIZE_IMU_FORWARD = fsSettings["visualize_imu_forward"];
        LOAD_PREVIOUS_POSE_GRAPH = fsSettings["load_previous_pose_graph"];
        FAST_RELOCALIZATION = fsSettings["fast_relocalization"];
        VINS_RESULT_PATH = VINS_RESULT_PATH + "/vins_result_loop.csv";
        std::ofstream fout(VINS_RESULT_PATH, std::ios::out);
        fout.close();
        fsSettings.release();
        //4.加载先前保存的位姿图
        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;
        }
    }

    fsSettings.release();
    /**
     * 5.订阅了七个topic
    */
    //IMU前向传播
    ros::Subscriber sub_imu_forward = n.subscribe("/vins_estimator/imu_propagate", 2000, imu_forward_callback);
    //订阅里程计topic
    ros::Subscriber sub_vio = n.subscribe("/vins_estimator/odometry", 2000, vio_callback);
    //订阅图像img
    ros::Subscriber sub_image = n.subscribe(IMAGE_TOPIC, 2000, image_callback);
    //订阅keyframe pose
    ros::Subscriber sub_pose = n.subscribe("/vins_estimator/keyframe_pose", 2000, pose_callback);
    //订阅相机到IMU之间的外参
    ros::Subscriber sub_extrinsic = n.subscribe("/vins_estimator/extrinsic", 2000, extrinsic_callback);
    //订阅点云topic
    ros::Subscriber sub_point = n.subscribe("/vins_estimator/keyframe_point", 2000, point_callback);
    //订阅重定位相对位姿
    ros::Subscriber sub_relo_relative_pose = n.subscribe("/vins_estimator/relo_relative_pose", 2000, relo_relative_pose_callback);
    //6.创建pose_graph中要发布的5个topic的发布器
    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_key_odometrys = n.advertise<visualization_msgs::Marker>("key_odometrys", 1000);
    pub_vio_path = n.advertise<nav_msgs::Path>("no_loop_path", 1000);
    pub_match_points = n.advertise<sensor_msgs::PointCloud>("match_points", 100);

    std::thread measurement_process;
    std::thread keyboard_command_process;
    //7.创建process线程，也相当于是pose_graph的主线程
    measurement_process = std::thread(process);
    //8.创建command线程，监听命令行中键盘的输入
    keyboard_command_process = std::thread(command);


    ros::spin();

    return 0;
}

总结一下，main函数中主要做了以下工作：

main函数中，订阅的topic回调函数中对msg的解析处理基本上都是将解析出来的数据存入本pose_graph节点中的buf当中，这些数据在后边的process线程中会用到。process线程当中的工作才是posegraph节点的灵魂所在，所以也是我们后边要重点分析的。

2.poseGraph构造函数

在main函数中有个posegraph对象，该对象是在pose_graph_node.cpp中创建的全局对象。PoseGraph类的构造函数在创建该对象的时候，创建了一个优化4自由度位姿的线程，全局优化就是在这个线程t_optimization当中进行的，optimize4DoF我们后边会重点关注。

/**
 * PoseGraph构造函数
*/
PoseGraph::PoseGraph()
{
    posegraph_visualization = new CameraPoseVisualization(1.0, 0.0, 1.0, 1.0);
    posegraph_visualization->setScale(0.1);
    posegraph_visualization->setLineWidth(0.01);
    //创建位姿优化线程
    t_optimization = std::thread(&PoseGraph::optimize4DoF, this);
    earliest_loop_index = -1;
    t_drift = Eigen::Vector3d(0, 0, 0);
    yaw_drift = 0;
    r_drift = Eigen::Matrix3d::Identity();
    w_t_vio = Eigen::Vector3d(0, 0, 0);
    w_r_vio = Eigen::Matrix3d::Identity();
    global_index = 0;
    sequence_cnt = 0;
    sequence_loop.push_back(0);
    base_sequence = 1;

}

3.process线程

process线程入口函数代码如下：

/**
 * process线程入口函数
*/
void process()
{
    //1.是否进行闭环检测的判断,不过不做闭环检测则直接返回
    if (!LOOP_CLOSURE)
        return;
    while (true)//该线程一直在循环执行
    {
        sensor_msgs::ImageConstPtr image_msg = NULL;
        sensor_msgs::PointCloudConstPtr point_msg = NULL;
        nav_msgs::Odometry::ConstPtr pose_msg = NULL;

        // find out the messages with same time stamp
        m_buf.lock();
        //2.获取“相同时间戳”内的pose_msg、image_msg、point_msg
        if(!image_buf.empty() && !point_buf.empty() && !pose_buf.empty())
        {
            //图像时间戳晚于位姿时间戳，则将该位姿pop出去
            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())
            {
                //图像时间戳晚于点云时间戳，则将点云位姿pop出去
                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_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();
            }
        }
        m_buf.unlock();

        if (pose_msg != NULL)
        {
            //printf(" pose time %f \n", pose_msg->header.stamp.toSec());
            //printf(" point time %f \n", point_msg->header.stamp.toSec());
            //printf(" image time %f \n", image_msg->header.stamp.toSec());
            // skip fisrt few SKIP_FIRST_CNT值为10
            //3.剔除掉了前SKIP_FIRST_CNT（值为10）帧数据
            if (skip_first_cnt < SKIP_FIRST_CNT)
            {
                skip_first_cnt++;
                continue;
            }
            //SKIP_CNT在euroc.launch中为0
            if (skip_cnt < SKIP_CNT)
            {
                skip_cnt++;
                continue;
            }
            else
            {
                skip_cnt = 0;
            }
            //4.解析image_msg信息存入ptr变量当中
            cv_bridge::CvImageConstPtr ptr;
            if (image_msg->encoding == "8UC1")
            {
                sensor_msgs::Image img;
                img.header = image_msg->header;
                img.height = image_msg->height;
                img.width = image_msg->width;
                img.is_bigendian = image_msg->is_bigendian;
                img.step = image_msg->step;
                img.data = image_msg->data;
                img.encoding = "mono8";
                ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8);
            }
            else
                ptr = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::MONO8);
            
            cv::Mat image = ptr->image;
            // build keyframe
            //5.将当前图像位置和上次图像的位置之间的距离大于SKIP_DIS的图像，创建为关键帧并加入到位姿图当中
            Vector3d T = Vector3d(pose_msg->pose.pose.position.x,
                                  pose_msg->pose.pose.position.y,
                                  pose_msg->pose.pose.position.z);
            Matrix3d R = Quaterniond(pose_msg->pose.pose.orientation.w,
                                     pose_msg->pose.pose.orientation.x,
                                     pose_msg->pose.pose.orientation.y,
                                     pose_msg->pose.pose.orientation.z).toRotationMatrix();
            //计算当前帧图像的位置距离上帧图像之间的距离，并判断是否大于SKIP_DIS
            if((T - last_t).norm() > SKIP_DIS)//SKIP_DIS值为0
            {
                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);
                    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;
                //位姿图中加入关键帧，flag_detect_loop设置为1
                posegraph.addKeyFrame(keyframe, 1);
                m_process.unlock();
                frame_index++;
                last_t = T;
            }
        }
        //6.线程休眠5ms
        std::chrono::milliseconds dura(5);
        std::this_thread::sleep_for(dura);
    }
}

process中的核心操作，就是根据estimator节点当中发送的关键帧位姿来创建“符合条件”的新关键帧添加到位姿图当中。所有的闭环检测和重定位等操作都在posegraph.addKeyFrame当中进行，后边重点分析这块。

4.command线程

command线程中要处理的工作相对要少的多，主要是检测用户键盘输入是否为's'和'n'。如果接收到了键盘输入的's'，则保存位姿图到指定的路径中；如果接收到的键盘输入为'n'，则创建新的位姿图序列，这里最多支持5个位姿图序列。每一个关键帧都有其所在的序列（对应KeyFrame类当中的int sequence）。这里保存位姿图的操作，在后边将闭环检测和重定位分析清楚后对其再做解释。

void command()
{
    if (!LOOP_CLOSURE)
        return;
    while(1)
    {
        //检查用户键盘输入是否为s
        char c = getchar();
        if (c == 's')
        {
            m_process.lock();
            //用户键盘输入s后，保存当前的位姿图(地图)
            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();
        }
        //检查用户键盘输入是否为n，为n则开始一个新的图像序列
        if (c == 'n')
            new_sequence();

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