前言
本文主要介绍VINS的重定位模块（relocalization），主要在代码中/pose_graph节点的相关部分实现。
从论文的内容上来说，主要包括了VINS中的回环检测、特征匹配与检验、重定位等内容，即论文第七章（VII. RELOCALIZATION）。先简要介绍下论文中的内容：
A. 回环检测
1、利用DBoW2进行回环检测。
2、除了用于单目VIO的角点特征外，还添加了500个角点并使用BRIEF描述子描述。额外的角点特征用于在回环检测中实现更好的召回率。
3、DBoW2在时间和空间一致性检查后返回回环检测候选帧。
4、VINS保留所有用于特征检索的BRIEF描述子，丢弃原始图像以减少内存消耗
5、由于单目VIO可以观测到滚动和俯仰角，VINS并不需要依赖旋转不变性，如ORB SLAM中使用的ORB特性。
B. 特征恢复
1、检测到回环时，通过BRIEF描述子匹配找到对应关系，建立局部滑动窗口与回环候选帧之间的连接。
2、直接描述子匹配可能会造成大量异常值，使用两步进行几何上的异常值剔除。
1）2D-2D：RANSAC的基本矩阵检验。
2）3D-2D：RANSAC的PNP检验。
当内点超过一定阈值时，我们将该候选帧视为正确的循环检测并执行重定位。
C. 紧耦合重定位
1、重定位过程使单目VIO维持的当前滑动窗口与过去的位姿图对齐。
2、将所有回环帧的位姿作为常量，利用所有IMU测量值、局部视觉测量和从回环中提取特征对应值，共同优化滑动窗口。
流程图
代码实现
pose_graph文件夹
- keyframe.cpp/.h 构建关键帧类、描述子计算、匹配关键帧与回环帧
- pose_graph.cpp/.h 位姿图的建立与图优化、回环检测与闭环
- pose_graph_node.cpp ROS 节点函数、回调函数、主线程
输入输出
输入：
1、订阅了/vins_estimator节点发布的多个topic，包括关键帧的位姿（keyframe_pose）、重定位位姿（relo_relative_pose）、相机到IMU的外参估计（extrinsic）、VIO里程计信息PQV（odometry）、关键帧中的3D点云（keyframe_point）、IMU传播值（imu_propagate）。
2、图像，即订阅了传感器或者rosbag发布的topic：“/cam0/image_raw”。
pose_graph_node.cpp
注意此cpp在开头全局变量定义的时候，构建了一个全局的位姿图优化对象，另外介绍一下在之后回调函数和process线程中会用到的几个队列：
PoseGraph posegraph;
queue<sensor_msgs::ImageConstPtr> image_buf;//原始图像数据
queue<sensor_msgs::PointCloudConstPtr> point_buf;//世界坐标系下的地图点云
queue<nav_msgs::Odometry::ConstPtr> pose_buf;//当前帧的 pose
queue<Eigen::Vector3d> odometry_buf;
| 函数 | 功能 |
|---|---|
| void new_sequence() | 开始一个新的图像序列（地图合并功能） |
| void image_callback(const sensor_msgs::ImageConstPtr &image_msg) | 图像数据回调函数，将image_msg放入image_buf，同时根据时间戳检测是否是新的图像序列 |
| void point_callback(const sensor_msgs::PointCloudConstPtr &point_msg) | 地图点云回调函数，把point_msg放入point_buf |
| void pose_callback(const nav_msgs::Odometry::ConstPtr &pose_msg) | 图像帧位姿回调函数，把pose_msg放入pose_buf |
| void imu_forward_callback(const nav_msgs::Odometry::ConstPtr &forward_msg) | imu前向递推的回调函数，从IMU预积分的位姿得到IMU位姿和cam位姿，得到低延迟和高频率结果 |
| void relo_relative_pose_callback(const nav_msgs::Odometry::ConstPtr &pose_msg) | 重定位回调函数，将重定位帧的相对位姿放入loop_info，updateKeyFrameLoop()进行回环更新 |
| void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg) | VIO回调函数，根据pose_msg中的位姿得到IMU位姿和cam位姿 |
| void extrinsic_callback(const nav_msgs::Odometry::ConstPtr &pose_msg) | 相机IMU的外参回调函数，得到相机到IMU的外参tic和qic |
| void process() | 主线程 |
| void command() | 按键控制线程，包括s保存位姿图、n开始一个新图像序列 |
| int main(int argc, char **argv) | 节点程序入口 |
程序入口 int main(int argc, char **argv)
1、ROS初始化，设置句柄。
ros::init(argc, argv, "pose_graph");
ros::NodeHandle n("~");
posegraph.registerPub(n);
2、从launch文件读取参数和参数文件config中的参数。其中：
VISUALIZATION_SHIFT_X、VISUALIZATION_SHIFT_Y为可视化界面中图像x轴y轴的偏移量，一般设置为0；
SKIP_CNT为之后运行process()内循环的间隔；
SKIP_DIS为判断是否构建关键帧的距离标准；
visualize_camera_size为可视化界面图像的尺寸；
loop_closure=1即进行回环检测。
3、如果需要进行回环检测则读取词典和BRIEF描述子的模板文件，同时读取config中的其他参数、设置带回环的结果输出路径。
//读取字典
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描述子的模板文件
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());
4、按需求加载先前位姿图（LOAD_PREVIOUS_POSE_GRAPH=1）。
扫描二维码关注公众号，回复：
5660429 查看本文章
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;
}
5、订阅各个topic并执行各自回调函数。
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);
6、发布/pose_graph的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);
7、设置主线程和键盘控制线程。
std::thread measurement_process;
std::thread keyboard_command_process;
//pose graph主线程
measurement_process = std::thread(process);
//键盘操作的线程
keyboard_command_process = std::thread(command);
主线程 process()
如果LOOP_CLOSURE为0，即不需要进行回环检测就直接返回；如果需要则通过while (true)不断循环以下过程：（注意在使用每个队列buf的时候要加锁m_buf）。
1、得到具有相同时间戳的pose_msg、image_msg、point_msg。
m_buf.lock();
if(!image_buf.empty() && !point_buf.empty() && !pose_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");
}
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();
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();
2、构建pose_graph中用到的关键帧：这里用到的策略是先剔除最开始的SKIP_FIRST_CNT帧，然后每隔SKIP_CNT，将将距上一关键帧距离（平移向量的模）超过SKIP_DIS的图像创建为关键帧。
//剔除最开始的SKIP_FIRST_CNT帧
if (skip_first_cnt < SKIP_FIRST_CNT)
{
skip_first_cnt++;
continue;
}
//每隔SKIP_CNT帧进行一次 SKIP_CNT=0
if (skip_cnt < SKIP_CNT)
{
skip_cnt++;
continue;
}
else
skip_cnt = 0;
cv_bridge::CvImageConstPtr ptr;
cv::Mat image = ptr->image;
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)
{
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);
}
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();
3、在posegraph中添加关键帧，将flag_detect_loop=1即设置回环检测。
posegraph.addKeyFrame(keyframe, 1);
4、休眠5ms
std::chrono::milliseconds dura(5);
std::this_thread::sleep_for(dura);
可以看到，process()的最重要的内容在于如何构建keyframe对象，以及将其通过addKeyFrame添加到posegraph对象中，而这部分分别在KeyFrame和pose_graph文件中。
pose_graph.cpp/.h
该文件主要构建了位姿图类：class PoseGraph，以及其他功能性函数，比如：
YawPitchRollToRotationMatrix将欧拉角转换为旋转矩阵；
RotationMatrixTranspose对矩阵进行转置；
RotationMatrixRotatePoint将Rt矩阵相乘等。
还构造了四自由度残差的结构，这部分留到四自由度位姿图优化中再讨论。这里主要讨论PoseGraph中的函数，值得注意的是PoseGraph的构造函数中创建了一个4自由度位姿图优化线程。
t_optimization = std::thread(&PoseGraph::optimize4DoF, this);
| 函数 | 功能 |
|---|---|
| void PoseGraph::registerPub(ros::NodeHandle &n) | 发布轨迹path的topic |
| void PoseGraph::loadVocabulary(std::string voc_path) | 加载Brief字典 |
| void PoseGraph::addKeyFrame(KeyFrame* cur_kf, bool flag_detect_loop) | 添加关键帧，完成了回环检测与闭环的过程 |
| void PoseGraph::loadKeyFrame(KeyFrame* cur_kf, bool flag_detect_loop) | 载入关键帧 |
| KeyFrame* PoseGraph::getKeyFrame(int index) | 返回索引为index的关键帧 |
| int PoseGraph::detectLoop(KeyFrame* keyframe, int frame_index) | 回环检测 |
| void PoseGraph::addKeyFrameIntoVoc(KeyFrame* keyframe) | 将当前帧的描述子存入字典数据库 |
| void PoseGraph::optimize4DoF() | 四自由度位姿图优化函数 |
| void PoseGraph::updatePath() | 更新轨迹并发布 |
| void PoseGraph::savePoseGraph() | 保存位姿图到file_path |
| void PoseGraph::loadPoseGraph() | 从file_path读取位姿图 |
| void PoseGraph::publish() | 用于发布topic：pub_pg_path、pub_path、pub_base_path |
| void PoseGraph::updateKeyFrameLoop(int index, Eigen::Matrix<double, 8, 1 > &_loop_info) | 更新关键帧的回环信息 |
void PoseGraph::addKeyFrame(KeyFrame* cur_kf, bool flag_detect_loop)
1、如果sequence_cnt != cur_kf->sequence，则新建一个新的图像序列
2、获取当前帧的位姿vio_P_cur、vio_R_cur并更新
cur_kf->getVioPose(vio_P_cur, vio_R_cur);
vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;
vio_R_cur = w_r_vio * vio_R_cur;
cur_kf->updateVioPose(vio_P_cur, vio_R_cur);
cur_kf->index = global_index;
global_index++;
3、进行回环检测，返回回环候选帧的索引
loop_index = detectLoop(cur_kf, cur_kf->index);
4、如果存在回环候选帧，即loop_index != -1：
1）将当前帧与回环帧进行描述子匹配，如果成功则确定存在回环
//获取回环候选帧
KeyFrame* old_kf = getKeyFrame(loop_index);
//当前帧与回环候选帧进行描述子匹配
if (cur_kf->findConnection(old_kf))
{
//earliest_loop_index为最早的回环候选帧
if (earliest_loop_index > loop_index || earliest_loop_index == -1)
earliest_loop_index = loop_index;
2）计算当前帧与回环帧的相对位姿，纠正当前帧位姿w_P_cur、w_R_cur
Vector3d w_P_old, w_P_cur, vio_P_cur;
Matrix3d w_R_old, w_R_cur, vio_R_cur;
old_kf->getVioPose(w_P_old, w_R_old);
cur_kf->getVioPose(vio_P_cur, vio_R_cur);
//获取当前帧与回环帧的相对位姿relative_q、relative_t
Vector3d relative_t;
Quaterniond relative_q;
relative_t = cur_kf->getLoopRelativeT();
relative_q = (cur_kf->getLoopRelativeQ()).toRotationMatrix();
//重新计算当前帧位姿w_P_cur、w_R_cur
w_P_cur = w_R_old * relative_t + w_P_old;
w_R_cur = w_R_old * relative_q;
//回环得到的位姿和VIO位姿之间的偏移量shift_r、shift_t
double shift_yaw;
Matrix3d shift_r;
Vector3d shift_t;
shift_yaw = Utility::R2ypr(w_R_cur).x() - Utility::R2ypr(vio_R_cur).x();
shift_r = Utility::ypr2R(Vector3d(shift_yaw, 0, 0));
shift_t = w_P_cur - w_R_cur * vio_R_cur.transpose() * vio_P_cur;
3）如果存在多个图像序列，则将所有图像序列都合并到世界坐标系下
if (old_kf->sequence != cur_kf->sequence && sequence_loop[cur_kf->sequence] == 0)
{
w_r_vio = shift_r;
w_t_vio = shift_t;
vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;
vio_R_cur = w_r_vio * vio_R_cur;
cur_kf->updateVioPose(vio_P_cur, vio_R_cur);
list<KeyFrame*>::iterator it = keyframelist.begin();
for (; it != keyframelist.end(); it++)
{
if((*it)->sequence == cur_kf->sequence)
{
Vector3d vio_P_cur;
Matrix3d vio_R_cur;
(*it)->getVioPose(vio_P_cur, vio_R_cur);
vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;
vio_R_cur = w_r_vio * vio_R_cur;
(*it)->updateVioPose(vio_P_cur, vio_R_cur);
}
}
sequence_loop[cur_kf->sequence] = 1;
}
4）将当前帧放入优化队列中
m_optimize_buf.lock();
optimize_buf.push(cur_kf->index);
m_optimize_buf.unlock();
5、获取VIO当前帧的位姿P、R，根据偏移量计算得到实际位姿。并进行位姿更新
//获取VIO当前帧的位姿P、R，根据偏移量得到实际位姿
cur_kf->getVioPose(P, R);
P = r_drift * P + t_drift;
R = r_drift * R;
//更新当前帧的位姿P、R
cur_kf->updatePose(P, R);
6、发布path[sequence_cnt]
Quaterniond Q{R};
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header.stamp = ros::Time(cur_kf->time_stamp);
pose_stamped.header.frame_id = "world";
pose_stamped.pose.position.x = P.x() + VISUALIZATION_SHIFT_X;
pose_stamped.pose.position.y = P.y() + VISUALIZATION_SHIFT_Y;
pose_stamped.pose.position.z = P.z();
pose_stamped.pose.orientation.x = Q.x();
pose_stamped.pose.orientation.y = Q.y();
pose_stamped.pose.orientation.z = Q.z();
pose_stamped.pose.orientation.w = Q.w();
path[sequence_cnt].poses.push_back(pose_stamped);
path[sequence_cnt].header = pose_stamped.header;
7、保存闭环轨迹到VINS_RESULT_PATH
if (SAVE_LOOP_PATH)
{
ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
loop_path_file.setf(ios::fixed, ios::floatfield);
loop_path_file.precision(0);
loop_path_file << cur_kf->time_stamp * 1e9 << ",";
loop_path_file.precision(5);
loop_path_file << P.x() << ","<< P.y() << ","<< P.z() << ","
<< Q.w() << ","<< Q.x() << ","<< Q.y() << ","<< Q.z() << ","<< endl;
loop_path_file.close();
}
8、绘制可视化轨迹中帧间的连线，发布topic：pub_pg_path、pub_path、pub_base_path
keyframelist.push_back(cur_kf);
publish();
int PoseGraph::detectLoop(KeyFrame* keyframe, int frame_index)
该函数用于检测当前帧与先前帧是否可能存在回环，若存在则返回回环候选帧的索引。在函数中使用大量DEBUG条件语句，用于在调试时对当前状态进行可视化输出，这里就不介绍了。
1、查询字典数据库，得到与每一帧的相似度评分ret
QueryResults ret;
db.query(keyframe->brief_descriptors, ret, 4, frame_index - 50);
2、添加当前关键帧到字典数据库中
db.add(keyframe->brief_descriptors);
3、通过相似度评分判断是否存在回环候选帧
bool find_loop = false;
//确保与相邻帧具有好的相似度评分
if (ret.size() >= 1 &&ret[0].Score > 0.05)
for (unsigned int i = 1; i < ret.size(); i++)
{
//评分大于0.015则认为是回环候选帧
if (ret[i].Score > 0.015)
{
find_loop = true;
int tmp_index = ret[i].Id;
}
}
4、如果在先前检测到回环候选帧再判断：当前帧的索引值是否大于50，即系统开始的前50帧不进行回环；
返回评分大于0.015的最早的关键帧索引min_index，如果不存在回环或判断失败则返回-1
if (find_loop && frame_index > 50)
{
int min_index = -1;
for (unsigned int i = 0; i < ret.size(); i++)
{
if (min_index == -1 || (ret[i].Id < min_index && ret[i].Score > 0.015))
min_index = ret[i].Id;
}
return min_index;
}
else
return -1;
keyframe.cpp/.h
该文件主要构建了两个类：
1、class BriefExtractor，构建Brief产生器，用于通过Brief模板文件对图像特征点计算Brief描述子，
class BriefExtractor
{
public:
//读取 构建字典时使用的相同的Brief模板文件，构造BriefExtractor
virtual void operator()(const cv::Mat &im, vector<cv::KeyPoint> &keys, vector<BRIEF::bitset> &descriptors) const;
//运算符重载了“（）”来计算描述子。
BriefExtractor(const std::string &pattern_file);
DVision::BRIEF m_brief;
};
2、class KeyFrame，构建关键帧，通过BRIEF描述子匹配关键帧和回环候选帧。其成员函数包括：（省去了部分get和set函数）
| 函数 | 功能 |
|---|---|
| KeyFrame::KeyFrame() | 两种构造函数，分别为创建新关键帧和加载先前帧 |
| void KeyFrame::computeWindowBRIEFPoint() | 计算窗口中所有特征点的描述子 |
| void KeyFrame::computeBRIEFPoint() | 额外检测500个新的特征点并计算所有特征点的描述子，为了回环检测 |
| bool KeyFrame::searchInAera() | 关键帧中某个特征点的描述子与回环帧的所有描述子匹配 |
| void KeyFrame::searchByBRIEFDes() | 将关键帧与回环帧进行BRIEF描述子匹配 |
| void KeyFrame::FundmantalMatrixRANSAC() | 通过RANSAC的基本矩阵检验去除匹配异常的点 |
| void KeyFrame::PnPRANSAC() | 通过RANSAC的PNP检验去除匹配异常的点 |
| bool KeyFrame::findConnection(KeyFrame* old_kf) | 寻找并建立关键帧与回环帧之间的匹配关系 |
| int KeyFrame::HammingDis(const BRIEF::bitset &a, const BRIEF::bitset &b) | 计算两个描述子之间的汉明距离 |
此外还有函数reduceVector()用于剔除status为0的点
static void reduceVector(vector<Derived> &v, vector<uchar> status)
{
int j = 0;
for (int i = 0; i < int(v.size()); i++)
if (status[i])
v[j++] = v[i];
v.resize(j);
}
void KeyFrame::searchByBRIEFDes
该函数的作用是将此关键帧对象与某个回环帧进行BRIEF描述子匹配，其参数包括：
void KeyFrame::searchByBRIEFDes(std::vector<cv::Point2f> &matched_2d_old,//param[out]回环帧匹配后的二维坐标
std::vector<cv::Point2f> &matched_2d_old_norm,//param[out]回环帧匹配后的二维归一化坐标
std::vector<uchar> &status,//param[out]匹配状态，成功为1
const std::vector<BRIEF::bitset> &descriptors_old,//param[in]回环帧的描述子
const std::vector<cv::KeyPoint> &keypoints_old,//param[in]回环帧的二维坐标
const std::vector<cv::KeyPoint> &keypoints_old_norm)//param[in]回环帧的二维归一化坐标
{
//vector<BRIEF::bitset> window_brief_descriptors为这个关键帧所有特征点对应的brief描述子
for(int i = 0; i < (int)window_brief_descriptors.size(); i++)
{
cv::Point2f pt(0.f, 0.f);
cv::Point2f pt_norm(0.f, 0.f);
//对关键帧中每个特征点的描述子与回环帧的所有描述子匹配，如果能找到汉明距离小于80的最小值和索引即为该特征点的最佳匹配，相应的status置为1
if (searchInAera(window_brief_descriptors[i], descriptors_old, keypoints_old, keypoints_old_norm, pt, pt_norm))
status.push_back(1);
else
status.push_back(0);
matched_2d_old.push_back(pt);
matched_2d_old_norm.push_back(pt_norm);
}
}
bool KeyFrame::findConnection(KeyFrame* old_kf)
该函数的主要目的是寻找并建立关键帧与回环帧之间的匹配关系，返回True即为确定构成回环。
1、将关键帧与回环帧进行BRIEF描述子匹配，并剔除匹配失败的点
searchByBRIEFDes(matched_2d_old, matched_2d_old_norm, status, old_kf->brief_descriptors, old_kf->keypoints, old_kf->keypoints_norm);
reduceVector(matched_2d_cur, status);
reduceVector(matched_2d_old, status);
reduceVector(matched_2d_cur_norm, status);
reduceVector(matched_2d_old_norm, status);
reduceVector(matched_3d, status);
reduceVector(matched_id, status);
//printf("search by des finish\n");
2、如果能匹配的特征点能达到最小回环匹配个数，则用RANSAC PnP检测再去除误匹配的点，
if ((int)matched_2d_cur.size() > MIN_LOOP_NUM)
{
//RANSAC PnP检测去除误匹配的点
status.clear();
PnPRANSAC(matched_2d_old_norm, matched_3d, status, PnP_T_old, PnP_R_old);
reduceVector(matched_2d_cur, status);
reduceVector(matched_2d_old, status);
reduceVector(matched_2d_cur_norm, status);
reduceVector(matched_2d_old_norm, status);
reduceVector(matched_3d, status);
reduceVector(matched_id, status);
3、将此关键帧和回环帧拼接起来，将对应的匹配点相连以绘制回环匹配图，并发布为pub_match_img。
int gap = 10;
cv::Mat gap_image(ROW, gap, CV_8UC1, cv::Scalar(255, 255, 255));
cv::Mat gray_img, loop_match_img;
cv::Mat old_img = old_kf->image;
//这里将image、gap_image、old_img水平拼接起来成为gray_img
cv::hconcat(image, gap_image, gap_image);
cv::hconcat(gap_image, old_img, gray_img);
//灰度图gray_img转换成RGB图loop_match_img
cvtColor(gray_img, loop_match_img, CV_GRAY2RGB);
//在图片loop_match_img上标注出匹配点和之间的连线
for(int i = 0; i< (int)matched_2d_cur.size(); i++)
{
cv::Point2f cur_pt = matched_2d_cur[i];
cv::circle(loop_match_img, cur_pt, 5, cv::Scalar(0, 255, 0));
}
for(int i = 0; i< (int)matched_2d_old.size(); i++)
{
cv::Point2f old_pt = matched_2d_old[i];
old_pt.x += (COL + gap);
cv::circle(loop_match_img, old_pt, 5, cv::Scalar(0, 255, 0));
}
for (int i = 0; i< (int)matched_2d_cur.size(); i++)
{
cv::Point2f old_pt = matched_2d_old[i];
old_pt.x += (COL + gap) ;
cv::line(loop_match_img, matched_2d_cur[i], old_pt, cv::Scalar(0, 255, 0), 2, 8, 0);
}
//在loop_match_img下面垂直拼接一个notation，写上当前帧和先前帧的索引值和序列号
cv::Mat notation(50, COL + gap + COL, CV_8UC3, cv::Scalar(255, 255, 255));
putText(notation, "current frame: " + to_string(index) + " sequence: " + to_string(sequence), cv::Point2f(20, 30), CV_FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255), 3);
putText(notation, "previous frame: " + to_string(old_kf->index) + " sequence: " + to_string(old_kf->sequence), cv::Point2f(20 + COL + gap, 30), CV_FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255), 3);
cv::vconcat(notation, loop_match_img, loop_match_img);
//若达到最小回环匹配点数，将loop_match_img的宽和高缩小一半并发布为pub_match_img
if ((int)matched_2d_cur.size() > MIN_LOOP_NUM)
{
cv::Mat thumbimage;
cv::resize(loop_match_img, thumbimage, cv::Size(loop_match_img.cols / 2, loop_match_img.rows / 2));
sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", thumbimage).toImageMsg();
msg->header.stamp = ros::Time(time_stamp);
pub_match_img.publish(msg);
}
}
4、如果在PNP检验后仍能达到最小回环匹配点数则进行先对位姿检验，通过则确定构成回环，将回环帧索引和相对位姿存入loop_index、loop_info，并返回True。
if ((int)matched_2d_cur.size() > MIN_LOOP_NUM)
{
relative_t = PnP_R_old.transpose() * (origin_vio_T - PnP_T_old);
relative_q = PnP_R_old.transpose() * origin_vio_R;
relative_yaw = Utility::normalizeAngle(Utility::R2ypr(origin_vio_R).x() - Utility::R2ypr(PnP_R_old).x());
//相对位姿检验
if (abs(relative_yaw) < 30.0 && relative_t.norm() < 20.0)
{
has_loop = true;
loop_index = old_kf->index;
loop_info << relative_t.x(), relative_t.y(), relative_t.z(),
relative_q.w(), relative_q.x(), relative_q.y(), relative_q.z(),
relative_yaw;
if(FAST_RELOCALIZATION)
{
////快速重定位功能，略
}
return true;
}
}
return false;
}