一种基于EPnP的双目相机位姿估计方法
阅读说明：本技术 一种基于EPnP的双目相机位姿估计方法 (Binocular camera pose estimation method based on EPnP ) 是由 郭健 吕思聪 叶雅婷 李胜 惠玉卓 于 2021-05-17 设计创作，主要内容包括：本发明公开了一种基于EPnP的双目相机位姿估计方法,该方法步骤如下：计算双目相机的3D点在世界坐标系下的三维坐标和测量误差；利用已知的3D点,基于主成分分析原理选择世界坐标系内四个3D控制点；据此求解世界坐标系下每个3D点对应的hb坐标；求解四个3D控制点在相机坐标系下的坐标；根据控制点,将所有的3D点在相机坐标系下的坐标恢复出来；根据世界坐标系下每个3D点坐标和相机坐标系下的3D点坐标,计算相机坐标系相对世界坐标系的旋转分量和平移分量,得到相机位姿。本发明增强了双目相机下位姿估计的鲁棒性,提高了相对位姿估计的准确性,实现了双目相机系统下,高效、精准的相机位姿估计。(The invention discloses a binocular camera pose estimation method based on EPnP, which comprises the following steps: calculating three-dimensional coordinates and measurement errors of the 3D point of the binocular camera in a world coordinate system; selecting four 3D control points in a world coordinate system based on a principal component analysis principle by using known 3D points; solving hb coordinates corresponding to each 3D point in the world coordinate system; solving coordinates of the four 3D control points in a camera coordinate system; recovering coordinates of all the 3D points in a camera coordinate system according to the control points; and calculating a rotation component and a translation component of the camera coordinate system relative to the world coordinate system according to the coordinates of each 3D point in the world coordinate system and the coordinates of the 3D points in the camera coordinate system to obtain the camera pose. The invention enhances the robustness of the estimation of the lower pose of the binocular camera, improves the accuracy of the estimation of the relative pose, and realizes the high-efficiency and accurate estimation of the pose of the camera under a binocular camera system.)
技术领域
本发明属于机器人视觉技术领域，特别是一种基于EPnP的双目相机位姿估计方法。
背景技术
在一个机器人视觉系统中，机器人实现自主导航定位系统的关键在于获取相机在位姿参数。该问题可以通过求解n点透视问题(PnP)获得。PnP能在已知多对3D世界坐标与2D像素坐标匹配点的情况下，求解相机坐标系相对世界坐标系的旋转变量R和平移分量t。
长期以来，PnP算法及其改进算法在图像测、计算机视觉、机器人学、AR等领域都获得了研究关注。目前，大多数非迭代的PnP算法会首先求解特征点的深度，以获得特征点在相机坐标系中的3D坐标，如P3P算法，但是此类算法在求解过程中存在无解和无穷多解的情况。
发明内容
本发明的目的在于提供一种鲁棒性好、准确性高的基于EPnP的双目相机位姿估计方法。
实现本发明目的的技术解决方案为：一种基于EPnP的双目相机位姿估计方法，包括以下步骤：
步骤1、计算双目相机的3D点在世界坐标系下的三维坐标和测量误差；
步骤2、利用已知的3D点，基于主成分分析PCA原理选择世界坐标系内四个3D 控制点Cwj，j＝1，...，4；
步骤3、根据步骤2得到的四个3D控制点，求解世界坐标系下每个3D点Pwi对应的hb坐标；
步骤4、求解四个3D控制点在相机坐标系下的坐标Ccj；
步骤5、根据控制点Ccj，将所有的3D点在相机坐标系下的坐标Pci恢复出来；
步骤6、根据Pwi和Pci，计算相机坐标系相对世界坐标系的旋转分量R和平移分量t，得到相机位姿。
进一步地，步骤1中双目相机的3D点是通过上一帧图像Fk-1和当前帧图像Fk的左相机进行特征提取和匹配获得的，k为当前图像编号，且k为大于等于2的自然数；
其中，当前帧图像Fk中左相机中的n个点的相机坐标为pci＝[uci vci]T，和上一帧对应的n个3D点在世界坐标系下的三维坐标为Pwi＝[Xwi Ywi Zwi]T，i＝1...n，其中n为上一帧图像Fk-1和当前帧图像Fk的左相机进行特征提取和匹配获得的3D点的数量。
进一步地，步骤1所述的世界坐标系下的三维坐标和测量误差，假设左右相机坐标系的x轴在同一条直线上，根据相似三角形原理，计算3D点的深度d
其中Δx表示视差，xcl、xcr分别为3D点在左相机和右相机成像平面上的投影位置的横坐标值，b为双目相机基线长度；
当使用视差法计算3D点三维坐标，由像素化误差导致视差产生误差，则带有误差的深度为
其中，为带有误差的视差，δΔx为视差误差，δd为深度误差；
继而推得：
其中，δxcl、δxcr分别为左右两相机的数字图像像素化误差；δΔx远小于Δx，则δd和δx同分布，δd的期望和Δx成反比；
在实际情况中，3D点在左相机和右相机成像平面上的投影位置的纵坐标值ycl、ycr不同；因此，能够得到带有误差的3D点相机坐标系坐标
其中，P、分别为3D点相机坐标系在相机坐标系下的坐标真值和测量值，δPc为观测误差，pcl为左相机坐标中3D点P在左相机平面的投影坐标；
因此，双目相机对3D点的观测误差δPc为：
假设|δxcl|≤δx，max，|δycl|≤δy，max，得到
其中δx，max和δy，max是pc在x轴和y轴的最大误差，取δx，max＝0.5/fx，δy，max＝0.5/ fy；δXc、δYc、δZc是3D点对应的Pc在x轴、y轴、z轴的误差；δXc，max、δYc，max、δZc，max是3D点对应的Pc在x轴、y轴、z轴的最大误差；
定义权重向量Kδc＝[kδc1 kδc1 kδc1]T＝[1/δXc，max 1/δYc，max 1/δZc，max]T，Kδc用来描述对应3D点相机坐标三个维度的可靠度。
进一步地，步骤2中基于主成分分析PCA原理选择控制点的过程如下：
步骤2.1、设置控制点Cw1，第一个控制点Cw1选择在n个3D点的重心位
其中，Kδwi≈RwKδc为Pwi对应的权重向量，Rw为世界坐标系下相机的位姿的旋转分量，Xw1、Yw1、Zw1分别为加权和的X、Y、Z轴分量；
步骤2.2、将Pwi点集进行中心化处理，得到A
步骤2.3、计算其余控制点：计算ATA的3个特征点λ1、λ2、λ3，对应特征向量是v1、 v2、v3，则其余的控制点为
其中，Cwj，j＝2，...，4为求解得到的其余控制点，n＝4。
进一步地，步骤3中求解世界坐标系下每个3D点Pwi对应的hb坐标，对于世界坐标系内四个3D控制点Cwj，j＝1...n，使得所有的Pwi都能够用Cwj线性表示，即：
其中称aij为hb坐标，求解每个Pwi对应的hb坐标aij，j＝1...4。
进一步地，步骤3中四个3D控制点Cwj，j＝1...n，当相机位姿为四个3D控制点Cwj在相机坐标系下的坐标Ccj＝RT·(Cwj-t)，3D点在相机坐标系下的三维坐标Pci为
其中，R、t分别为相机位姿T的旋转分量和水平分量，aij为每个Pwi对应的hb坐标。
进一步地，步骤4中求解控制点在相机坐标系下的坐标Ccj，具体步骤如下：
步骤4.1、因为已知所有3D点匹配的相机平面的pci＝[uci vci]T，i＝1...n，则列出一组线性方程组
将上式方程组概括为
Mx＝0
其中，M为齐次方程组的系数矩阵，M的大小为2n×12；
方程组的解为
其中vi为M的0奇异值对应的右奇异向量，βi为vi对于的系数；x＝ [Cc1 Cc2 Cc3 Cc4]T，包含4个控制点在相机坐标系中的三维坐标，共12维；N表示前N个零奇异值，在此取1≤N≤3；
步骤4.2、若相机坐标系中的控制点坐标和世界坐标系中的控制点坐标一一对应，则两两的距离一定相等，于是有
||Cwa-Cwb||2＝||Cca-Ccb||2
其中，||Cwa-Cwb||表示第a和第b个控制点在世界坐标系下的距离，||Cca-Ccb|| 表示第a和第b个控制点在空间坐标系下的距离，a，b∈[1∶4]且a≠b；分别取N为1 或2或3：
当N＝1，x＝βv，转化为||βva-βvb||2＝||Cca-Ccb||2，则
当N＝2，x＝β1v1+β2v2，转化为
||β1(v1a-v1b)+β2(v2a-v2b)||2＝||Cca-Ccb||2
通过解该方程组来求出β1和β2；其中，设s1＝v1a-v1b，s2＝v2a-v2b，c＝||Cca- Ccb||2；
N＝3时β1、β2和β3的解法同N＝2；
步骤4.3、以步骤4.2求出的N为1、2和3时β的近似解为初值，通过高斯牛顿法，求解β值。
进一步地，步骤5中根据控制点Ccj，将所有的3D点在相机坐标系下的坐标Pci恢复出来，使用步骤3中求解出的hb坐标，计算出Pci：
其中，aij为每个Pwi对应的hb坐标，Ccj为四个3D控制点Cwj在相机坐标系下的坐标。
进一步地，步骤6中根据Pwi和Pci，计算R和t，具体步骤如下：
步骤6.1、对所有Pwi和Pci进行去中心化得到Qw、Qc：
其中
步骤6.2、对做奇异值分解，得到
WTW＝U∑VT
R＝UVT
其中，U的列向量是矩阵WTW的特征向量，V的列向量是矩阵WWT的特征向量；
步骤6.3、重复步骤6.1、6.2，分别对N为1、2和3三种情况计算R和t；用重投影误差来评估R和t，取误差最小的R和t作为最终的结果。
本发明与现有技术相比，其显著优点为：
(1)针对双目相机在测量过程中误差的不确定性，提出了针对双目相机测量模型的改进EPnP算法，能够在已知前后两帧双目图像、上一帧通过双目相机三角测量获得的3D点坐标以及该3D点在当前帧图像上的投影坐标，计算出当前帧相对上一帧的相对位姿，提高了EPnP算法在双目相机系统的鲁棒性；
(2)在EPnP算法的基础上，引入了误差模型，将世界坐标系中的3D坐标表示为一组虚拟的控制点的加权和，由于四个控制点不能共面，故而可以求解出唯一的相机坐标系下的坐标，提高了位姿估计结果的准确性；
(3)能够快速计算出前后两帧图像之间相机的位姿变换，实现了双目相机系统下，高效、精准的相机位姿估计，为机器人自主定位导航技术提供了有力保障；
附图说明
图1为本发明基于EPnP的双目相机位姿估计方法的流程图。
图2为本发明中的双目视差模型示意图。
具体实施方式
如图1、2所示，一种基于EPnP的双目相机位姿估计方法，包括如下步骤：
步骤1、计算双目相机的3D点在世界坐标系下的三维坐标和测量误差；
所述双目相机的3D点是通过上一帧图像Fk-1和当前帧图像Fk的左相机进行特征提取和匹配获得的，k为当前图像编号，且k为大于等于2的自然数；其中，当前帧图像 Fk中左相机中的n个点的相机坐标为pci＝[uci vci]T，和上一帧对应的n个3D点在世界坐标系下的三维坐标为Pwi＝[Xwi Ywi Zwi]T，i＝1...n，其中n为上一帧图像Fk-1和当前帧图像Fk的左相机进行特征提取和匹配获得的3D点的数量。
所述的世界坐标系下的三维坐标和测量误差，假设左右相机坐标系的x轴在同一条直线上，根据相似三角形原理，计算3D点的深度
其中Δx表示视差，xcl、xcr分别为3D点在左相机和右相机成像平面上的投影位置的横坐标值，b为双目相机基线长度；
当使用视差法计算3D点三维坐标，由像素化误差导致视差产生误差，则带有误差的深度为
其中，为带有误差的视差，δΔx为视差误差，δd为深度误差；
继而推得：
其中，δxcl、δxcr分别为左右两相机的数字图像像素化误差；δΔx远小于Δx，则δd和δx同分布，δd的期望和Δx成反比；
在实际情况中，3D点在左相机和右相机成像平面上的投影位置的纵坐标值ycl、ycr不同；因此，能够得到带有误差的3D点相机坐标系坐标
其中，P、分别为3D点相机坐标系在相机坐标系下的坐标真值和测量值，δPc为观测误差，pcl为左相机坐标中3D点P在左相机平面的投影坐标；
因此，双目相机对3D点的观测误差δPc为：
假设|δxcl|≤δx，max，|δycl|≤δy，max，得到
其中δx，max和δy，max是pc在x轴和y轴的最大误差，取δx，max＝0.5/fx，δy，max＝0.5/ fy；δXc、δYc、δZc是3D点对应的Pc在x轴、y轴、z轴的误差；δXc，max、δYc，max、δZc，max是3D点对应的Pc在x轴、y轴、z轴的最大误差；
通过分析δPc可知每个3D点坐标的误差不同，这样不利于求解相机位姿。为解决这一问题，结合以上分析，定义权重向量Kδc＝[kδc1 kδc1 kδc1]T＝ [1/δXc，max 1/δYc，max1/δZc，max]T，Kδc用来描述对应3D点相机坐标三个维度的可靠度。
步骤2、利用已知的3D点，基于主成分分析PCA原理选择世界坐标系内四个3D 控制点Cwj，j＝1，...，4，具体如下：
步骤2.1、设置控制点Cw1，第一个控制点Cw1选择在n个3D点的重心位
其中，Kδwi≈RwKδc为Pwi对应的权重向量，Rw为世界坐标系下相机的位姿的旋转分量，Xw1、Yw1、Zw1分别为加权和的X、Y、Z轴分量；
步骤2.2、将Pwi点集进行中心化处理，得到A
步骤2.3、计算其余控制点：计算ATA的3个特征点λ1、λ2、λ3，对应特征向量是v1、 v2、v3，则其余的控制点为
其中，Cwj，j＝2，...，4为求解得到的其余控制点，n＝4。
步骤3、根据步骤2得到的四个3D控制点，求解世界坐标系下每个3D点Pwi对应的hb坐标。
步骤3中求解世界坐标系下每个3D点Pwi对应的hb坐标，对于世界坐标系内四个 3D控制点Cwj，j＝1...n，使得所有的Pwi都能够用Cwj线性表示，即：
其中称aij为hb坐标(homogeneous barycentric coordinates)，代入上式，获得每个Pwi对应的aij，j＝1...4；
优选地，四个3D控制点Cwj，j＝1...n，当相机位姿为四个3D控制点Cwj在相机坐标系下的坐标Ccj＝RT·(Cwj-t)，3D点在相机坐标系下的三维坐标Pci为
其中，R、t分别为相机位姿T的旋转分量和水平分量，aij为每个Pwi对应的hb坐标。
步骤4、求解四个3D控制点在相机坐标系下的坐标Ccj，具体步骤如下：
步骤4.1、因为已知所有3D点匹配的相机平面的pci＝[uci vci]T，i＝1...n，则列出一组线性方程组
将上式方程组概括为
Mx＝0
其中，M为齐次方程组的系数矩阵，M的大小为2n×12；
方程组的解为
其中vi为M的0奇异值对应的右奇异向量，βi为vi对于的系数；x＝ [Cc1 Cc2 Cc3 Cc4]T，包含4个控制点在相机坐标系中的三维坐标，共12维；N表示前N个零奇异值，在此取1≤N≤3；
步骤4.2、若相机坐标系中的控制点坐标和世界坐标系中的控制点坐标一一对应，则两两的距离一定相等，于是有
||Cwa-Cwb||2＝||Cca-Ccb||2
其中，||Cwa-Cwb||表示第a和第b个控制点在世界坐标系下的距离，||Cca-Ccb|| 表示第a和第b个控制点在空间坐标系下的距离，a，b∈[1∶4]且a≠b；分别取N为1 或2或3：
当N＝1，x＝βv，转化为||βva-βvb||2＝||Cca-Ccb||2，则
当N＝2，x＝β1v1+β2v2，转化为
||β1(v1a-v1b)+β2(v2a-v2b)||2＝||Cca-Ccb||2
通过解该方程组来求出β1和β2；其中，设s1＝v1a-v1b，s2＝v2a-v2b，c＝||Cca- Ccb||2；
N＝3时β1、β2和β3的解法同N＝2；
步骤4.3、以步骤4.2求出的N为1、2和3时β的近似解为初值，通过高斯牛顿法，求解β值。
步骤5、根据控制点Ccj，将所有的3D点在相机坐标系下的坐标Pci恢复出来。
假设当相机位姿为在相机坐标系内有Ccj＝RT·(Cwj-t)，3D点在相机坐标系下的三维坐标为
因此世界坐标系下的hb坐标与其他相机坐标系下的hb坐标相同。使用步骤3中求解出的hb坐标，计算出Pci：
其中，aij为每个Pwi对应的hb坐标，Ccj为四个3D控制点Cwj在相机坐标系下的坐标。
步骤6、根据Pwi和Pci，计算相机坐标系相对世界坐标系的旋转分量R和平移分量t，得到相机位姿，具体步骤如下：
步骤6.1、对所有Pwi和Pci进行去中心化得到Qw、Qc：
其中
步骤6.2、对做奇异值分解，得到
WTW＝U∑VT
R＝UVT
其中，U的列向量是矩阵WTW的特征向量，V的列向量是矩阵WWT的特征向量；
步骤6.3、重复步骤6.1、6.2，分别对N为1、2和3三种情况计算R和t。用重投影误差来评估R和t，取误差最小的R和t作为改进EPnP的结果。
综上所述，本发明针对双目相机在测量过程中误差的不确定性，提出了针对双目相机测量模型的改进EPnP算法，提高了EPnP算法在双目相机系统的鲁棒性；在EPnP 算法的基础上，引入了误差模型，提高了位姿估计的准确性；能够快速计算出前后两帧图像之间相机的位姿变换，为机器人自主定位导航技术提供有力保障。