第五篇 建圖與優化模組
阿新 • • 發佈：2019-09-21
建圖模組
mapping_module在初始化系統的時候進行例項化，在構建例項的時候會例項化local_map_cleaner和local_bundle_adjuster。系統啟動的時候會在另外一個執行緒中啟動該模組。
// src/openvslam/system.cc:78 mapper_ = new mapping_module(map_db_, camera_->setup_type_ == camera::setup_type_t::Monocular); // src/openvslam/system.cc:123 mapping_thread_ = std::unique_ptr<std::thread>(new std::thread(&openvslam::mapping_module::run, mapper_));
對新增的關鍵幀進行建圖
// src/openvslam/mapping_module.cc:128
void mapping_module::mapping_with_new_keyframe()對新增的關鍵幀進行建圖mapping_with_new_keyframe 取出佇列中最早放入的關鍵幀 設定原始關鍵幀ID 將新關鍵幀儲存入資料庫store_new_keyframe 計算當前關鍵幀的BoW特徵向量 獲取當前關鍵幀的lm 如果當前關鍵幀可以觀測到該lm，將lm新增入local_map_cleaner的fresh_landmarks中 否則 新增lm的觀測 更新lm幾何資訊 計算lm的描述子 更新圖連線update_connections 獲取圖關鍵幀所有lm 統計出共視關鍵幀以及共視lm數量情況，篩選出共視lm大於15的關鍵幀weight_covisibility_pairs 尋找出共視lm數量最多的關鍵幀當做最近的共視關鍵幀 圖中新增連線add_connection 對weight_covisibility_pairs進行降序排列，更新ordered_covisibilities_和ordered_weights_ 更新生成樹。最近的共視關鍵幀設定為父樹，當前關鍵幀為子樹 儲存關鍵幀 去除冗餘的lm（remove_redundant_landmarks） 移除的邏輯 1. observed_ratio小於觀測門限，需要從區域性地圖buffer和資料庫中移除； 2. 如果lm被新增之後一段時間內，被觀測到的其他幀觀測到的次數<=2,則認為時無效幀，需要從區域性地圖buffer和資料庫中移除； 3. 如果lm被新增之後一段時間內被多次觀測到，則認為該lm有效，只從區域性地圖buffer中移除； 依據當前關鍵幀和共視關鍵幀重新三角化lm（create_new_landmarks） 獲取當前關鍵幀權重高的共視關鍵幀 逐個計算當前關鍵幀與共視關鍵幀之間的本質矩陣 利用本質矩陣計算出兩關鍵幀lm的匹配情況 三角化匹配的lm，三角化成功的點將新增入庫 檢測處理重複的lm（update_new_keyframe） 獲取兩層共視關鍵幀 融合重複的lm(fuse_landmark_duplication) 獲取當前幀的lm 逐個共視關鍵幀去重（replace_duplication） 如果lm沒有被共視關鍵幀觀測到 將lm重投影至該共視關鍵幀，提取共視關鍵幀投影區域附近的特徵點，如果相似度很高且重投影誤差很小， 使用共視關鍵幀的lm點取代當前的lm，如果共視關鍵幀沒有對應的lm則把當前lm新增到共視關鍵幀 獲取所有共視關鍵幀的lm集合再次進行去重（replace_duplication） 這裡和上面去重輸入引數是不一樣的，這裡的關鍵幀是當前關鍵幀，lm是共視關鍵幀lm集合 更新當前幀lm幾何資訊和圖連線 進行區域性地圖BA（local_bundle_adjuster） 祛除冗餘的關鍵幀remove_redundant_keyframes 獲取當前關鍵幀的共視關鍵幀 逐個共視關鍵幀計算冗餘觀測count_redundant_observations 獲取當前共視關鍵幀的lm 逐個lm統計其共視關鍵幀數量，如果不小於3個就認為lm是冗餘的num_redundant_obs++ lm深度有效就認為是有效的lm，num_valid_obs++ 如果num_redundant_obs / num_valid_obs > 0.9，則認為該共視關鍵幀是冗餘的，移除掉 將新的關鍵幀傳送給全域性優化模組佇列
觀測門限清理
在跟蹤模組優化區域性地圖optimize_current_frame_with_local_map時，首先會統計出當前幀可以被觀測到的lm，會呼叫lm->increase_num_observable()，位姿優化後統計inlier的lm，呼叫lm->increase_num_observed()，因此num_observed_記錄的是跟蹤過程中真正有效的lm，num_observable_記錄的是區域性地圖中可以被當前幀觀測到的lm，如果num_observed_ / num_observable_的值很小的話，說明該lm對位姿評估沒有太大意義，可以從區域性地圖buffer和資料庫中清除掉。
// src/openvslam/tracking_module.cc:340
bool tracking_module::optimize_current_frame_with_local_map()全域性優化模組
和建圖模組一樣， 全域性優化模組global_optimization_module在初始化系統的時候進行例項化，在構建例項的時候會例項化graph_optimizer、loop_detector和loop_bundle_adjuster。系統啟動的時候會在另外一個執行緒中啟動該模組。
// src/openvslam/system.cc:80
global_optimizer_ = new global_optimization_module(map_db_, bow_db_, bow_vocab_, camera_->setup_type_ != camera::setup_type_t::Monocular);
// src/openvslam/system.cc:124
global_optimization_thread_ = std::unique_ptr<std::thread>(new std::thread(&openvslam::global_optimization_module::run, global_optimizer_));
run
取出佇列中最早放入的關鍵幀
設定標記保證在迴環檢測和校正期間關鍵幀不被擦除
將關鍵幀傳入迴環檢測模組
檢測迴環候選detect_loop_candidates
迴環檢測功能被禁用或者剛剛被校正過，則不需要檢測迴環候選，直接將關鍵幀新增到bow_db
1. 通過查詢BoW資料庫來搜尋迴圈候選者
在查詢之前，計算當前關鍵幀和每個共視關鍵幀之間的BoW相似性的最小分數
獲取迴環候選acquire_loop_candidates
獲取與當前關鍵幀相連的關鍵幀集合（通過graph_node）
統計當前關鍵幀和其他關鍵幀共享單詞數量情況
將最大共享單詞數量*0.8作為最小共享單詞門限
計算共享單詞數滿足條件的候選關鍵幀於當前關鍵幀的bow得分
挑選出不大於最小分數候選關鍵幀對兒score_keyfrm_pairs
計算每個候選關鍵幀（score_keyfrm_pairs）鄰域的得分並取總和best_total_score
大於best_total_score*0.75才為有效的候選幀
沒有迴環候選幀將當前關鍵幀新增到BoW database
2. 尋找連續關鍵幀集合find_continuously_detected_keyframe_sets
逐個查看回環候選關鍵幀
獲取相連的關鍵幀集合
檢測與之前連續關鍵幀集合是否相連，如果相連，新增入當前連續關鍵幀集合
3. 將連線數大於2的關鍵幀放入迴環候選
4. 儲存當前連續關鍵幀集合以便下次使用
如果當前關鍵沒有找到迴環候選，那麼當前幀是可以被刪除的
驗證迴環並且從中選出一個validate_candidates
使用線性和非線性的方式評估當前關鍵幀和候選關鍵幀的sim3，挑選出迴環候選幀
校正迴環correct_loop
獲取當前關鍵幀的共視關鍵幀
獲取迴環校正前的共視關鍵幀的Sim3s_nw
計算迴環校正後的共視關鍵幀的Sim3s_nw
校正共視關鍵幀中的lm的位置資訊
校正共視關鍵幀的位姿資訊
處理迴環融合帶來的重複lm（replace_duplicated_landmarks）
用迴環候選關鍵中的lm替換當前幀中的lm
使用match::fuse檢測重複的關鍵點
獲取新的連線關係，進行圖優化sim3
優化的變數只有關鍵幀位姿，優化完成後使用優化的結果調整landmark
添加回環邊
最後進行loop BA
問題
- 搞清楚線性和非線性sim3；