回环检测的任务主要是为了检测机器人是否到达之前相同的位置，并且消累计的误差。VINS_Fusion的回环检测和VINS_Mono基本相似。只是loop_fusion中读取的很多参数是直接设定好的。回环部分我们先从pose_graph_node.cpp开始。

一、pose_graph_node.cpp

1.1 读取参数

首先看主函数，主要开头是设置基本的参数和读取.yaml文件和字典文件等。这部分其实没有太多需要理解的，就是简单的读取参数和复制，并且完成了ROS节点的初始化工作。

ros::init(argc, argv, "loop_fusion");ros::NodeHandle n("~");posegraph.registerPub(n);//和mono不同之处是直接赋值VISUALIZATION_SHIFT_X = 0;VISUALIZATION_SHIFT_Y = 0;SKIP_CNT = 0;SKIP_DIS = 0;if(argc != 2){printf("please intput: rosrun loop_fusion loop_fusion_node [config file] \n""for example: rosrun loop_fusion loop_fusion_node ""/home/tony-ws1/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml \n");return 0;}string config_file = argv[1];printf("config_file: %s\n", argv[1]);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;//与Mono不同直接不要判断是否回环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;BRIEF_PATTERN_FILE = pkg_path + "/../support_files/brief_pattern.yml";cout << "BRIEF_PATTERN_FILE" << BRIEF_PATTERN_FILE << endl;int pn = config_file.find_last_of('/');std::string configPath = config_file.substr(0, pn);std::string cam0Calib;fsSettings["cam0_calib"] >> cam0Calib;std::string cam0Path = configPath + "/" + cam0Calib;printf("cam calib path: %s\n", cam0Path.c_str());// 和前面一样，生成一个相机模型回环读入的yaml文件m_camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile(cam0Path.c_str());fsSettings["image0_topic"] >> IMAGE_TOPIC;        fsSettings["pose_graph_save_path"] >> POSE_GRAPH_SAVE_PATH;fsSettings["output_path"] >> VINS_RESULT_PATH;fsSettings["save_image"] >> DEBUG_IMAGE;LOAD_PREVIOUS_POSE_GRAPH = fsSettings["load_previous_pose_graph"];VINS_RESULT_PATH = VINS_RESULT_PATH + "/vio_loop.csv";//保存路径文件std::ofstream fout(VINS_RESULT_PATH, std::ios::out);fout.close();int USE_IMU = fsSettings["imu"];posegraph.setIMUFlag(USE_IMU);fsSettings.release();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;}

1.2 回调函数

这部分就是接收前端得到的地图点和特征点的话题，并且完成参数的赋值。

//vio回调函数根据pose_msg中的位姿得到imu位姿和cam位姿ros::Subscriber sub_vio = n.subscribe("/vins_estimator/odometry", 2000, vio_callback);//将image_msg放入到image_buf中，同时更具时间挫检测是否有新的图像序列ros::Subscriber sub_image = n.subscribe(IMAGE_TOPIC, 2000, image_callback);//图像位姿回调函数，把pose_msg放入到pose_bufros::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);//地图的点云，存放到point_bufros::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);

这里我们主挑出一个讲解，比如vio_callback这个回调函数，当接收到/usb_cam/image_raw这个话题时，就会调用回调函数。这里设计ROS的话题基本通讯不了解的可以查看我之前写的文章：ROS学习笔记——通讯机制_每日亿学的博客-CSDN博客

这部分的代码也很容易理解，就是定义和话题类型相同的对象，接收数据并且完成赋值。

void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
{//ROS_INFO("vio_callback!");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;vio_t = posegraph.w_r_vio * vio_t + posegraph.w_t_vio;vio_q = posegraph.w_r_vio *  vio_q;vio_t = posegraph.r_drift * vio_t + posegraph.t_drift;vio_q = posegraph.r_drift * vio_q;nav_msgs::Odometry odometry;odometry.header = pose_msg->header;odometry.header.frame_id = "world";odometry.pose.pose.position.x = vio_t.x();odometry.pose.pose.position.y = vio_t.y();odometry.pose.pose.position.z = vio_t.z();odometry.pose.pose.orientation.x = vio_q.x();odometry.pose.pose.orientation.y = vio_q.y();odometry.pose.pose.orientation.z = vio_q.z();odometry.pose.pose.orientation.w = vio_q.w();pub_odometry_rect.publish(odometry);Vector3d vio_t_cam;Quaterniond vio_q_cam;vio_t_cam = vio_t + vio_q * tic;vio_q_cam = vio_q * qic;        cameraposevisual.reset();cameraposevisual.add_pose(vio_t_cam, vio_q_cam);cameraposevisual.publish_by(pub_camera_pose_visual, pose_msg->header);}

1.3 创建线程

回环部分的核心内容就是在process这个线程中了。

    std::thread measurement_process;std::thread keyboard_command_process;measurement_process = std::thread(process);keyboard_command_process = std::thread(command);

二、process函数

2.1 时间戳对齐

一开始我们读取了，原始图像、前端的位姿和地图点。但是对应的时间戳并没有对齐，因为我们得到的位姿都是上一个原图处理的，所以位姿和地图点的时间都晚于原图的时间。所以VINS中一开始就是基于时间戳的大小来对齐，如果原图的时间撮大于则删除这帧的图像。

if(!image_buf.empty() && !point_buf.empty() && !pose_buf.empty()&&!Mono_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");}// 上面确保了image_buf <= point_buf && image_buf <= pose_buf// 下面根据pose时间找时间戳同步的原图和地图点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();    // 取出来posepose_buf.pop();while (!pose_buf.empty())   // 清空所有的pose，回环的帧率慢一些没关系pose_buf.pop();// 找到对应pose的原图while (image_buf.front()->header.stamp.toSec() < pose_msg->header.stamp.toSec()){image_buf.pop();}Mono_msg = Mono_buf.front();//传递符合的一帧if(!Mono_buf.empty())Mono_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();}

2.2 降采样

回环检测频率不需要很高，所以每帧的间隔需要一定的时间。首先一开始有一个简单的降频，SKIP_FIRST_CNT为10，skip_first_cnt加到大于10就向下。但是，skip_first_cnt它没有地方置位为0！之后skip_cnt < SKIP_CNT这里在初始化开始skip_cnt和SKIP_CNT都是0意味着skip_cnt要加到溢出才能进入下面内容！这里如果你希望关键帧之间间隔时间加长可以在skip_cnt < SKIP_CNT将skip_first_cnt置位并且更改SKIP_FIRST_CNT大小。

if (skip_first_cnt < SKIP_FIRST_CNT){skip_first_cnt++;continue;}if (skip_cnt < SKIP_CNT)// 降频，每隔SKIP_CNT帧处理一次超过SKIP_CNT=0{skip_cnt++;continue;}else{skip_cnt = 0;}// 通过cvbridge得到opencv格式的图像cv_bridge::CvImageConstPtr ptr;

完成降采样之后，就是对地图点和位姿进行赋值了，之后给关键帧进行初始化。

三、创建KeyFrame

创建关键帧需时间戳，关键帧序号，位姿、原始图像、地图点、特征点。

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

 进入KeyFrame的构造函数，基本就是对关键帧类中的成员变量进行赋值。主要重要的函数为：

computeWindowBRIEFPoint（）和computeBRIEFPoint（）。

// create keyframe online
/*** @brief Construct a new Key Frame:: Key Frame object，创建一个KF对象，计算已有特征点的描述子，同时额外提取fast角点并计算描述子* * @param[in] _time_stamp KF的时间戳* @param[in] _index KF的索引* @param[in] _vio_T_w_i vio节点中的位姿* @param[in] _vio_R_w_i * @param[in] _image 对应的原图* @param[in] _point_3d KF对应VIO节点中的世界坐标* @param[in] _point_2d_uv 像素坐标* @param[in] _point_2d_norm 归一化相机坐标* @param[in] _point_id 地图点的idx* @param[in] _sequence 序列号*/
// create keyframe online
KeyFrame::KeyFrame(double _time_stamp, int _index, Vector3d &_vio_T_w_i, Matrix3d &_vio_R_w_i, cv::Mat &_image,vector &_point_3d, vector &_point_2d_uv, vector &_point_2d_norm,vector &_point_id, int _sequence)
{time_stamp = _time_stamp;//传递时间戳  单目相机的时间戳使用cam0的时间戳index = _index;	//索引vio_T_w_i = _vio_T_w_i;vio_R_w_i = _vio_R_w_i;		//转化到转化到w->iT_w_i = vio_T_w_i;R_w_i = vio_R_w_i;origin_vio_T = vio_T_w_i;		origin_vio_R = vio_R_w_i;image = _image.clone();		//复制当前帧的图像cv::resize(image, thumbnail, cv::Size(80, 60));	// 这个缩小尺寸应该是为了可视化point_3d = _point_3d;		//特征点的世界坐标point_2d_uv = _point_2d_uv;	//像素坐标point_2d_norm = _point_2d_norm;	//归一化相机坐标point_id = _point_id;	//地图点的idxhas_loop = false;	// 注意默认还没有检测到回环 这里可以修改是否产生回环loop_index = -1;	//和那一帧发生回环has_fast_point = false;loop_info << 0, 0, 0, 0, 0, 0, 0, 0;sequence = _sequence;		//注意序列号暂时和单目相机同步 computeWindowBRIEFPoint();	//计算描述子   因为mono没有前端所以不提取特征点描述子computeBRIEFPoint();		//提取fast角点和描述子if(!DEBUG_IMAGE){image.release();}}

3.1 计算关键帧描述子

这里就是计算之前对齐时间戳的特征点进行描述子提取，BRIEF_PATTERN_FILE在main函数参数读取时已经玩出赋值。之后将之前的角点转化为opencv的keypoint类，之后调用extractor的仿函数(如果不太了解这个可以理解为把类的名字当作函数来使用，其实就是重构了()运算符号）。

/*** @brief 计算已有特征点的描述子* */
void KeyFrame::computeWindowBRIEFPoint()
{// 定义一个描述子计算的对象BriefExtractor extractor(BRIEF_PATTERN_FILE.c_str());//point_2d_uv检测到角点的像素坐标，这循环相当于转化为opencv之后的keypointfor(int i = 0; i < (int)point_2d_uv.size(); i++){cv::KeyPoint key;	//转化为opencv的格式key.pt = point_2d_uv[i];	// 像素坐标用来计算描述子window_keypoints.push_back(key);	// 转成opencv格式的特征点坐标}//注意window_brief_descriptor输出的描述子//注意window_keypoints为当前帧的特征点extractor(image, window_keypoints, window_brief_descriptors);
}

进入这个仿函数，就是直接调用DBOW的接口函数从而来计算描述子。 

/// @brief 提取描述子
/// @param im 输入图像
/// @param keys 角点
/// @param descriptors 输出的描述子
void BriefExtractor::operator() (const cv::Mat &im, vector &keys, vector &descriptors) const
{//调用dbow的接口计算描述子m_brief.compute(im, keys, descriptors);
}

3.3  额外提取FAST角点和描述子

VINS中为了防止前端传递特征点数目过少，这里格外检测FAST角点并且计算它的描述子。代码中直接调用opencv中的cv::FAST检测，并且将检测的角点归一化存储。

// 额外提取fast特征点并计算描述子
void KeyFrame::computeBRIEFPoint()
{//同样的步骤提取描述子文件BriefExtractor extractor(BRIEF_PATTERN_FILE.c_str());const int fast_th = 20; //注意 corner detector response threshold这里可以修改阈值if(1)cv::FAST(image, keypoints, fast_th, true);//提取fast特征点else{vector tmp_pts;cv::goodFeaturesToTrack(image, tmp_pts, 500, 0.01, 10);for(int i = 0; i < (int)tmp_pts.size(); i++){cv::KeyPoint key;key.pt = tmp_pts[i];keypoints.push_back(key);}}extractor(image, keypoints, brief_descriptors);	//提取描述子for (int i = 0; i < (int)keypoints.size(); i++)	//计算归一化坐标{Eigen::Vector3d tmp_p;m_camera->liftProjective(Eigen::Vector2d(keypoints[i].pt.x, keypoints[i].pt.y), tmp_p);cv::KeyPoint tmp_norm;tmp_norm.pt = cv::Point2f(tmp_p.x()/tmp_p.z(), tmp_p.y()/tmp_p.z());keypoints_norm.push_back(tmp_norm);}
}

这里就完成了关键帧的初始化。之后开始检测回环。