VINS-Mono源码分析6— pose_graph1(回环检测与重定位)

ros::init(argc, argv, "pose_graph");
ros::NodeHandle n("~");

pose_graph包的入口，ROS程序的初始处理。

posegraph.registerPub(n);

void PoseGraph::registerPub(ros::NodeHandle &n)
{
   
    //RVIZ中显示的轨迹绿线
    pub_pg_path = n.advertise<nav_msgs::Path>("pose_graph_path", 1000);
    pub_base_path = n.advertise<nav_msgs::Path>("base_path", 1000);
    pub_pose_graph = n.advertise<visualization_msgs::MarkerArray>("pose_graph", 1000);
    for (int i = 1; i < 10; i++)
        pub_path[i] = n.advertise<nav_msgs::Path>("path_" + to_string(i), 1000);
}

一些待发布话题的定义

n.getParam("visualization_shift_x", VISUALIZATION_SHIFT_X);//0
n.getParam("visualization_shift_y", VISUALIZATION_SHIFT_Y);//0
n.getParam("skip_cnt", SKIP_CNT);//0
n.getParam("skip_dis", SKIP_DIS);//0
std::string config_file;
n.getParam("config_file", config_file);

从～/vins_estimator/launch文件夹下的euroc.launch文件中读取参数。

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

读取设置文件，默认是～/config/euroc文件夹下的euroc_config.yaml

double camera_visual_size = fsSettings["visualize_camera_size"];//0.4
cameraposevisual.setScale(camera_visual_size);//相框显示的尺度设置
cameraposevisual.setLineWidth(camera_visual_size / 10.0);//线宽

从设置文件读取参数，用于配置RVIZ下相机位姿状态的显示。

LOOP_CLOSURE = fsSettings["loop_closure"];//1
std::string IMAGE_TOPIC;
//默认为0
int LOAD_PREVIOUS_POSE_GRAPH;

从设置文件读取参数，用于配置是否开启回环检测，默认开启。

if (LOOP_CLOSURE)
{
   
    ROW = fsSettings["image_height"];//行数480
    COL = fsSettings["image_width"];//列数752
    //读取"pose_graph包的地址
    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);
}

从设置文件读取各种参数，注意loadVocabulary()函数，

void PoseGraph::loadVocabulary(std::string voc_path)
{
   
    voc = new BriefVocabulary(voc_path);
    db.setVocabulary(*voc, false, 0);
}

它的作用是加载词典，图像数据库的一些初始化操作，参考博客《VINS-Mono中的DBoW2关键代码注释》。

ros::Subscriber sub_imu_forward = n.subscribe("/vins_estimator/imu_propagate", 2000, imu_forward_callback);
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_relo_relative_pose = n.subscribe("/vins_estimator/relo_relative_pose", 2000, relo_relative_pose_callback);

订阅的话题及回调函数。

分析一下各种回调函数吧！

void imu_forward_callback(const nav_msgs::Odometry::ConstPtr &forward_msg)
{
   
    if (VISUALIZE_IMU_FORWARD)
    {
   
       ......
    }
}

目前没用到，修改参数文件的相关参数，可调用此函数

void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
{
   
    //vio_t和vio_q是IMU在世界坐标系的位姿
    Vector3d vio_t(pose_msg->pose.pose.position.x, pose_msg->pose.pose.position.y, pose_msg->pose.pose.position.z);
    Quaterniond vio_q;
    vio_q.w() = pose_msg->pose.pose.orientation.w;
    vio_q.x() = pose_msg->pose.pose.orientation.x;
    vio_q.y() = pose_msg->pose.pose.orientation.y;
    vio_q.z() = pose_msg->pose.pose.orientation.z;
    //w_r_vio默认单位阵、w_t_vio默认零向量
    vio_t = posegraph.w_r_vio * vio_t + posegraph.w_t_vio;
    vio_q = posegraph.w_r_vio *  vio_q;
    //r_drift默认单位阵、t_drift默认零向量，回环检测后值会变化
    vio_t = posegraph.r_drift * vio_t + posegraph.t_drift;
    vio_q = posegraph.r_drift * vio_q;

    Vector3d vio_t_cam;
    Quaterniond vio_q_cam;
    //相机在世界坐标系的位姿
    vio_t_cam = vio_t + vio_q * tic;
    vio_q_cam = vio_q * qic;        

    if (!VISUALIZE_IMU_FORWARD)
    {
   
        //相机位姿发布出去,待订阅后在RVIZ显示，即系统运行时的相框
        cameraposevisual.reset();
        cameraposevisual.add_pose(vio_t_cam, vio_q_cam);
        cameraposevisual.publish_by(pub_camera_pose_visual, pose_msg->header);
    }
    
    odometry_buf.push(vio_t_cam);
    if (odometry_buf.size() > 10)
    {
   
        odometry_buf.pop();
    }
    //key_odometrys的一些参数配置工作
    visualization_msgs::Marker key_odometrys;
    key_odometrys.header = pose_msg->header;
    key_odometrys.header.frame_id = "world";
    key_odometrys.ns = "key_odometrys";
    key_odometrys.type = visualization_msgs::Marker::SPHERE_LIST;
    key_odometrys.action = visualization_msgs::Marker::ADD;
    key_odometrys.pose.orientation.w = 1.0;
    key_odometrys.lifetime = ros::Duration();

    //static int key_odometrys_id = 0;
    key_odometrys.id = 0; //key_odometrys_id++;
    key_odometrys.scale.x = 0.1;
    key_odometrys.scale.y = 0.1;
    key_odometrys.scale.z = 0.1;
    key_odometrys.color.r = 1.0;
    //key_odometrys.color.g = 1.0;
    //key_odometrys.color.b = 1.0;
    key_odometrys.color.a = 1.0;
    //为了实现遍历功能，odometry_buf的元素先弹出，再压进去
    for (unsigned int i = 0; i < odometry_buf.size(); i++)
    {
   
        geometry_msgs::Point pose_marker;
        Vector3d vio_t;
        vio_t = odometry_buf.front();
        odometry_buf.pop();
        pose_marker.x = vio_t.x();
        pose_marker.y = vio_t.y();
        pose_marker.z = vio_t.z();
        key_odometrys.points.push_back(pose_marker);
        odometry_buf.push(vio_t);
    }
    //滑窗内相机轨迹发布出去,待订阅后在RVIZ显示，即系统运行时框框后的尾巴
    pub_key_odometrys.publish(key_odometrys);
   //默认不执行
    if (!LOOP_CLOSURE)
    {
   
        geometry_msgs::PoseStamped pose_stamped;
        pose_stamped.header = pose_msg->header;
        pose_stamped.header.frame_id = "world";
        pose_stamped.pose.position.x = vio_t.x();
        pose_stamped.pose.position.y = vio_t.y();
        pose_stamped.pose.position.z = vio_t.z();
        no_loop_path.header = pose_msg->header;
        no_loop_path.header.frame_id = "world";
        no_loop_path.poses.push_back(pose_stamped);
        pub_vio_path.publish(no_loop_path);
    }
}

void image_callback(const sensor_msgs::ImageConstPtr &image_msg)
{
   
    if(!LOOP_CLOSURE)
        return;
    m_buf.lock();
    //得到图片,存储在image_buf中
    image_buf.push(image_msg);
    m_buf.unlock();
   
    // 检测不稳定的相机流
    if (last_image_time == -1)
        last_image_time = image_msg->header.stamp.toSec();
    else if (image_msg->header.stamp.toSec() - last_image_time > 1.0 || image_msg->header.stamp.toSec() < last_image_time)
    {
   
        ROS_WARN("image discontinue! detect a new sequence!");
        new_sequence();
    }
    last_image_time = image_msg->header.stamp.toSec();
}

void pose_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
{
   
    if(!LOOP_CLOSURE)
        return;
    m_buf.lock(