[发明专利]一种基于EPnP的双目相机位姿估计方法

权利要求书

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点相机坐标三个维度的可靠度。