本文主要介紹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()查詢字典數據庫，得到與每一幀的相似度評分retadd()添加當前關鍵幀到字典數據庫中 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 代码详细解读——回环检测与重定位、四自由度位姿图优化的全部內容，希望文章能夠幫你解決所遇到的問題。
如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。
