[發明專利]一種用于深度相機的位姿估計方法

有效

• 申請號: 201910769449.7
• 申請日: 2019-08-20
• 公開（公告）號: CN110503688B
• 公開（公告）日: 2022-07-22
• 發明（設計）人: 朱俊濤;陳強
• 申請（專利權）人: 上海工程技術大學
• 主分類號: G06T7/73
• 分類號: G06T7/73;G06T7/33
• 代理公司: 上海唯智贏專利代理事務所(普通合伙) 31293
• 代理人: 姜曉艷

1.一種用于深度相機的位姿估計方法，其特征在于包括以下步驟：

步驟一、將深度相機設置在移動機構上進行拍攝，獲得深度圖和RBG彩色圖，從拍攝的每相鄰兩幀圖像中提取Oriented FAST and Rotated BRIEF ORB特征點對；

步驟二、利用N個深度信息缺失的ORB特征點對，計算相機位姿變化的估計值ξ_P，利用M個深度信息完整的ORB特征點對，計算相機位姿變化的估計值ξ_Q，進而得到總估計值ξ₀；

步驟三、構建融合了深度信息缺失的ORB特征點對和深度信息完整的ORB特征點對對應的誤差信息的最小重投影誤差模型，進而得到對應的雅可比矩陣J；

步驟四、根據所述總估計值ξ₀、最小重投影誤差模型和雅可比矩陣J，利用非線性優化方法，計算優化后的總估計值ξ_k，從而完成相鄰兩幀圖像拍攝過程中的相機位姿變化的估計；

對于N個深度信息缺失的ORB特征點對，利用RANSAC算法計算估計值ξ_P；對于M個深度信息完整的ORB特征點對，利用直接線性變換法計算估計值ξ_Q，再利用如下方程式，計算總估計值ξ₀；

對于N個深度信息缺失的ORB特征點對，利用重投影方法，結合PnP算法，構建其對應的誤差函數e_P；對于M個深度信息完整的ORB特征點對，利用重投影方法，結合ICP算法，構建其對應的誤差函數e_Q；然后構建融合誤差函數e_P和誤差函數e_Q的最小重投影誤差模型e，進而得到對應的雅可比矩陣J；

所述雅可比矩陣J的計算方法包括以下步驟：

步驟Ⅰ、對于N個深度信息缺失的ORB特征點對，記相鄰兩幀圖像上匹配的ORB特征點對對應的空間點齊次坐標分別為P_i＝(x_i,y_i,z_i,1)^T，其相鄰幀上的ORB特征點經過世界坐標系和像素坐標系的轉換關系得到對應的像素坐標為

利用重投影方法，計算初始幀的ORB特征點P_i＝(x_i,y_i,z_i,1)^T在相鄰幀上投影的像素齊次坐標p_i＝(u_i,v_i,1)^T，其李代數關系如下，

其中，si表示初始幀的ORB特征點在相鄰幀上的投影對應的深度信息，K表示相機的內參矩陣，∧表示反對稱符號，

結合PnP算法，構建深度信息缺失的ORB特征點對的誤差函數e_P；

步驟Ⅱ、對于M個深度信息完整的ORB特征點對，記相鄰兩幀圖像上匹配的ORB特征點對對應的空間點坐標分別為Q_i＝(X_i,Y_i,Z_i)^T，利用重投影方法，計算初始幀的ORB特征點Q_i＝(X_i,Y_i,Z_i)^T在相鄰幀上投影的空間點坐標其李代數關系如下，

其中，∧表示反對稱符號，

結合ICP算法，構建深度信息完整的ORB特征點對的誤差函數e_Q；

步驟Ⅲ、構建最小重投影誤差模型e，如下所示；

其中，δξ表示擾動量，f_x表示相機的焦距在像素坐標系的x軸分量，f_y表示相機的焦距在像素坐標系的y軸分量，點P′_i＝(x′_i,y′_i,z′_i,1)^T表示初始幀中深度信息缺失的ORB特征點P_i＝(x_i,y_i,z_i,1)^T乘以相機位姿變化的總估計值ξ₀后對應的點，點Q_i′＝(X′_i,Y′_i,Z′_i)^T表示初始幀中深度信息完整的ORB特征點Q_i＝(X_i,Y_i,Z_i)^T乘以相機位姿變化的總估計值ξ₀后對應的點。