[發明專利]一種視覺慣導設備的位姿跟蹤方法、裝置及視覺慣導設備

有效

• 申請號: 201911354390.1
• 申請日: 2019-12-25
• 公開（公告）號: CN113034538B
• 公開（公告）日: 2023-09-05
• 發明（設計）人: 毛洋;宋江新;毛慧;浦世亮
• 申請（專利權）人: 杭州海康威視數字技術股份有限公司
• 主分類號: G06T7/246
• 分類號: G06T7/246;G06T7/73
• 代理公司: 北京柏杉松知識產權代理事務所(普通合伙) 11413
• 代理人: 李欣;丁蕓

1.一種視覺慣導設備的位姿跟蹤方法，其特征在于，所述方法包括：

獲取圖像采集單元采集的當前幀，以及慣性測量單元采集的當前慣性數據；

將三維地圖中的地圖點投影至所述當前幀，確定所述當前幀中特征點與所述三維地圖中地圖點對應的第一視覺約束關系；獲取所述當前幀的上一幀中特征點與所述三維地圖中地圖點對應的第二視覺約束關系；根據所述當前慣性數據，確定所述當前幀與所述上一幀之間的慣性約束關系；獲取所述上一幀之前的歷史幀對所述上一幀的先驗約束關系；

利用所述第一視覺約束關系、所述第二視覺約束關系、所述慣性約束關系及所述先驗約束關系，確定視覺慣導設備的當前位姿；

其中，所述利用所述第一視覺約束關系、所述第二視覺約束關系、所述慣性約束關系及所述先驗約束關系，確定視覺慣導設備的當前位姿，包括：

利用所述第一視覺約束關系、所述第二視覺約束關系、所述慣性約束關系及所述先驗約束關系，構建視覺慣導設備的聯合優化函數；利用優化方法，對所述聯合優化函數中的視覺慣導設備的位姿進行優化，得到視覺慣導設備的當前位姿。

2.根據權利要求1所述的方法，其特征在于，在所述將三維地圖中的地圖點投影至所述當前幀，確定所述當前幀中特征點與所述三維地圖中地圖點對應的第一視覺約束關系；獲取所述當前幀的上一幀中特征點與所述三維地圖中地圖點對應的第二視覺約束關系；根據所述當前慣性數據，確定所述當前幀與所述上一幀之間的慣性約束關系；獲取所述上一幀之前的歷史幀對所述上一幀的先驗約束關系之前，所述方法還包括：

統計所述當前幀中特征點的數目；

所述將三維地圖中的地圖點投影至所述當前幀，確定所述當前幀中特征點與所述三維地圖中地圖點對應的第一視覺約束關系；獲取所述當前幀的上一幀中特征點與所述三維地圖中地圖點對應的第二視覺約束關系；根據所述當前慣性數據，確定所述當前幀與所述上一幀之間的慣性約束關系；獲取所述上一幀之前的歷史幀對所述上一幀的先驗約束關系，包括：

若所述數目小于預設閾值，則將三維地圖中的地圖點投影至所述當前幀，確定所述當前幀中特征點與所述三維地圖中地圖點對應的第一視覺約束關系；獲取所述當前幀的上一幀中特征點與所述三維地圖中地圖點對應的第二視覺約束關系；根據所述當前慣性數據，確定所述當前幀與所述上一幀之間的慣性約束關系；獲取所述上一幀之前的歷史幀對所述上一幀的先驗約束關系。

3.根據權利要求1所述的方法，其特征在于，所述將三維地圖中的地圖點投影至所述當前幀，確定所述當前幀中特征點與所述三維地圖中地圖點對應的第一視覺約束關系，包括：

獲取所述視覺慣導設備的當前跟蹤狀態；

根據所述當前跟蹤狀態，估計引導匹配窗口的大?。?/p>

將三維地圖中的地圖點投影至所述當前幀，得到所述地圖點對應的投影像素點；

將所述當前幀中所述投影像素點周圍符合所述大小的引導匹配窗口內的特征點與所述地圖點進行匹配，并根據匹配結果，確定所述當前幀中特征點與所述地圖點對應的第一視覺約束關系。

4.根據權利要求3所述的方法，其特征在于，所述當前跟蹤狀態包括跟蹤率及丟失時間，所述跟蹤率為所述當前幀跟蹤到所述三維地圖中地圖點的數目與所述上一幀跟蹤到所述三維地圖中地圖點的數目的比值。

5.根據權利要求1所述的方法，其特征在于，所述根據所述當前慣性數據，確定所述當前幀與所述上一幀之間的慣性約束關系，包括：

根據所述當前慣性數據，利用預積分策略，將所述當前慣性數據轉換為所述當前幀與所述上一幀之間的慣性約束關系。

6.根據權利要求1所述的方法，其特征在于，所述獲取所述上一幀之前的歷史幀對所述上一幀的先驗約束關系，包括：

獲取邊緣化時刻所述上一幀的狀態值、所述邊緣化時刻上一幀位姿的信息矩陣以及所述上一幀的實時狀態值；

根據所述邊緣化時刻所述上一幀的狀態值、所述邊緣化時刻上一幀位姿的信息矩陣以及所述上一幀的實時狀態值，確定所述上一幀之前的歷史幀對所述上一幀的先驗約束關系。