[發明專利]一種基于點線特征的RGB-D視覺SLAM方法

有效

• 申請號: 202011358438.9
• 申請日: 2020-11-27
• 公開（公告）號: CN112381890B
• 公開（公告）日: 2022-08-02
• 發明（設計）人: 方志軍;盧俊鑫;高永彬;楊淑群;方榮輝
• 申請（專利權）人: 上海工程技術大學
• 主分類號: G06T7/80
• 分類號: G06T7/80;G06T7/521;G01C21/00
• 代理公司: 上海海頌知識產權代理事務所(普通合伙) 31258
• 代理人: 何葆芳;馬云

1.一種基于點線特征的RGB-D視覺SLAM方法，其特征在于，包括如下步驟：

S1、RGB-D相機內參標定：以RGB-D相機為圖像采集設備，對RGB-D相機進行內參標定，其中，RGB-D相機的內參包括相機的主點、焦距和畸變系數；

S2、點、線特征的提?。簩Ξ斍皶r刻RGB-D相機獲取的圖像幀進行邊緣檢測和直線擬合，并進一步對擬合得到的直線進行篩選和線點采樣，采集出的點組成點集；

S3、獲取相機初始變換位姿：對RGB-D相機獲取的不同圖像幀進行變換位姿初始化，獲取相機初始變換位姿；

S4、關鍵幀到當前幀的直線投影：根據步驟S3得到的相機初始變換位姿將步驟S2得到的位于關鍵幀上的直線投影到當前幀；

S5、構建基于線段局部梯度的適配性誤差，形成視覺里程計模塊的誤差函數：根據步驟S4得到當前幀上的投影直線，利用投影直線與投影直線鄰域圖像數據的關系得到每條直線在當前幀的適配程度值，將每一條投影直線的適配程度值進行累加從而形成視覺里程計模塊的誤差函數；

S6、當前幀相機初始變換位姿的迭代優化和當前幀相機的位姿求解：根據步驟S5最后形成的誤差函數，利用非線性誤差函數優化方法進行相機初始變換位姿的迭代優化，并將優化得到的當前幀與關鍵幀的變換位姿左乘關鍵幀位姿ξ_k求解出當前幀相機的位姿ξ_c，從而獲取相機在當前幀時刻下的具體位置，實現相機的定位；

S7、關鍵幀的判斷：基于步驟S5得到當前幀的直線適配程度值進行當前幀是否是關鍵幀的判斷，如果判斷當前幀為關鍵幀，則將當前幀替換保存為新的關鍵幀，并將其插入后端滑動窗口，進行局部BA優化；如果判斷當前幀為非關鍵幀，則不進行關鍵幀的相關操作，返回步驟S2；

S8、局部BA優化：當步驟S7中判斷當前幀為關鍵幀時，將當前幀插入后端局部BA優化滑動窗口，利用步驟S2提取的點、線特征，步驟S6求解出的相機變換位姿，滑動窗口內的所有關鍵幀和所屬的點、線特征構建基于滑動窗口的共視圖局部BA優化；

S9、回環檢測：對步驟S8得到的新關鍵幀與已有的所有關鍵幀進行回環檢測以及全局圖優化，具體為：基于詞典模型，對每一個新插入的關鍵幀與之前的所有關鍵幀進行回環檢測，如果檢測到回環幀，則利用步驟S5中視覺里程計模塊構建的誤差函數進行新關鍵幀與回環幀之間所有關鍵幀的圖優化；

S10、重定位：當步驟S6中利用視覺里程計模塊的誤差函數求解出的相機變換位姿與設定的閾值相差過大時，則當前幀相機的位姿求解判定為失敗，則進行重定位；重定位過程中先進行步驟S9的回環檢測處理，尋找當前幀對應的回環幀，然后跳轉至步驟S3進行初始變換位姿的估計，進而繼續依次執行相應的后續操作；

S11、地圖構建：利用上述步驟求解得到每一關鍵幀的位姿以及上述步驟得到的點、線特征構建稀疏地圖；

S12、結果輸出：位姿保存，相機運動路徑繪制和地圖的保存與同步實時顯示。

2.根據權利要求1所述的基于點線特征的RGB-D視覺SLAM方法，其特征在于：所述步驟S1中，采用張正友相機標定法對RGB-D相機進行內參K標定。

3.根據權利要求1所述的基于點線特征的RGB-D視覺SLAM方法，其特征在于：所述步驟S2中，對RGB-D相機獲取的視頻圖像幀進行邊緣檢測和直線擬合，是先對RGB-D相機獲取的視頻圖像幀進行包括去噪、邊緣增強在內的預處理操作；然后利用邊緣探測器對預處理之后的圖像進行邊緣提?。蛔詈笤賹μ崛〕龅倪吘墧M合出所需的直線，對擬合出的直線進行篩選和線點采樣，采集出的點組成點集。

4.根據權利要求1所述的基于點線特征的RGB-D視覺SLAM方法，其特征在于，所述步驟S3中獲取相機初始變換位姿，包括如下步驟：

S31、先判斷當前RGB-D相機獲取的圖像幀屬于第幾幀，然后根據判斷結果對于不同圖像幀分別執行步驟S32、S33或S34中的相機位姿初始化方案，獲取對應圖像幀的初始變換位姿；

S32、如果當前幀圖像為第一幀圖像，變換位姿采用單位陣I進行初始化；

S33、如果當前幀為第二幀圖像或重定位圖像幀，利用步驟S2采集的點集，通過幾何變換中的透視N點投影法求解相機的初始變換位姿；

S34、如果當前幀非第一、第二和重定位圖像幀，基于連續運動模型進行變換位姿的初始化。