本文主要介紹VINS的閉環(huán)檢測(cè)重定位與位姿圖優(yōu)化部分，作為系列文章的最后一節(jié)。
回環(huán)檢測(cè)的關(guān)鍵就是如何有效檢測(cè)出相機(jī)曾經(jīng)經(jīng)過(guò)同一個(gè)地方，這樣可以避免較大的累積誤差，使得當(dāng)前幀和之前的某一幀迅速建立約束，形成新的較小的累積誤差。由于回環(huán)檢測(cè)提供了當(dāng)前數(shù)據(jù)與所有歷史數(shù)據(jù)的關(guān)聯(lián)，在跟蹤算法丟失后，還可以利用重定位。
論文中主要分為兩部分：回環(huán)檢測(cè)與重定位、4-DOF的位姿圖優(yōu)化。
第一部分主要是為了通過(guò)回環(huán)檢測(cè)找到當(dāng)前幀和候選幀的聯(lián)系，并通過(guò)簡(jiǎn)單的緊耦合重定位將局部滑動(dòng)窗口移動(dòng)與過(guò)去的位姿對(duì)齊。
第二部分是為了保證基于重定位結(jié)果對(duì)過(guò)去的所有位姿進(jìn)行全局優(yōu)化。
論文中內(nèi)容為
0.4 重定位
vins的重定位模塊主要包含回環(huán)檢測(cè),回環(huán)候選幀之間的特征匹配,緊耦合重定位三個(gè)部分
A、回環(huán)檢測(cè)（只對(duì)關(guān)鍵幀）
1、采用DBoW2詞袋位置識(shí)別方法進(jìn)行回環(huán)檢測(cè)。經(jīng)過(guò)時(shí)間空間一致性檢驗(yàn)后，DBoW2返回回環(huán)檢測(cè)候選幀。
2、除了用于單目VIO的角點(diǎn)特征外，還添加了500個(gè)角點(diǎn)并使用BRIEF描述子，描述子用作視覺(jué)詞袋在數(shù)據(jù)庫(kù)里進(jìn)行搜索。這些額外的角點(diǎn)能用來(lái)實(shí)現(xiàn)更好的回環(huán)檢測(cè)。
3、VINS只保留所有用于特征檢索的BRIEF描述子，丟棄原始圖像以減小內(nèi)存。
4、單目VIO可以觀測(cè)到滾動(dòng)和俯仰角，VINS并不需要依賴旋轉(zhuǎn)不變性。
B、回環(huán)候選幀之間的特征匹配
1、檢測(cè)到回環(huán)時(shí)，通過(guò)BRIEF描述子匹配找到對(duì)應(yīng)關(guān)系。但是直接的描述子匹配會(huì)導(dǎo)致很多外點(diǎn)。
2、本文提出兩步幾何剔除法：
1）2D-2D：使用RANSAC進(jìn)行F矩陣測(cè)試，
2）3D-2D：使用RANSAC進(jìn)行PnP，基于已知的滑動(dòng)窗特征點(diǎn)的3D位置，和回路閉合候選處圖像的2D觀測(cè)（像素坐標(biāo)）。
當(dāng)內(nèi)點(diǎn)超過(guò)一定閾值時(shí)，我們將該候選幀視為正確的循環(huán)檢測(cè)并執(zhí)行重定位。
C、緊耦合重定位
1、重定位過(guò)程使單目VIO維持的當(dāng)前滑動(dòng)窗口與過(guò)去的位姿圖對(duì)齊。
2、將所有回環(huán)幀的位姿作為常量，利用所有IMU測(cè)量值、局部視覺(jué)測(cè)量和從回環(huán)中提取特征對(duì)應(yīng)值，共同優(yōu)化滑動(dòng)窗口。
?0.5 全局位姿圖優(yōu)化
A、位姿圖中添加關(guān)鍵幀
B、4自由度位姿圖優(yōu)化
C、位姿圖管理
?
相關(guān)代碼都在pose_graph文件中，主要分為三個(gè)程序，分別為：
- keyframe.cpp/.h 構(gòu)建關(guān)鍵幀類、描述子計(jì)算、匹配關(guān)鍵幀與回環(huán)幀
- pose_graph.cpp/.h 位姿圖的建立與圖優(yōu)化、回環(huán)檢測(cè)與閉環(huán)
- pose_graph_node.cpp ROS 節(jié)點(diǎn)函數(shù)、回調(diào)函數(shù)、主線程
1、pose_graph_node.cpp
主要分為7個(gè)回調(diào)函數(shù)以及主線程process（）函數(shù)。
7個(gè)回調(diào)函數(shù)：包括關(guān)鍵幀的位姿（keyframe_pose）、重定位位姿（relo_relative_pose）、相機(jī)到IMU的外參估計(jì)（extrinsic）、VIO里程計(jì)信息PQV（odometry）、關(guān)鍵幀中的3D點(diǎn)云（keyframe_point）、IMU傳播值（imu_propagate）。
?
ROS初始化，設(shè)置句柄
從launch文件讀取參數(shù)和參數(shù)文件config中的參數(shù)
如果需要進(jìn)行回環(huán)檢測(cè)則讀取詞典和BRIEF描述子的模板文件，同時(shí)讀取config中的其他參數(shù)、設(shè)置帶回環(huán)的結(jié)果輸出路徑。
加載先前位姿圖?loadPoseGraph()
訂閱各個(gè)topic并執(zhí)行各自回調(diào)函數(shù)
發(fā)布/pose_graph的topic
設(shè)置主線程 process() 和鍵盤控制線程 command()
int main(int argc, char **argv)
{// 1.ROS初始化，設(shè)置句柄ros::init(argc, argv, "pose_graph");ros::NodeHandle n("~");posegraph.registerPub(n);// 2.讀取參數(shù)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.如果需要進(jìn)行回環(huán)檢測(cè)則讀取詞典和BRIEF描述子的模板文件，同時(shí)讀取config中的其他參數(shù)、設(shè)置帶回環(huán)的結(jié)果輸出路徑。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并執(zhí)行各自回調(diào)函數(shù)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.發(fā)布的話題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. 創(chuàng)建兩個(gè)線程，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（）函數(shù)
先判斷是否需要回環(huán)檢測(cè)
得到具有相同時(shí)間戳的pose_msg、image_msg、point_msg
構(gòu)建pose_graph中用到的關(guān)鍵幀，然后每隔SKIP_CNT，將將距上一關(guān)鍵幀距離（平移向量的模）超過(guò)SKIP_DIS的圖像創(chuàng)建為關(guān)鍵幀。其中，最核心的函數(shù)是KeyFrame類以及addKeyFrame（）函數(shù)。
//主線程
void process()
{// 1.先判斷是否需要回環(huán)檢測(cè)if (!LOOP_CLOSURE)return;while (true)// 不斷循環(huán){// 三個(gè)參數(shù)圖像、點(diǎn)云、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.得到具有相同時(shí)間戳的pose_msg、image_msg、point_msgm_buf.lock();if(!image_buf.empty() && !point_buf.empty() && !pose_buf.empty())// 首先三個(gè)都不為空{(diào)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.構(gòu)建pose_graph中用到的關(guān)鍵幀，然后每隔SKIP_CNT，將將距上一關(guān)鍵幀距離（平移向量的模）超過(guò)SKIP_DIS的圖像創(chuàng)建為關(guān)鍵幀。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 剔除最開(kāi)始的SKIP_FIRST_CNT幀，if (skip_first_cnt < SKIP_FIRST_CNT){skip_first_cnt++;continue;}// 3.2每隔SKIP_CNT幀進(jìn)行一次 SKIP_CNT=0if (skip_cnt < SKIP_CNT){skip_cnt++;continue;}else{skip_cnt = 0;}// 3.3ROS圖像轉(zhuǎn)為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 將距上一關(guān)鍵幀距離（平移向量的模）超過(guò)SKIP_DIS的圖像創(chuàng)建為關(guān)鍵幀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 創(chuàng)建為關(guān)鍵幀類型并加入到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中添加關(guān)鍵幀，flag_detect_loop=1回環(huán)檢測(cè)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
接下來(lái)將主要講解兩個(gè)函數(shù)：addKeyFrame（）以及detectLoop（）
4、addKeyFrame（）添加關(guān)鍵幀，進(jìn)行回環(huán)檢測(cè)與閉環(huán)
建一個(gè)新的圖像序列
getVioPose（）獲取當(dāng)前幀的位姿vio_P_cur、vio_R_cur并更新updateVioPose
detectLoop（）回環(huán)檢測(cè)，返回回環(huán)候選幀的索引loop_index
計(jì)算當(dāng)前幀與回環(huán)幀的相對(duì)位姿，糾正當(dāng)前幀位姿w_P_cur、w_R_cur；回環(huán)得到的位姿和VIO位姿之間的偏移量shift_r、shift_t；如果存在多個(gè)圖像序列，則將所有圖像序列都合并到世界坐標(biāo)系下
獲取VIO當(dāng)前幀的位姿P、R，根據(jù)偏移量得到實(shí)際位姿并更新updatePose當(dāng)前幀的位姿P、R
發(fā)布path[sequence_cnt]
保存閉環(huán)軌跡到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.建一個(gè)新的圖像序列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（）獲取當(dāng)前幀的位姿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（）回環(huán)檢測(cè)，返回回環(huán)候選幀的索引loop_indexloop_index = detectLoop(cur_kf, cur_kf->index);}else{addKeyFrameIntoVoc(cur_kf);}// 4.存在回環(huán)候選幀if (loop_index != -1){//printf(" %d detect loop with %d \n", cur_kf->index, loop_index);// 4.1 獲取回環(huán)候選幀 KeyFrame* old_kf = getKeyFrame(loop_index);// 當(dāng)前幀與回環(huán)候選幀進(jìn)行描述子匹配,如果成功則確定存在回環(huán)if (cur_kf->findConnection(old_kf)){//earliest_loop_index為最早的回環(huán)候選幀if (earliest_loop_index > loop_index || earliest_loop_index == -1)earliest_loop_index = loop_index;// 4.2 計(jì)算當(dāng)前幀與回環(huán)幀的相對(duì)位姿，糾正當(dāng)前幀位姿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);//獲取當(dāng)前幀與回環(huán)幀的相對(duì)位姿relative_q、relative_tVector3d relative_t;Quaterniond relative_q;relative_t = cur_kf->getLoopRelativeT();relative_q = (cur_kf->getLoopRelativeQ()).toRotationMatrix();//重新計(jì)算當(dāng)前幀位姿w_P_cur、w_R_curw_P_cur = w_R_old * relative_t + w_P_old;w_R_cur = w_R_old * relative_q;//回環(huán)得到的位姿和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如果存在多個(gè)圖像序列，則將所有圖像序列都合并到世界坐標(biāo)系下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 將當(dāng)前幀放入優(yōu)化隊(duì)列中m_optimize_buf.lock();optimize_buf.push(cur_kf->index);m_optimize_buf.unlock();}}m_keyframelist.lock();Vector3d P;Matrix3d R;// 5.獲取VIO當(dāng)前幀的位姿P、R，根據(jù)偏移量得到實(shí)際位姿cur_kf->getVioPose(P, R);P = r_drift * P + t_drift;R = r_drift * R;// 更新當(dāng)前幀的位姿P、Rcur_kf->updatePose(P, R);// 6.發(fā)布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.保存閉環(huán)軌跡到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++;}}//當(dāng)前幀與其回環(huán)幀連線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（）回環(huán)檢測(cè)返回候選幀索引
query()查詢字典數(shù)據(jù)庫(kù)，得到與每一幀的相似度評(píng)分retadd()添加當(dāng)前關(guān)鍵幀到字典數(shù)據(jù)庫(kù)中 hconcat()通過(guò)相似度評(píng)分判斷是否存在回環(huán)候選幀
對(duì)于索引值大于50的關(guān)鍵幀才考慮進(jìn)行回環(huán),即前50幀不回環(huán),返回評(píng)分大于0.015的最早的關(guān)鍵幀索引min_index
//回環(huán)檢測(cè)返回候選幀索引
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.查詢字典數(shù)據(jù)庫(kù)，得到與每一幀的相似度評(píng)分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.添加當(dāng)前關(guān)鍵幀到字典數(shù)據(jù)庫(kù)中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.通過(guò)相似度評(píng)分判斷是否存在回環(huán)候選幀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);}}//確保與相鄰幀具有好的相似度評(píng)分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)//評(píng)分大于0.015則認(rèn)為是回環(huán)候選幀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.對(duì)于索引值大于50的關(guān)鍵幀才考慮進(jìn)行回環(huán),即前50幀不回環(huán)//返回評(píng)分大于0.015的最早的關(guān)鍵幀索引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（）位姿圖優(yōu)化
//四自由度位姿圖優(yōu)化
void PoseGraph::optimize4DoF()
{while(true){int cur_index = -1;int first_looped_index = -1;//取出最新一個(gè)待優(yōu)化幀作為當(dāng)前幀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++){//找到第一個(gè)回環(huán)候選幀，//回環(huán)檢測(cè)到幀以前的都略過(guò)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);//回環(huán)檢測(cè)到的幀參數(shù)設(shè)為固定if ((*it)->index == first_looped_index || (*it)->sequence == 0){ problem.SetParameterBlockConstant(euler_array[i]);problem.SetParameterBlockConstant(t_array[i]);}//add edge//對(duì)于每個(gè)i, 只計(jì)算它之前五個(gè)的位置和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)// 如果檢測(cè)到回環(huán){//必須回環(huán)檢測(cè)的幀號(hào)大于或者等于當(dāng)前幀的回環(huán)檢測(cè)匹配幀號(hào)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;//根據(jù)優(yōu)化后的參數(shù)更新參與優(yōu)化的關(guān)鍵幀的位姿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++;}//根據(jù)當(dāng)前幀的drift，更新全部關(guān)鍵幀位姿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
主要有兩個(gè)類：BriefExtractor和KeyFrame
第一個(gè)類BriefExtractor：通過(guò)Brief模板文件，對(duì)圖像的關(guān)鍵點(diǎn)計(jì)算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;
};
第二個(gè)類KeyFrame：構(gòu)建關(guān)鍵幀，通過(guò)BRIEF描述子匹配關(guān)鍵幀和回環(huán)候選幀
?
?
代碼流程圖的出處：VINS-Mono代碼解讀——回環(huán)檢測(cè)與重定位 pose graph loop closing by Manii
總結(jié)
以上是生活随笔為你收集整理的VINS-Mono 代码详细解读——回环检测与重定位、四自由度位姿图优化的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。
如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。
