1、闭环检测常用方法有哪些？

2、ORB SLAM2中Loop Closing的具体实现流程是怎样的？

3、VINS回环检测与重定位、四自由度位姿图优化

3.1 第一部分：回环检测与重定位

3.1.1 回环检测（只对关键帧）

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

3.1.3 紧耦合重定位

3.2 第二部分：全局位姿图优化

3.3 代码解析

3.3.1 pose_graph_node.cpp

3.3.2 process()函数

3.3.3 pose_graph.h

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

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

3.3.6 optimize4DoF（）位姿图优化

3.3.7 keyframe.h

参考文章：

在正式开始学习VINS之前，先来看几个SLAM面试中的小问题：

1、闭环检测常用方法有哪些？

1. ORB SLAM中采用的是词袋模型进行闭环检测筛选出候选帧，再通过求解Sim3判断最合适的关键帧。

Sim3：使用3对匹配点来进行相似变换（similarity transformation）的求解，进而解出两个坐标系之间的旋转矩阵、平移向量和尺度。

2. LSD SLAM中的闭环检测主要是根据视差、关键帧连接关系，找出候选帧，然后对每个候选帧和测试的关键帧之间进行双向Sim3跟踪，如果求解出的两个李代数满足马氏距离在一定范围内，则认为闭环成功。

3. 点云上的回环检测用的是scan-context。

4. RGB-D SLAM 回环检测，回环的本质就是识别曾经到过的地方，最简单的回环检测策略，就是把新来的关键帧与之前所有的关键帧进行比较，不过这样会导致越往后，需要比较的帧越多。所以，稍微快速一点的方法是在过去的帧里随机挑选一些，与当前帧进行比较。更进一步的，也可以用图像处理/模式识别的方法计算图像间的相似性，对相似的图像进行检测。

RGB-D 中的回环检测策略流程概括如下：

初始化关键帧序列：F，并将第一帧 $f_{0}$ 放入F。
对于新来的一帧I，计算F中最后一帧与I的运动，并估计该运动的大小e。有以下几种可能：
1. 若e> $E_{error}$ ，说明运动太大，可能是计算错误，丢弃该帧；
2. 若没有匹配上（match太少），说明该帧图像质量不高，丢弃；
3. 若e< $E_{key}$ ，说明离前一个关键帧很近，同样丢弃；
4. 剩下的情况，只有是特征匹配成功的，运动估计正确，同时离上一个关键帧有一定距离，则把I作为新的关键帧，进入回环检测程序；
近距离回环：匹配I与F末尾m个关键帧，匹配成功时，在图里增加一条边。
随机回环：随机在F里取n个帧，与I进行匹配。若匹配上（这里说的匹配是根据两帧内点数量进行判断，内点数量少于阈值则匹配失败；而内点数量inliers计算来自两帧之间cv::solvePnPRansac函数最后一个参数），往图里增加一条边。
将I放在F末尾。若有新的数据，则回2；若无，则进行优化与地图拼接。

总结来说的话，就是根据关键帧进行近距离回环检测和随机回环检测，看当前帧能否与历史关键帧进行匹配，匹配成功就使用g2o添加边，然后进行全局BA位姿优化。

6. 如下，ORB-SLAM2中的回环检测。

2、ORB SLAM2中Loop Closing的具体实现流程是怎样的？

①首先是通过回环检测（Bow得分）和共视关系检查从所有关键帧中筛选出一组和当前帧有可能形成闭环的候选帧；

②然后利用相似求解器Sim3Solver求解出候选帧与当前帧之间的相似变换（注意这里是单目相似变换，而双目或者RGBD是刚体变换），利用相似变换找出更多的匹配地图点；

③然后进行闭环位姿优化（对应下面的回答），如果优化结果较好的话就不再判断其他候选帧。

④下一步就是闭环矫正，通过得出来的相似变换对当前帧进行位姿调整并且传播到与当前帧相连的关键帧，回环两侧的关键帧完成对齐，然后利用调整过的位姿更新这些相连关键帧对应的地图点，同时在Covisibility Graph里面增加闭环边，然后进行Essential Graph的优化（即全局位姿优化），当前帧与闭环匹配帧之间的边不进行优化，

⑤最后再来一个全局BA优化即完成了Loop Closing的全部流程。

3、VINS回环检测与重定位、四自由度位姿图优化

下面正式开始学习VINS代码中的回环检测部分。

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

第一部分主要是为了通过回环检测找到当前帧和候选帧的联系，并通过简单的紧耦合重定位将局部滑动窗口移动与过去的位姿对齐。
第二部分是为了保证基于重定位结果对过去的所有位姿进行全局优化。

3.1 第一部分：回环检测与重定位

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

3.1.1 回环检测（只对关键帧）

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

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

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

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

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

1、检测到回环时，通过BRIEF描述子匹配找到对应关系。但是直接的描述子匹配会导致很多外点。 2、本文提出两步几何剔除法：
1）2D-2D： 使用RANSAC进行F矩阵测试，
2）3D-2D： 使用RANSAC进行PnP，基于已知的滑动窗特征点的3D位置，和回路闭合候选处图像的2D观测（像素坐标）。当内点超过一定阈值时，我们将该候选帧视为正确的循环检测并执行重定位。

3.1.3 紧耦合重定位

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

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

3.2 第二部分：全局位姿图优化

A、位姿图中添加关键帧
B、4自由度位姿图优化
C、位姿图管理

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

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

3.3 代码解析

3.3.1 pose_graph_node.cpp

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

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

1、ROS初始化，设置句柄
2、从launch文件读取参数和参数文件config中的参数
3、如果需要进行回环检测则读取词典和BRIEF描述子的模板文件，同时读取
4、config中的其他参数、设置带回环的结果输出路径。
5、加载先前位姿图 loadPoseGraph()
6、订阅各个topic并执行各自回调函数
7、发布/pose_graph的topic
8、设置主线程 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;
}

3.3.2 process()函数

1、先判断是否需要回环检测
2、得到具有相同时间戳的pose_msg、image_msg、point_msg
3、构建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.3.3 pose_graph.h

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

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

1、建一个新的图像序列
        2、getVioPose（）获取当前帧的位姿vio_P_cur、vio_R_cur并更新 updateVioPose
        3、detectLoop（）回环检测，返回回环候选帧的索引loop_index
        4、计算当前帧与回环帧的相对位姿，纠正当前帧位姿w_P_cur、w_R_cur；回环得到的位姿和VIO位姿之间的偏移量shift_r、shift_t；如果存在多个图像序列，则将所有图像序列都合并到世界坐标系下
        5、获取VIO当前帧的位姿P、R，根据偏移量得到实际位姿并更新 updatePose当前帧的位姿P、R
        6、发布path[sequence_cnt]
        7、保存闭环轨迹到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();
}

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

1、query()查询字典数据库，得到与每一帧的相似度评分ret 2、add()添加当前关键帧到字典数据库中。
3、hconcat()通过相似度评分判断是否存在回环候选帧。
4、对于索引值大于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;}

3.3.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);}
}

3.3.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描述子匹配关键帧和回环候选帧。

参考文章：

1、https://blog.csdn.net/hltt3838/article/details/105378326/?utm_medium=distribute.pc_relevant.none-task-blog-2~default~baidujs_title~default-0.base&spm=1001.2101.3001.4242 VINS-Mono 代码详细解读——回环检测与重定位、四自由度位姿图优化

2、https://blog.csdn.net/hltt3838/article/details/109454760 VINS-Mono 代码解析四、闭环检测和优化

3、https://blog.csdn.net/qq_41839222/article/details/87878550 VINS-Mono代码解读——回环检测与重定位 pose graph loop closing

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. ...
视觉SLAM十四讲学习笔记——ch11回环检测
文章目录 11.1 理论部分 11.2 实践部分 11.2.1 创建字典 11.2.2 利用已经创建的vocabulary.yml.gz字典,得到图片与图片之间相似性的评分 11.2.3 利用规模更大 ...
视觉slam十四讲 pdf_视觉SLAM十四讲|第12讲回环检测
1. 什么是回环检测前面有说过累积误差的问题,前一时刻的误差会积累到后面,导致画不成圈圈,如图12-1所示,而画圈圈(全局一致性)很重要,所以需要有一个步骤来纠正当前的计算偏差. 回环检测通过判断相 ...
【SLAM十四讲】ch11 回环检测词袋法实验得出相似分数后计算PR曲线 VPR实验编辑中
[SLAM十四讲]ch11 回环检测词袋法实验得出相似分数后计算PR曲线 [SLAM十四讲]ch11 回环检测词袋法实验得出相似分数后计算PR曲线 DBow3库安装 ch11编译 ch11 词 ...
SLAM学习——回环检测
1.回环检测回环检测的关键,就是如何有效的检测出相机经过同一个地方这件事.它关系到我们估计的轨迹和地图在长时间下的正确性. 由于回环检测提供了当前数据与所有历史数据的关联,在跟踪算法丢失后,我们还可 ...
segMatch:基于3D点云分割的回环检测
该论文的地址是:https://arxiv.org/pdf/1609.07720.pdf segmatch是一个提供车辆的回环检测的技术,使用提取和匹配分割的三维激光点云技术.分割的例子可以在下面的图 ...
详解 | SLAM回环检测问题
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达本文经知乎作者fishmarch授权转载,二次转载请联系作者原文 ...
综述 | SLAM回环检测方法
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者丨任旭倩来源丨计算机视觉life 在视觉SLAM问题中,位姿的估计往往是一个递推的过程,即由上一 ...

VINS-Mono学习（四）——回环检测与重定位

1、闭环检测常用方法有哪些？

2、ORB SLAM2中Loop Closing的具体实现流程是怎样的？

3、VINS回环检测与重定位、四自由度位姿图优化

3.1 第一部分：回环检测与重定位

3.1.1 回环检测（只对关键帧）

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

3.1.3 紧耦合重定位

3.2 第二部分：全局位姿图优化

3.3 代码解析

3.3.1 pose_graph_node.cpp

3.3.2 process()函数

3.3.3 pose_graph.h

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

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

3.3.6 optimize4DoF（）位姿图优化

3.3.7 keyframe.h

参考文章：

VINS-Mono学习（四）——回环检测与重定位相关推荐

最新文章

热门文章