一种基于EPnP的双目相机位姿估计方法技术

本发明专利技术公开了一种基于EPnP的双目相机位姿估计方法，该方法步骤如下：计算双目相机的3D点在世界坐标系下的三维坐标和测量误差；利用已知的3D点，基于主成分分析原理选择世界坐标系内四个3D控制点；据此求解世界坐标系下每个3D点对应的hb坐标；求解四个3D控制点在相机坐标系下的坐标；根据控制点，将所有的3D点在相机坐标系下的坐标恢复出来；根据世界坐标系下每个3D点坐标和相机坐标系下的3D点坐标，计算相机坐标系相对世界坐标系的旋转分量和平移分量，得到相机位姿。本发明专利技术增强了双目相机下位姿估计的鲁棒性，提高了相对位姿估计的准确性，实现了双目相机系统下，高效、精准的相机位姿估计。位姿估计。位姿估计。

【技术实现步骤摘要】
一种基于EPnP的双目相机位姿估计方法


[0001]本专利技术属于机器人视觉
，特别是一种基于EPnP的双目相机位姿估计方法。

技术介绍

[0002]在一个机器人视觉系统中，机器人实现自主导航定位系统的关键在于获取相机在位姿参数。该问题可以通过求解n点透视问题(PnP)获得。PnP能在已知多对3D世界坐标与2D像素坐标匹配点的情况下，求解相机坐标系相对世界坐标系的旋转变量R和平移分量t。
[0003]长期以来，PnP算法及其改进算法在图像测、计算机视觉、机器人学、AR等领域都获得了研究关注。目前，大多数非迭代的PnP算法会首先求解特征点的深度，以获得特征点在相机坐标系中的3D坐标，如P3P算法，但是此类算法在求解过程中存在无解和无穷多解的情况。

技术实现思路

[0004]本专利技术的目的在于提供一种鲁棒性好、准确性高的基于EPnP的双目相机位姿估计方法。
[0005]实现本专利技术目的的技术解决方案为：一种基于EPnP的双目相机位姿估计方法，包括以下步骤：
[0006]步骤1、计算双目相机的3D点在本文档来自技高网...

【技术保护点】

【技术特征摘要】
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点相机坐标三个维度的可靠度。4.根据权利要求3所述的基于EPnP的双目相机位姿估计方法，其特征在于，步骤2中基于主成分分析PCA原理选择控制点的过程如下：步骤2.1、设置控制点C
w1
，第一个控制点C
w1
选择在n个3D点的重心位选择在n个3D点的重心位其中，K
δwi
≈R
w
K
δc
为P
wi
对应的权重向量，R
w
为世界坐标系下相机的位姿的旋转分量，X
w1
、Y
w1
、Z
w1
分别为加权和的X、Y、Z轴分量；
步骤2.2、将P
wi
点集进行中心化处理，得到A步骤2.3、计算其余控制点：计算A
T
A的3个特征点λ1、λ2、λ3，对应特征向量是v1、v2、v3，则其余的控制点为其中，C
wj
，j...

【专利技术属性】
技术研发人员：郭健，吕思聪，叶雅婷，李胜，惠玉卓，
申请(专利权)人：南京理工大学，
类型：发明
国别省市：