第五篇 openvslam建圖與優化模組梳理
- 2019 年 10 月 3 日
- 筆記
建圖模組
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；