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

在審

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

1.一種基于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，得到相機位姿。

2.根據權利要求1所述的基于EPnP的雙目相機位姿估計方法，其特征在于，步驟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點的數量。

3.根據權利要求2所述的基于EPnP的雙目相機位姿估計方法，其特征在于，步驟1所述的世界坐標系下的三維坐標和測量誤差，假設左右相機坐標系的x軸在同一條直線上，根據相似三角形原理，計算3D點的深度d

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

當使用視差法計算3D點三維坐標，由像素化誤差導致視差產生誤差，則帶有誤差的深度為

其中，為帶有誤差的視差，δΔx為視差誤差，δd為深度誤差；

繼而推得：

其中，δx_cl、δx_cr分別為左右兩相機的數字圖像像素化誤差；δΔx遠小于Δx，則δd和δx同分布，δd的期望和Δx成反比；

在實際情況中，3D點在左相機和右相機成像平面上的投影位置的縱坐標值y_cl、y_cr不同；因此，能夠得到帶有誤差的3D點相機坐標系坐標

其中，P、分別為3D點相機坐標系在相機坐標系下的坐標真值和測量值，δP_c為觀測誤差，p_cl為左相機坐標中3D點P在左相機平面的投影坐標；

因此，雙目相機對3D點的觀測誤差δP_c為：

假設|δx_cl|≤δ_x，max，|δy_cl|≤δ_y,max，得到

其中δ_x,max和δ_y,max是p_c在x軸和y軸的最大誤差，取δ_x,max＝0.5/f_x，δ_y,max＝0.5/f_y；δX_c、δY_c、δZ_c是3D點對應的P_c在x軸、y軸、z軸的誤差；δX_c，max、δY_c,max、δZ_c,max是3D點對應的P_c在x軸、y軸、z軸的最大誤差；

定義權重向量K_δc＝[k_δc1 k_δc1 k_δc1]^T＝[1/δX_c，max 1/δY_c，max 1/δZ_c，max]^T，K_δc用來描述對應3D點相機坐標三個維度的可靠度。