回环检测的任务主要是为了检测机器人是否到达之前相同的位置，并且消累计的误差。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_buf
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);
//地图的点云，存放到point_buf
ros::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(); // 取出来pose
pose_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();
}最近在重新看了这部分，对于时间戳我们可以理解：其中
front back
图像queue：。。。。。
位姿queue: 。 。。。。
地图点： 。。。。。。
基本就是保持以上的时间顺序来对齐时间戳，对于图像的位姿会判断时间戳是相等还是是第一个大于pose的时间，之后就会将当前帧n-1,n-2...pop掉。地图点则是一直满足要大于位姿点。这样就一直保持各个buf里不会一直堆积数据。
看到github上有人说内存泄露问题在这，就是简单的限制img_buf的大小。这里buf都有严格的时间戳来管理。不能通过大小来随意pop，所以如果你想减小loop的内存我建议不要在这里动手。
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<cv::Point3f> &_point_3d, vector<cv::Point2f> &_point_2d_uv, vector<cv::Point2f> &_point_2d_norm,
vector<double> &_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->i
T_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; //地图点的idx
has_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之后的keypoint
for(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<cv::KeyPoint> &keys, vector<BRIEF::bitset> &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<cv::Point2f> 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);
}
}这里就完成了关键帧的初始化。之后开始检测回环。
四、添加关键帧
4.1 addKeyFrame
我们构建出了关键帧之后，就需要添加。这个函数输入为关键帧的指针，和是否检测回环默认flag_detect_loop=true。
void PoseGraph::addKeyFrame(KeyFrame* cur_kf, bool flag_detect_loop)
首先要获取当前帧的平移和旋转，但是以防传递来的不是当前的时序。这里因为loop是独立的什么时候启动停止都可以。如果不是一个时序的我们就复位一些变量。
Vector3d vio_P_cur;
Matrix3d vio_R_cur;
if (sequence_cnt != cur_kf->sequence)//发生了sequence的跳变
{
sequence_cnt++;
sequence_loop.push_back(0);
// 如果发生跳变复位一些变量
w_t_vio = Eigen::Vector3d(0, 0, 0);
w_r_vio = Eigen::Matrix3d::Identity();
m_drift.lock();
t_drift = Eigen::Vector3d(0, 0, 0);
r_drift = Eigen::Matrix3d::Identity();
m_drift.unlock();
}通过Keyfram中的函数获取当前帧在时间坐标系下的旋转和平移。这里vio_P_cur和vio_R_cur我们要搞清楚是word->i帧下的坐标，更新到i+1下的旋转和平移。还要更新好当前关键帧中的位姿。为了记录关键帧序号和个数global_index会随着添加的关键帧一直叠加。
// 更新一下VIO位姿
cur_kf->getVioPose(vio_P_cur, vio_R_cur);//从word->到vio坐标
//update 回环修正后的消除累计误差的位姿
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); // 更新VIO位姿
cur_kf->index = global_index; // 赋值索引
global_index++;
int loop_index = -1; //重置回环索引号4.2 detectLoop
这个函数输入关键帧，和关键帧的当前序号，返回int类型，如果检测到回环则返回当前帧的序号。
loop_index = detectLoop(cur_kf, cur_kf->index);
其实这里的步骤很简单，我简化如下：
//step1 调用词袋查询接口，查询结果是ret，最多返回4个备选KF，查找距离当前至少50帧的KF
db.query(keyframe->brief_descriptors, ret, 4, frame_index - 50);
//step2 在这里不断加入描述子
db.add(keyframe->brief_descriptors);
通过上面几步我们就可以得到候选的4个关键帧，这里选取的是当前序号的后50帧。所以这里其实可以释放回环的内存！！但是我尝试还是存在问题，关键帧因为需要优化所以不能随意释放！
//step3 ret按照得分大小降序排列的，这里确保返回的候选KF数目至少一个且得分满足要求
//注意 保证其中的一个关键帧的得分要大于0.05 最少备选项大于1
if (ret.size() >= 1 &&ret[0].Score > 0.05)
// 开始遍历其他候选帧
for (unsigned int i = 1; i < ret.size(); i++)
{
//if (ret[i].Score > ret[0].Score * 0.3)
// 如果有大于一个候选帧且得分满足要求
if (ret[i].Score > 0.015) //注意 希望别的关键帧也大于一定的阈值
{
find_loop = true; //就认为找到回环了
int tmp_index = ret[i].Id;
if (DEBUG_IMAGE && 0) //和上面操作重复了不执行
{
auto it = image_pool.find(tmp_index);
cv::Mat tmp_image = (it->second).clone();
putText(tmp_image, "loop score:" + to_string(ret[i].Score), cv::Point2f(10, 50), CV_FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255));
cv::hconcat(loop_result, tmp_image, loop_result);
}
}
}这里希望，有一个关键帧的得分要大于0.05，当然作者希望其他候选关键帧也能大于0.015。因为如果当前为回环，附近的图像也应该有和当前帧有一定的相似程度。
//step4 认为找到了回环并且当前帧是第50帧以后
if (find_loop && frame_index > 50)
{
int min_index = -1;
// 寻找得分大于0.015的idx最小的那个帧，这样可以尽可能调整更多帧的位姿
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; //当前帧和min_index形成了回环
}这里希望关键帧暂满50帧才检测回环，作者对前50帧一般都在初始化所以不需要回环功能。这里希望找到得分大于0.015的最小帧，这样可以优化到最远的回环帧。如果满足则返回序号，否则返回-1。
4.3 回环校验
等待更新...