本文主要介绍VINS的闭环检测重定位与位姿图优化部分，作为系列文章的最后一节。

回环检测的关键就是如何有效检测出相机曾经经过同一个地方，这样可以避免较大的累积误差，使得当前帧和之前的某一帧迅速建立约束，形成新的较小的累积误差。由于回环检测提供了当前数据与所有历史数据的关联，在跟踪算法丢失后，还可以利用重定位。

论文中主要分为两部分：回环检测与重定位、4-DOF的位姿图优化。

第一部分主要是为了通过回环检测找到当前帧和候选帧的联系，并通过简单的紧耦合重定位将局部滑动窗口移动与过去的位姿对齐。

第二部分是为了保证基于重定位结果对过去的所有位姿进行全局优化。

论文中内容为

0.4 重定位

vins的重定位模块主要包含回环检测,回环候选帧之间的特征匹配,紧耦合重定位三个部分

A、回环检测（只对关键帧）

1、采用DBoW2词袋位置识别方法进行回环检测。经过时间空间一致性检验后，DBoW2返回回环检测候选帧。

2、除了用于单目VIO的角点特征外，还添加了500个角点并使用BRIEF描述子，描述子用作视觉词袋在数据库里进行搜索。这些额外的角点能用来实现更好的回环检测。

3、VINS只保留所有用于特征检索的BRIEF描述子，丢弃原始图像以减小内存。

4、单目VIO可以观测到滚动和俯仰角，VINS并不需要依赖旋转不变性。

B、回环候选帧之间的特征匹配

1、检测到回环时，通过BRIEF描述子匹配找到对应关系。但是直接的描述子匹配会导致很多外点。

2、本文提出两步几何剔除法：

1）2D-2D：使用RANSAC进行F矩阵测试，

2）3D-2D：使用RANSAC进行PnP，基于已知的滑动窗特征点的3D位置，和回路闭合候选处图像的2D观测（像素坐标）。

当内点超过一定阈值时，我们将该候选帧视为正确的循环检测并执行重定位。

C、紧耦合重定位

1、重定位过程使单目VIO维持的当前滑动窗口与过去的位姿图对齐。

2、将所有回环帧的位姿作为常量，利用所有IMU测量值、局部视觉测量和从回环中提取特征对应值，共同优化滑动窗口。

0.5 全局位姿图优化

A、位姿图中添加关键帧

B、4自由度位姿图优化

C、位姿图管理

相关代码都在pose_graph文件中，主要分为三个程序，分别为：

keyframe.cpp/.h 构建关键帧类、描述子计算、匹配关键帧与回环帧
pose_graph.cpp/.h 位姿图的建立与图优化、回环检测与闭环
pose_graph_node.cpp ROS 节点函数、回调函数、主线程

1、pose_graph_node.cpp

主要分为7个回调函数以及主线程process（）函数。

7个回调函数：包括关键帧的位姿（keyframe_pose）、重定位位姿（relo_relative_pose）、相机到IMU的外参估计（extrinsic）、VIO里程计信息PQV（odometry）、关键帧中的3D点云（keyframe_point）、IMU传播值（imu_propagate）。

ROS初始化，设置句柄

从launch文件读取参数和参数文件config中的参数

如果需要进行回环检测则读取词典和BRIEF描述子的模板文件，同时读取config中的其他参数、设置带回环的结果输出路径。

加载先前位姿图 loadPoseGraph()

订阅各个topic并执行各自回调函数

发布/pose_graph的topic

设置主线程 process() 和键盘控制线程 command()

int main(int argc, char **argv)
{// 1.ROS初始化，设置句柄ros::init(argc, argv, "pose_graph");ros::NodeHandle n("~");posegraph.registerPub(n);// 2.读取参数n.getParam("visualization_shift_x", VISUALIZATION_SHIFT_X);n.getParam("visualization_shift_y", VISUALIZATION_SHIFT_Y);n.getParam("skip_cnt", SKIP_CNT);n.getParam("skip_dis", SKIP_DIS);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);LOOP_CLOSURE = fsSettings["loop_closure"];std::string IMAGE_TOPIC;int LOAD_PREVIOUS_POSE_GRAPH;//3.如果需要进行回环检测则读取词典和BRIEF描述子的模板文件，同时读取config中的其他参数、设置带回环的结果输出路径。if (LOOP_CLOSURE){ROW = fsSettings["image_height"];COL = fsSettings["image_width"];// 3.1读取字典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);// 3.2读取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());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;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();// 3.3加载先前的位姿图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();// 4.订阅话题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);// 5.发布的话题topicpub_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);// 6. 创建两个线程，process 和 commandstd::thread measurement_process;std::thread keyboard_command_process;//pose graph主线程measurement_process = std::thread(process);//键盘操作的线程keyboard_command_process = std::thread(command);ros::spin();return 0;
}

2、process（）函数

先判断是否需要回环检测

得到具有相同时间戳的pose_msg、image_msg、point_msg

构建pose_graph中用到的关键帧，然后每隔SKIP_CNT，将将距上一关键帧距离（平移向量的模）超过SKIP_DIS的图像创建为关键帧。其中，最核心的函数是KeyFrame类以及addKeyFrame（）函数。

//主线程
void process()
{// 1.先判断是否需要回环检测if (!LOOP_CLOSURE)return;while (true)// 不断循环{// 三个参数图像、点云、VIO位姿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// 2.得到具有相同时间戳的pose_msg、image_msg、point_msgm_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();// 3.构建pose_graph中用到的关键帧，然后每隔SKIP_CNT，将将距上一关键帧距离（平移向量的模）超过SKIP_DIS的图像创建为关键帧。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 first few// 3.1 剔除最开始的SKIP_FIRST_CNT帧，if (skip_first_cnt < SKIP_FIRST_CNT){skip_first_cnt++;continue;}// 3.2每隔SKIP_CNT帧进行一次  SKIP_CNT=0if (skip_cnt < SKIP_CNT){skip_cnt++;continue;}else{skip_cnt = 0;}// 3.3ROS图像转为Opencv类型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);}elseptr = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::MONO8);cv::Mat image = ptr->image;// build keyframeVector3d 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();// 3.4 将距上一关键帧距离（平移向量的模）超过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);//printf("u %f, v %f \n", p_2d_uv.x, p_2d_uv.y);}// 3.5 创建为关键帧类型并加入到posegraph中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;//在posegraph中添加关键帧，flag_detect_loop=1回环检测posegraph.addKeyFrame(keyframe, 1);m_process.unlock();frame_index++;last_t = T;}}// 4.休眠5msstd::chrono::milliseconds dura(5);std::this_thread::sleep_for(dura);}
}

3、pose_graph.h

接下来将主要讲解两个函数：addKeyFrame（）以及detectLoop（）

4、addKeyFrame（）添加关键帧，进行回环检测与闭环

建一个新的图像序列

getVioPose（）获取当前帧的位姿vio_P_cur、vio_R_cur并更新updateVioPose

detectLoop（）回环检测，返回回环候选帧的索引loop_index

计算当前帧与回环帧的相对位姿，纠正当前帧位姿w_P_cur、w_R_cur；回环得到的位姿和VIO位姿之间的偏移量shift_r、shift_t；如果存在多个图像序列，则将所有图像序列都合并到世界坐标系下

获取VIO当前帧的位姿P、R，根据偏移量得到实际位姿并更新updatePose当前帧的位姿P、R

发布path[sequence_cnt]

保存闭环轨迹到VINS_RESULT_PATH

void PoseGraph::addKeyFrame(KeyFrame* cur_kf, bool flag_detect_loop)
{//shift to base frameVector3d vio_P_cur;Matrix3d vio_R_cur;// 1.建一个新的图像序列if (sequence_cnt != cur_kf->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();}// 2.getVioPose（）获取当前帧的位姿vio_P_cur、vio_R_cur并更新updateVioPosecur_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++;int loop_index = -1;if (flag_detect_loop){TicToc tmp_t;// 3.detectLoop（）回环检测，返回回环候选帧的索引loop_indexloop_index = detectLoop(cur_kf, cur_kf->index);}else{addKeyFrameIntoVoc(cur_kf);}// 4.存在回环候选帧if (loop_index != -1){//printf(" %d detect loop with %d \n", cur_kf->index, loop_index);// 4.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;// 4.2 计算当前帧与回环帧的相对位姿，纠正当前帧位姿w_P_cur、w_R_curVector3d 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_tVector3d relative_t;Quaterniond relative_q;relative_t = cur_kf->getLoopRelativeT();relative_q = (cur_kf->getLoopRelativeQ()).toRotationMatrix();//重新计算当前帧位姿w_P_cur、w_R_curw_P_cur = w_R_old * relative_t + w_P_old;w_R_cur = w_R_old * relative_q;//回环得到的位姿和VIO位姿之间的偏移量shift_r、shift_tdouble 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; // shift vio pose of whole sequence to the world frame// 4.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.4 将当前帧放入优化队列中m_optimize_buf.lock();optimize_buf.push(cur_kf->index);m_optimize_buf.unlock();}}m_keyframelist.lock();Vector3d P;Matrix3d R;// 5.获取VIO当前帧的位姿P、R，根据偏移量得到实际位姿cur_kf->getVioPose(P, R);P = r_drift * P + t_drift;R = r_drift * R;// 更新当前帧的位姿P、Rcur_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_PATHif (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.draw local connectionif (SHOW_S_EDGE){list<KeyFrame*>::reverse_iterator rit = keyframelist.rbegin();for (int i = 0; i < 4; i++){if (rit == keyframelist.rend())break;Vector3d conncected_P;Matrix3d connected_R;if((*rit)->sequence == cur_kf->sequence){(*rit)->getPose(conncected_P, connected_R);posegraph_visualization->add_edge(P, conncected_P);}rit++;}}//当前帧与其回环帧连线if (SHOW_L_EDGE){if (cur_kf->has_loop){//printf("has loop \n");KeyFrame* connected_KF = getKeyFrame(cur_kf->loop_index);Vector3d connected_P,P0;Matrix3d connected_R,R0;connected_KF->getPose(connected_P, connected_R);//cur_kf->getVioPose(P0, R0);cur_kf->getPose(P0, R0);if(cur_kf->sequence > 0){//printf("add loop into visual \n");posegraph_visualization->add_loopedge(P0, connected_P + Vector3d(VISUALIZATION_SHIFT_X, VISUALIZATION_SHIFT_Y, 0));}}}//posegraph_visualization->add_pose(P + Vector3d(VISUALIZATION_SHIFT_X, VISUALIZATION_SHIFT_Y, 0), Q);keyframelist.push_back(cur_kf);publish();m_keyframelist.unlock();
}

5、detectLoop（）回环检测返回候选帧索引

query()查询字典数据库，得到与每一帧的相似度评分ret

add()添加当前关键帧到字典数据库中

hconcat()通过相似度评分判断是否存在回环候选帧

对于索引值大于50的关键帧才考虑进行回环,即前50帧不回环,返回评分大于0.015的最早的关键帧索引min_index

//回环检测返回候选帧索引
int PoseGraph::detectLoop(KeyFrame* keyframe, int frame_index)
{// put image into image_pool; for visualizationcv::Mat compressed_image;if (DEBUG_IMAGE){int feature_num = keyframe->keypoints.size();cv::resize(keyframe->image, compressed_image, cv::Size(376, 240));putText(compressed_image, "feature_num:" + to_string(feature_num), cv::Point2f(10, 10), CV_FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255));image_pool[frame_index] = compressed_image;}TicToc tmp_t;QueryResults ret;TicToc t_query;// 1.查询字典数据库，得到与每一帧的相似度评分retdb.query(keyframe->brief_descriptors, ret, 4, frame_index - 50);//printf("query time: %f", t_query.toc());//cout << "Searching for Image " << frame_index << ". " << ret << endl;// 2.添加当前关键帧到字典数据库中TicToc t_add;db.add(keyframe->brief_descriptors);//printf("add feature time: %f", t_add.toc());// ret[0] is the nearest neighbour's score. threshold change with neighour score// 3.通过相似度评分判断是否存在回环候选帧bool find_loop = false;cv::Mat loop_result;if (DEBUG_IMAGE){loop_result = compressed_image.clone();if (ret.size() > 0)putText(loop_result, "neighbour score:" + to_string(ret[0].Score), cv::Point2f(10, 50), CV_FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255));}// visual loop result if (DEBUG_IMAGE){for (unsigned int i = 0; i < ret.size(); i++){int tmp_index = ret[i].Id;auto it = image_pool.find(tmp_index);cv::Mat tmp_image = (it->second).clone();putText(tmp_image, "index:  " + to_string(tmp_index) + "loop score:" + to_string(ret[i].Score), cv::Point2f(10, 50), CV_FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255));cv::hconcat(loop_result, tmp_image, loop_result);}}//确保与相邻帧具有好的相似度评分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)//评分大于0.015则认为是回环候选帧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);}}}
/*if (DEBUG_IMAGE){cv::imshow("loop_result", loop_result);cv::waitKey(20);}
*/// 4.对于索引值大于50的关键帧才考虑进行回环,即前50帧不回环//返回评分大于0.015的最早的关键帧索引min_indexif (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;}elsereturn -1;}

6、optimize4DoF（）位姿图优化

//四自由度位姿图优化
void PoseGraph::optimize4DoF()
{while(true){int cur_index = -1;int first_looped_index = -1;//取出最新一个待优化帧作为当前帧m_optimize_buf.lock();while(!optimize_buf.empty()){cur_index = optimize_buf.front();first_looped_index = earliest_loop_index;optimize_buf.pop();}m_optimize_buf.unlock();if (cur_index != -1){printf("optimize pose graph \n");TicToc tmp_t;m_keyframelist.lock();KeyFrame* cur_kf = getKeyFrame(cur_index);int max_length = cur_index + 1;// w^t_i   w^q_idouble t_array[max_length][3];Quaterniond q_array[max_length];double euler_array[max_length][3];double sequence_array[max_length];ceres::Problem problem;ceres::Solver::Options options;options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;//options.minimizer_progress_to_stdout = true;//options.max_solver_time_in_seconds = SOLVER_TIME * 3;options.max_num_iterations = 5;ceres::Solver::Summary summary;ceres::LossFunction *loss_function;loss_function = new ceres::HuberLoss(0.1);//loss_function = new ceres::CauchyLoss(1.0);ceres::LocalParameterization* angle_local_parameterization =AngleLocalParameterization::Create();list<KeyFrame*>::iterator it;int i = 0;for (it = keyframelist.begin(); it != keyframelist.end(); it++){//找到第一个回环候选帧，//回环检测到帧以前的都略过if ((*it)->index < first_looped_index)continue;(*it)->local_index = i;Quaterniond tmp_q;Matrix3d tmp_r;Vector3d tmp_t;(*it)->getVioPose(tmp_t, tmp_r);tmp_q = tmp_r;t_array[i][0] = tmp_t(0);t_array[i][1] = tmp_t(1);t_array[i][2] = tmp_t(2);q_array[i] = tmp_q;Vector3d euler_angle = Utility::R2ypr(tmp_q.toRotationMatrix());euler_array[i][0] = euler_angle.x();euler_array[i][1] = euler_angle.y();euler_array[i][2] = euler_angle.z();sequence_array[i] = (*it)->sequence;problem.AddParameterBlock(euler_array[i], 1, angle_local_parameterization);problem.AddParameterBlock(t_array[i], 3);//回环检测到的帧参数设为固定if ((*it)->index == first_looped_index || (*it)->sequence == 0){   problem.SetParameterBlockConstant(euler_array[i]);problem.SetParameterBlockConstant(t_array[i]);}//add edge//对于每个i, 只计算它之前五个的位置和yaw残差for (int j = 1; j < 5; j++){if (i - j >= 0 && sequence_array[i] == sequence_array[i-j]){Vector3d euler_conncected = Utility::R2ypr(q_array[i-j].toRotationMatrix());Vector3d relative_t(t_array[i][0] - t_array[i-j][0], t_array[i][1] - t_array[i-j][1], t_array[i][2] - t_array[i-j][2]);relative_t = q_array[i-j].inverse() * relative_t;double relative_yaw = euler_array[i][0] - euler_array[i-j][0];ceres::CostFunction* cost_function = FourDOFError::Create( relative_t.x(), relative_t.y(), relative_t.z(),relative_yaw, euler_conncected.y(), euler_conncected.z());problem.AddResidualBlock(cost_function, NULL, euler_array[i-j], t_array[i-j], euler_array[i], t_array[i]);}}//add loop edgeif((*it)->has_loop)// 如果检测到回环{//必须回环检测的帧号大于或者等于当前帧的回环检测匹配帧号assert((*it)->loop_index >= first_looped_index);int connected_index = getKeyFrame((*it)->loop_index)->local_index;Vector3d euler_conncected = Utility::R2ypr(q_array[connected_index].toRotationMatrix());Vector3d relative_t;relative_t = (*it)->getLoopRelativeT();double relative_yaw = (*it)->getLoopRelativeYaw();ceres::CostFunction* cost_function = FourDOFWeightError::Create( relative_t.x(), relative_t.y(), relative_t.z(),relative_yaw, euler_conncected.y(), euler_conncected.z());problem.AddResidualBlock(cost_function, loss_function, euler_array[connected_index], t_array[connected_index], euler_array[i], t_array[i]);}if ((*it)->index == cur_index)break;i++;}m_keyframelist.unlock();ceres::Solve(options, &problem, &summary);//std::cout << summary.BriefReport() << "\n";//printf("pose optimization time: %f \n", tmp_t.toc());/*for (int j = 0 ; j < i; j++){printf("optimize i: %d p: %f, %f, %f\n", j, t_array[j][0], t_array[j][1], t_array[j][2] );}*/m_keyframelist.lock();i = 0;//根据优化后的参数更新参与优化的关键帧的位姿for (it = keyframelist.begin(); it != keyframelist.end(); it++){if ((*it)->index < first_looped_index)continue;Quaterniond tmp_q;tmp_q = Utility::ypr2R(Vector3d(euler_array[i][0], euler_array[i][1], euler_array[i][2]));Vector3d tmp_t = Vector3d(t_array[i][0], t_array[i][1], t_array[i][2]);Matrix3d tmp_r = tmp_q.toRotationMatrix();(*it)-> updatePose(tmp_t, tmp_r);if ((*it)->index == cur_index)break;i++;}//根据当前帧的drift，更新全部关键帧位姿Vector3d cur_t, vio_t;Matrix3d cur_r, vio_r;cur_kf->getPose(cur_t, cur_r);cur_kf->getVioPose(vio_t, vio_r);m_drift.lock();yaw_drift = Utility::R2ypr(cur_r).x() - Utility::R2ypr(vio_r).x();r_drift = Utility::ypr2R(Vector3d(yaw_drift, 0, 0));t_drift = cur_t - r_drift * vio_t;m_drift.unlock();//cout << "t_drift " << t_drift.transpose() << endl;//cout << "r_drift " << Utility::R2ypr(r_drift).transpose() << endl;//cout << "yaw drift " << yaw_drift << endl;it++;for (; it != keyframelist.end(); it++){Vector3d P;Matrix3d R;(*it)->getVioPose(P, R);P = r_drift * P + t_drift;R = r_drift * R;(*it)->updatePose(P, R);}m_keyframelist.unlock();updatePath();}std::chrono::milliseconds dura(2000);std::this_thread::sleep_for(dura);}
}

7、keyframe.h

主要有两个类：BriefExtractor和KeyFrame

第一个类BriefExtractor：通过Brief模板文件，对图像的关键点计算Brief描述子

class BriefExtractor
{
public: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;
};

第二个类KeyFrame：构建关键帧，通过BRIEF描述子匹配关键帧和回环候选帧

代码流程图的出处：VINS-Mono代码解读——回环检测与重定位 pose graph loop closing by Manii

VINS-Mono 代码详细解读——回环检测与重定位、四自由度位姿图优化相关推荐

一文详解回环检测与重定位
标题:VINS-Mono代码解读-回环检测与重定位 pose graph loop closing 作者:Manii 来源:https://blog.csdn.net/qq_41839222/cate ...
【SLAM】VINS-MONO解析——回环检测和重定位
9. 回环检测与重定位本部分内容涉及到的代码大部分在pose_graph文件夹下,少部分在vins_estimator里. 原创内容,转载请先与我联系并注明出处,谢谢! 系列内容请点击:[SLAM] ...
视觉SLAM⑪----回环检测
目录 11.0 本章目标 11.1 概述 11.1.1 回环检测的意义 11.1.2 回环检测的方法 11.1.3 准确率和召回率 11.2 词袋模型 11.3 字典 11.3.1 字典的结构 11. ...
开源！用于3D激光雷达SLAM回环检测的实时词袋模型BoW3D
点击进入->3D视觉工坊学习交流群 0. 笔者个人体会回环检测对于SLAM系统的全局一致性有着至关重要的影响.现有的视觉SLAM回环检测大多是基于词袋模型,也就是2012年推出的BoW2库和2 ...
回环检测回环校正（一）：回环检测原理
回环检测回环校正(一):回环检测原理一.回环检测回环校正的意义参考: [1]徐宽. 融合IMU信息的双目视觉SLAM研究[D].哈尔滨工业大学,2018. 我们在进行运动估计之后,当固定了第一帧图 ...
SC-A-LOAM：在A-LOAM中加入回环检测
Thanks to LOAM, A-LOAM, and LIO-SAM code authors. The major codes in this repository are borrowed fr ...
segMatch:基于3D点云分割的回环检测
该论文的地址是:https://arxiv.org/pdf/1609.07720.pdf segmatch是一个提供车辆的回环检测的技术,使用提取和匹配分割的三维激光点云技术.分割的例子可以在下面的图 ...
Karto的后端优化与回环检测功能对比测试与分析
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达上篇文章讲解了Karto的前端是如何工作的. 这篇文章将slam_karto中的后端优化部分的代码添加 ...
M2DP：一种新的三维点云描述子及其在回环检测中的应用
文章:M2DP: A Novel 3D Point Cloud Descriptor and Its Application in Loop Closure Detection 作者:Li He , ...
经典激光slam配准及回环检测框架：ScanContext
论文名称: Scan context: Egocentric spatial descriptor for place recognition within 3d point cloud map 论文 ...

VINS-Mono 代码详细解读——回环检测与重定位、四自由度位姿图优化