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

说明书

本发明公开了一种基于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为双目相机基线长度；