資料型別和結構

該SLAM工程中的幾個基本資料結構及相互之間的關系
前端、后端及地圖之間的關系
相機型別

程式運行程序

一、順序執行程序

1.初始化程序(第一幀)，

assert(vo->Init() == true);

Init()函式位于visual_odometry.cpp檔案中，

首先傳遞了組態檔路徑(config_file_path_)，后來可以發現在組態檔中可以找到資料集檔案(dataset)存放的路徑，
然后對各個運行結構初始化，對于前端(主執行緒)初始化時生成frontend_指標，猜測初始化時相機幀姿態位置與世界坐標系重合，后端初始化時生成backend_指標，并且通過Backendloop函式創建執行緒并啟動，但隨后便進入map_update_.wait(lock)，被阻塞，地圖初始化時生成Map_指標，

 vo->Run();

進入run()函式后，總體上進入了如下流程

首先進入step()函式，讀取資料集(NextFrame)并在前端新建幀(frontend_->AddFrame(new_frame))，
進入AddFrame()函式，此時前端狀態為FrontendStatus::INITING，
進入StereoInit()函式，首先通過DetectFeatures()檢測左圖特征點并將特征集添加入該幀(回傳左圖特征點個數)，然后通過FindFeaturesInRight()檢測能與左特征點配對的右側特征點并將特征集添加入該幀(回傳匹配成功的特征點個數)，若配對數量<初始化要求最低特征點，則回傳false，此時由于狀態status未改變仍為FrontendStatus::INITING，下一幀讀入時將仍進入StereoInit()函式，
若配對數量足夠，進入BuildInitMap()函式，通過左右配對特征點，三角化還原觀測到的路標的三維坐標，(生成各個路標點后，要用AddObservation()添加該路標的觀測特征點，同時要在相應的特征點中要添加指向該路標的指標)，
通過InsertMapPoint()將全部的路標點加入到地圖中，并將此幀設為關鍵幀(因為是第一個可以提取有效數量路標點的幀)，并插入該關鍵幀于Map_，
backend_->UpdateMap()觸發后端地圖更新，通過map_update_.notify_one()喚醒被阻塞的后端行程，優化位姿和路標點（但是只有第一幀時的優化結果不知道是什么樣子），退出BuildInitMap()函式后，將狀態status改為FrontendStatus::TRACKING_GOOD
StereoInit()函式結束，last_frame_ = current_frame_;把當前幀傳遞給上一幀，
進入步驟1回圈，

2.第二次進入回圈(第二幀)

首先進入step()函式，讀取資料集(NextFrame)并在前端新建幀(frontend_->AddFrame(new_frame))，(與初始化程序無異)
switch (status_)，(這里無論是bad或是good都會進入Track()函式)，
Track()函式：若上一幀存在，則基于恒速假設(即假設A->B,B->C間的位姿變換相同)，將當前幀的位姿T3估計為T2=T21·T1(已知)，T3=T21·T2，(但此時relative_motion_為初始默認值，可能為單位矩陣)，
進入TrackLastFrame()函式，使用上一幀與當前幀的左圖進行光流追蹤，先預估特征點在當前幀左圖出現的位置(光流追蹤可能以這個為參考)，使用上一幀左圖的特征點集[i]，若上一幀存在對應的路標點，則用該路標點與當前幀的預計位姿得到該幀左圖的特征點位置；若不存在對應路標點(即上一幀的該特征點的路標點三角化失敗)，則直接使用上一幀特征點位置作為該幀特征點位置，這樣得到兩組特征點集(kps_last、kps_current)，對其光流追蹤，得到當前幀追蹤后的特征點集(與上一幀匹配成功的)，并對匹配的特征點根據上一幀添加路標點指向(注意這里只進行了特征點->路標點的指向)，
進入EstimateCurrentPose()函式，通過當前幀預估位姿，左圖特征點集，提取的路標點建立區域優化，僅對當前幀的位姿進行優化，并將優化后的位姿作為當前幀的位姿，最后將計算誤差較大對應的特征點與其指向的路標點解除關系，但并未洗掉該特征點，（該函式回傳值為匹配特征點-例外特征點=誤差范圍內的有效特征點）
根據有效特征點tracking_inliers的數量與閾值(num_features_tracking_和num_features_tracking_bad_)比較，以此決定狀態status為TRACKING_GOOD還是TRACKING_BAD還是LOST丟失，隨后退出Track()函式，
進入 InsertKeyframe()函式，在這個函式內部根據有效匹配
特征點數量判斷是否將此幀設為關鍵幀，
7.1 若設為關鍵幀，則為該幀分配關鍵幀ID，在地圖中插入該幀，并為該幀已有特征點與路標添加雙指向關聯，隨后進入DetectFeatures()，通過mask掩模遮蓋以有的特征點，以檢測新的特征點，(事實上與上一幀追蹤到的特征點少，可能是因為含有的新特征->新路標點較多) 然后通過FindFeaturesInRight()函式在右圖中找到對應于左圖的特征集，對于左圖未關聯路標的特征點，且在右圖能尋到相匹配的特征點，嘗試三角化得到對應的路標，為新路標與特征點添上雙指向關聯，并在地圖中添加該路標，隨后觸發地圖更新，
7.2 若不設為關鍵幀，則退出InsertKeyframe()函式，
計算相對位姿變換，得到relative_motion_為下次恒速假設做準備，

二、三角化演算法與SVD求解

檔案為algorithm.h
演算法的數學原理參考該文章三角測量（Triangulation 三角化）與 SVD 求解
需要注意的是，在程式中triangulation()函式得到的實際上是雙目視角下的坐標，需要左乘Twc后得到世界坐標系下的路標點，即Yw=Twc·Yc，

SE3 current_pose_Twc = current_frame_->Pose().inverse();

三、Map::RemoveOldKeyframe()與Map::CleanMap()

根據之前的分析可知，對于提取新路標點做出貢獻的僅有關鍵幀，非關鍵幀只優化了位姿，其特征點也僅關聯了舊的路標點，后端優化時僅優化Map中激活的關鍵幀和激活的路標點，路標點的觀測次數僅由關鍵幀中的特征點貢獻，

Map::RemoveOldKeyframe(): Map新增關鍵幀后，同時會將該幀加入激活關鍵幀中，如果激活關鍵幀總數量多于閾值，則需要剔除舊的關鍵幀，方法是尋找與當前幀最近與最遠的兩個關鍵幀(使用相對位姿變換的范數比較)，如果存在很近的幀，優先刪掉最近的；否則刪掉最遠的，激活關鍵幀的變化會引起激活的路標點的被觀測次數的變化，因此通過移除的關鍵幀的左、右側特征點尋找其關聯的路標點，并減少路標點的觀測次數(即激活的路標點與要剔除的關鍵幀之間的聯系)，
Map::CleanMap()：減少某些激活的路標點的觀測次數后，會出現某些激活的特征點未被任何激活的關鍵幀觀測到的情況(即路標點被觀測次數為0)，此時需要將這些路標點從激活路標點中去除，使其不再參與優化，

轉載請註明出處，本文鏈接：https://www.uj5u.com/qita/259163.html

標籤：AI

上一篇：141家！工業互聯網企業市值排行榜出爐！

下一篇：人工智能相關論文 2020 accepted papers list

《SLAM十四講第二版》CH13專案學習筆記