本篇笔记紧接着上一篇VINS-Mono代码阅读笔记（十）：vins_estimator中的非线性优化，来接着学习VINS-Mono系统中重定位和全局优化部部分的代码。这部分代码在pose_graph节点中，首先看一下在第一篇笔记中贴出来的下面这张系统运行时的Node graph图。
从这张图中可以看到pose_graph在系统节点中所处的位置以及它要订阅和发布的topic有哪些，rviz就是根据posegraph节点中发布的topic来进行可视化显示的。那么下面就根据这张图，结合代码和论文来学习这部分代码。
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节点的灵魂所在，所以也是我们后边要重点分析的。
在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;
}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当中进行，后边重点分析这块。
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);
}
}VINS-Mono代码阅读笔记（十二）：将关键帧加入位姿图当中