[發明專利]一種融合雙目相機與IMU的定位與三維線框結構重建方法與系統

有效

• 申請號: 201810917346.6
• 申請日: 2018-08-13
• 公開（公告）號: CN109166149B
• 公開（公告）日: 2021-04-02
• 發明（設計）人: 王星博;石瑞星
• 申請（專利權）人: 武漢大學
• 主分類號: G06T7/73
• 分類號: G06T7/73;G06T5/00;G01C21/16;G01C21/20
• 代理公司: 武漢科皓知識產權代理事務所(特殊普通合伙) 42222
• 代理人: 王琪

1.一種融合雙目相機與IMU的定位與三維線框結構重建方法，其特征在于，包括如下步驟：

步驟1，讀取慣性測量信息流和雙目視頻流數據，并根據相機的參數，對圖像進行幾何校正，消除左右視圖的畸變，使得行對準；同時在時間維度上對齊讀入的慣性測量數據與雙目圖像數據；

步驟2，在純雙目視覺模式下運行，提取特征點和關鍵幀中的LSD特征線段，進行左右目圖像特征匹配，并基于關鍵幀進行跟蹤定位與建圖；

步驟3，利用步驟2中估計的關鍵幀的位置與姿態，初始化慣性測量單元；

步驟4，完成步驟3的IMU初始化以后，重新進行特征提取與圖像匹配，而后聯合雙目視覺與慣性測量單元進行跟蹤定位與建圖；

步驟5，根據步驟4中的關鍵幀的位姿，更新步驟2中特征線段端點的坐標，得到初始的三維線框的位置；然后，遍歷三維線框中的所有線段，計算線段間距離和夾角，與角度閾值和距離閾值相比較，判斷線段是否表示同一條邊，將同一邊的線段聚為一個組，得到不同的線段的組；

步驟6，遍歷步驟5得到的線段的組的集合，分別使用各組內包含的所有線段擬合出各組所對應的邊；具體實現包括如下步驟，

步驟6.1，獲取每個組內所有線段的端點，基于M估計的方法進行直線迭代加權擬合，分為以下幾個子步驟：

步驟6.1.1，選取Huber’s M-estimators，對應的ρ函數為：

其中，r表示自變量；

步驟6.1.2，使用選取的ρ函數，構造參數求解模型F：

其中n為每組線段的總數，ρ為選擇的Distance function，a_i、L為觀測值，X為直線方程中的未知量，空間直線可以表示為：

待求參數為X₀、Y₀、Z₀、m、n，觀測值，即每條線段的端點坐標為X、Y、Z；

將空間直線轉化為矩陣形式可得：

帶入上面的求解模型：

n條線段得到2n個M-估計方程，通過模型殘差倒數加權最小二乘迭代擬合求解(X₀,Y₀,Z₀),(m,n,1)；

步驟6.1.3，解得直線通過的三維點坐標P:(X₀,Y₀,Z₀)以及直線的方向向量l_direction:(m,n,1)；

步驟6.2，在步驟6.1將同組線段進行直線擬合，確定直線的范圍，得到表示邊的線段，分為以下子步驟：

步驟6.2.1，遍歷同組內所有線段的端點，找到距離最遠的兩個點P1、P2；

步驟6.2.2，根據擬合直線的方向向量l_direction，提取P1、P2對應方向向量主方向的值，帶入直線方程中，解得表示邊框線段的兩個端點，從而確定線段的空間位置；

步驟7，所有組的線段擬合完成后，統計擬合結果數量N₂，設定擬合閾值N_delta，若|N-N₂|N_delta，N為線段總數，則說明所有線段擬合完畢，不存在表示同一條邊的線段組合；若|N-N₂|≥N_delta，N＝N₂，返回步驟5中重新計算線段間的距離；

步驟8，輸出三維線框結構。