[發明專利]一種基于EPnP的雙目相機位姿估計方法

在審

• 申請號: 202110532984.8
• 申請日: 2021-05-17
• 公開（公告）號: CN113506337A
• 公開（公告）日: 2021-10-15
• 發明（設計）人: 郭健;呂思聰;葉雅婷;李勝;惠玉卓
• 申請（專利權）人: 南京理工大學
• 主分類號: G06T7/593
• 分類號: G06T7/593;G06K9/62;G06K9/46
• 代理公司: 南京理工大學專利中心 32203
• 代理人: 薛云燕

本發明公開了一種基于EPnP的雙目相機位姿估計方法，該方法步驟如下：計算雙目相機的3D點在世界坐標系下的三維坐標和測量誤差；利用已知的3D點，基于主成分分析原理選擇世界坐標系內四個3D控制點；據此求解世界坐標系下每個3D點對應的hb坐標；求解四個3D控制點在相機坐標系下的坐標；根據控制點，將所有的3D點在相機坐標系下的坐標恢復出來；根據世界坐標系下每個3D點坐標和相機坐標系下的3D點坐標，計算相機坐標系相對世界坐標系的旋轉分量和平移分量，得到相機位姿。本發明增強了雙目相機下位姿估計的魯棒性，提高了相對位姿估計的準確性，實現了雙目相機系統下，高效、精準的相機位姿估計。

技術領域

本發明屬于機器人視覺技術領域，特別是一種基于EPnP的雙目相機位姿估計方法。

背景技術

在一個機器人視覺系統中，機器人實現自主導航定位系統的關鍵在于獲取相機在位姿參數。該問題可以通過求解n點透視問題(PnP)獲得。PnP能在已知多對3D世界坐標與2D像素坐標匹配點的情況下，求解相機坐標系相對世界坐標系的旋轉變量R和平移分量t。

長期以來，PnP算法及其改進算法在圖像測、計算機視覺、機器人學、AR等領域都獲得了研究關注。目前，大多數非迭代的PnP算法會首先求解特征點的深度，以獲得特征點在相機坐標系中的3D坐標，如P3P算法，但是此類算法在求解過程中存在無解和無窮多解的情況。

發明內容

本發明的目的在于提供一種魯棒性好、準確性高的基于EPnP的雙目相機位姿估計方法。

實現本發明目的的技術解決方案為：一種基于EPnP的雙目相機位姿估計方法，包括以下步驟：

步驟1、計算雙目相機的3D點在世界坐標系下的三維坐標和測量誤差；

步驟2、利用已知的3D點，基于主成分分析PCA原理選擇世界坐標系內四個3D 控制點C_wj，j＝1，...，4；

步驟3、根據步驟2得到的四個3D控制點，求解世界坐標系下每個3D點P_wi對應的hb坐標；

步驟4、求解四個3D控制點在相機坐標系下的坐標C_cj；

步驟5、根據控制點C_cj，將所有的3D點在相機坐標系下的坐標P_ci恢復出來；

步驟6、根據P_wi和P_ci，計算相機坐標系相對世界坐標系的旋轉分量R和平移分量t，得到相機位姿。

進一步地，步驟1中雙目相機的3D點是通過上一幀圖像F_k-1和當前幀圖像F_k的左相機進行特征提取和匹配獲得的，k為當前圖像編號，且k為大于等于2的自然數；

其中，當前幀圖像F_k中左相機中的n個點的相機坐標為p_ci＝[u_ci v_ci]^T，和上一幀對應的n個3D點在世界坐標系下的三維坐標為P_wi＝[X_wi Y_wi Z_wi]^T，i＝1...n，其中n為上一幀圖像F_k-1和當前幀圖像F_k的左相機進行特征提取和匹配獲得的3D點的數量。

進一步地，步驟1所述的世界坐標系下的三維坐標和測量誤差，假設左右相機坐標系的x軸在同一條直線上，根據相似三角形原理，計算3D點的深度d

其中Δx表示視差，x_cl、x_cr分別為3D點在左相機和右相機成像平面上的投影位置的橫坐標值，b為雙目相機基線長度；