欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

VINS-Mono代码阅读笔记(十一):进入pose_graph节点代码分析

程序员文章站 2022-03-07 11:36:24
...

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

VINS-Mono代码阅读笔记(十一):进入pose_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函数中主要做了以下工作:

VINS-Mono代码阅读笔记(十一):进入pose_graph节点代码分析

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

 

相关标签: SLAM