CN110555880B - 一种焦距未知的p6p相机位姿估计方法 - Google Patents

Abstract

本发明公开了一种焦距未知的P6P相机位姿估计方法，包括S1:读取n对2D‑3D关键点，选取其中6对，并根据所建立的旋转矩阵解算模型的模型特征对关键点的信息进行预处理；S2:将对应的3D坐标点均与第一个3D坐标点组成向量，并垂直投影到成像面；S3:引入垂直投影向量与相机投影模型之间的联系，建立含有旋转矩阵的方程组，解算旋转矩阵；S4:根据所求旋转矩阵，求解位移向量，得到相机坐标系与世界坐标系的相对关系。本发明能够估计出图像拍摄时的相机位姿。

Description

一种焦距未知的P6P相机位姿估计方法

技术领域

本发明涉及计算机视觉技术领域，尤其涉及一种焦距未知的P6P相机位姿估计方法。

背景技术

PnP求解方法是指通过多对3D与2D匹配点，在已知或者未知相机内参的情况下，求解被观察物体与相机之间相对位置变换的问题，是SLAM前端位姿跟踪部分中常用的方法之一。

1981年，PnP问题被首次提出。目前主要的PnP求解方法有P3P(Perspective-Three-Point，简称P3P)、EPnP(Efficient PnP，简称EPnP)、UPnP(Uncalibrated PnP，简称UPnP)和DLT(Direct Linear Transform,简称DLT)等方法。P3P方法指的是2003年由Gao等人提出了P3P问题的完整解算方案。2009年，Vincent Lepetit等人提出了高精度快速位姿估计方法EPnP。EPnP方法利用四个关键点对求得封闭解后，将该解作为非线性优化的初值，优化提高精度。2013年，Adrian等人提出了UPnP方法，该方法可在未知焦距的情况下实现相机位姿估计。P3P方法的解算需要已知相机内参才能求解。EPnP和UPnP方法是线性拟合方法，其精确性建立在关键点的数量之上，而在关键点数量较少时，无法给出精确估计。在DLT方法中，直接将传输矩阵看做了12个未知数，而忽略了它们之间的联系，因此求出的解不一定满足旋转矩阵的约束。在实际应用中，有时只能获取图片，并不能获得相机内参，例如对于偷拍图片，通过偷拍图片对拍摄位置进行估计可对案件的侦破提供线索。因此，针对关键点数量较少，未标定相机的位姿估计方法具有研究价值。

发明内容

有鉴于此，本发明的目的是提供一种焦距未知的P6P相机位姿估计方法，并可以在估计旋转矩阵的基础上，进一步估计焦距和位移矩阵，从而完成PnP问题的解算。

本发明通过以下技术手段解决上述技术问题：

一种焦距未知的P6P相机位姿估计方法，包括：

S1:读取n对2D-3D关键点，选取其中6对，并根据所建立的旋转矩阵解算模型的模型特征对关键点的信息进行预处理；

S2:将对应的3D坐标点均与第一个3D坐标点组成向量，并垂直投影到成像面；

S3:引入垂直投影向量与相机投影模型之间的联系，建立含有旋转矩阵的方程组，解算旋转矩阵；

S4:根据所求旋转矩阵，求解位移向量，得到世界坐标系与相机坐标系的相对关系。

进一步，所述关键点选取的方法为：

根据图像匹配结果，按照匹配度从大到小排列匹配点，选择前8个点作为备选点；

以图像原点为起点，与各备选点组成向量l_i。将夹角最大的两个向量分别选为l₁和l₂，优先选取与l₂向量夹角大于90度且与l₁夹角小于90度的点，或与l₂向量夹角小于90度且与l₁夹角大于90度的点作为其余四个关键点。

进一步，将点对投影到成像面，并根据在成像面的几何约束建立不含未知焦距的旋转矩阵解算模型。

进一步，所述n≥6。

本发明的有益效果：

本发明的相机位姿估计方法，在步骤S2中提到的将3D关键点垂直投影到成像面这一操作，由于相机焦距也与成像面垂直，可以将相机焦距对PnP问题的影响消除，进而建立了不需要进行相机标定，仅需图像信息，即像素物理尺寸和关键点的像素坐标，即可实现位姿估计的模型。在传统的相机位姿估计中，除了图像信息外，还需要获得相机内参信息才能求解，因此只能对已知相机内参的图片进行位姿估计，这种情况下，可供利用的图片有限。本发明中公开的相机位姿估计方法解决了这个问题，可以通过对未知来源的图像的分析，估计图像拍摄时相机的位姿。另外，传统的P3P方法建立在四个不共面的3D关键点上；本发明中公开的模型并不建立在此基础之上，但由于步骤S3中用到相似三角形定理，故需要满足在选点方案中提到的几何约束，这一几何约束可通过对图像信息的预处理和关键点选取策略有效规避。因此，在相机内参未知或无法获得相机内参的情况下，例如网络图片、通过黑客技术获取的非合作方相机拍摄的图片、偷拍图片等，该方法可以估计出图像拍摄时的相机位姿。

附图说明

图1为本发明实施例提供的相机位姿估计方法的流程图；

图2为本发明实施例提供的旋转矩阵解算模型；

图3为本发明实施例提供的2D坐标点与3D垂直投影点的比例关系图；

图4为本发明实施例提供的位移矩阵解算模型。

具体实施方式

以下将结合附图和具体实施例对本发明进行详细说明：

如图1所示，本发明的一种焦距未知的P6P相机位姿估计方法，包括：

S1:读取n对2D-3D关键点，选取其中6对，并根据所建立的旋转矩阵解算模型的模型特征对关键点的信息进行预处理；其中，

所述关键点选取的方法为：

根据图像匹配结果，按照匹配度从大到小排列匹配点，选择前8个点作为备选点；

以图像原点为起点，与各备选点组成向量l_i。将夹角最大的两个向量分别选为l₁和l₂，优先选取与l₂向量夹角大于90度且与l₁夹角小于90度的点，或与l₂向量夹角小于90度且与l₁夹角大于90度的点作为其余四个关键点。

S2:将对应的3D坐标点均与第一个3D坐标点组成向量，并垂直投影到成像面。

如图2所示，^WP_i是3D关键点，

是^WP_i在成像面的垂直投影点，则^CL_i,j是向量

在成像面的垂直投影，点^WP_i、

与^WP_j构成面Π_i,j，将面Π_i,j的法向量记为^Cn_i,j，则^Cn_i,j和^CL_i,j可分别表示为：

其中^Cn为成像面法向量，在相机坐标系下，^Cn＝[0,0,1]^T。R为旋转矩阵，t为位移向量，定义为R＝rot(X,α)·rot(Y,β)·rot(Z,θ)，^CP_i＝R·^WP_i+t，rot(X,α)表示绕X轴旋转α的旋转矩阵，同理，rot(Y,β)和rot(Z,θ)分别表示绕Y轴和Z轴的旋转矩阵。

S3:为了构建2D-3D坐标系之间的联系，引入垂直投影向量与相机投影模型之间的联系，建立含有旋转矩阵的方程组，解算旋转矩阵。

如图3中所示，O为图像原点，^CP_i为与^WP_i相对应的2D坐标点。伸缩

使

平行于^CL_1,i，则^CL_1,i可表示为：

^CL_1,i＝-μ^Cv_1,i (3)

^Cn_1,i ^T·^Cv_1,i＝0 (4)

根据相似三角形定理可知，存在比例关系：

因为对应向量方向相同，长度关系可转化为向量关系：

^Cv_1,2 ^T·^CL_1,i＝^Cv_1,i ^T·^CL_1,2 (5)

将式子(2)代入式子(5)，则λ_i可表示为：

其中，(x_i,y_i,0)表示

单位化之后的坐标，用(X_i,Y_i,Z_i)表示^WP_i的坐标，则A_1,i,B_1,i为：

进一步地，

和

均垂直于面Π_1,2，则λ₂可以表示为：

λ₂＝(y₁·A_1,2-x₁·B_1,2)/(y₂·A_1,2-x₂·B_1,2) (7)

将式子(1)、(6)、(7)代入(4)式，构建含有未知R的方程组：

方程组中包含三个未知旋转角度，首先将方程组未知数α视为变量，β和θ视为参数，可解出用β和θ表示的sinα和cosα的表达式；再将β视为变量，进一步利用这些表达式，解出含有θ的表达式：

其中

只含有未知量θ，由于cos²2β+sin²2β＝1，则有：

进一步地整理为：

k_i和l_i为系数，由关键点坐标变化而来。将等式(8)两边取平方，可整理为：

δ_i为与k_i、l_i有关的系数，其奇数项系数为0。至此，PnP问题转变为多项式求解问题。对方程求解可得θ，并依次解得β和α。

将旋转矩阵代入式子(2)，得^CL_1,2＝[A_1,2,B_1,2,0]^T,当选取的6对点中存在点k，^CP_k＝[x_k,y_k,0]^T，满足x_k·A_1,2+y_kB_1,2＝0，即点l_k垂直于^CL_1,2时，则从备选点中选一个与l_k不共线的点作为新点k，重复步骤S2，S3，解算旋转矩阵。

S4:根据所求旋转矩阵，求解位移向量[tx,ty,tz]^T，得到相机坐标系与世界坐标系的相对关系。

如图4所示，^CX、^CY为相机坐标系的坐标轴，^CO为相机中心，点^WP_i到平面^CX^CO^CY的距离用d_i表示，l_i表示垂直投影点到图像原点的距离，f表示焦距，R·^WP_i＝[Pxi,Pyi,Pzi]^T，则根据相似三角形可知：

其中d_i＝Pzi+tz，则有：

同时焦距f也可求出。利用焦距f，可将^CP_i更新为相机坐标系下的三维坐标。根据几何关系^Cn_i'^T·(R·^WP_i+t)＝0可求出tx,ty，其中，

以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。本发明未详细描述的技术、形状、构造部分均为公知技术。

Claims (3)

1.一种焦距未知的P6P相机位姿估计方法，其特征在于，包括：

S1:读取n对2D-3D关键点，选取其中6对，并根据所建立的旋转矩阵解算模型的模型特征对关键点的信息进行预处理；

S2:将对应的3D坐标点均与第一个3D坐标点组成向量，垂直投影到成像面，具体为：

^WP_i是3D关键点，

是^WP_i在成像面的垂直投影点，则^CL_i,j是向量

在成像面的垂直投影，点^WP_i、

与^WP_j构成面Π_i,j，将面Π_i,j的法向量记为^Cn_i,j，则^Cn_i,j和^CL_i,j可分别表示为：

其中^Cn为成像面法向量，在相机坐标系下，^Cn＝[0,0,1]^T； R为旋转矩阵，t为位移向量，定义为R＝rot(X,α)·rot(Y,β)·rot(Z,θ)，^CP_i＝R·^WP_i+t，rot(X,α)表示绕X轴旋转α的旋转矩阵，同理，rot(Y,β)和rot(Z,θ)分别表示绕Y轴和Z轴的旋转矩阵；

S3:引入垂直投影向量与相机投影模型之间的联系，建立含有旋转矩阵的方程组，解算旋转矩阵，具体为：

O为图像原点，^CP_i为与^WP_i相对应的2D坐标点，伸缩

使

平行于^CL_1,i，则^CL_1,i可表示为：

^CL_1,i＝-μ^Cv_1,i (3)

^Cn_1,i ^T·^Cv_1,i＝0 (4)

根据相似三角形定理可知，存在比例关系：

因为对应向量方向相同，长度关系可转化为向量关系：

^Cv_1,2 ^T·^CL_1,i＝^Cv_1,i ^T·^CL_1,2 (5)

将式子(2)代入式子(5)，则λ_i可表示为：

其中，(x_i,y_i,0)表示

单位化之后的坐标，用(X_i,Y_i,Z_i)表示^WP_i的坐标，则A_1,i,B_1,i为：

进一步地，

和

均垂直于面Π_1,2，则λ₂可以表示为：

λ₂＝(y₁·A_1,2-x₁·B_1,2)/(y₂·A_1,2-x₂·B_1,2) (7)

将式子(1)、(6)、(7)代入(4)式，构建含有未知R的方程组：

方程组中包含三个未知旋转角度，首先将方程组未知数α视为变量，β和θ视为参数，可解出用β和θ表示的sinα和cosα的表达式；再将β视为变量，进一步利用这些表达式，解出含有θ的表达式：

其中

只含有未知量θ，由于cos²2β+sin²2β＝1，则有：

进一步地整理为：

k_i和l_i为系数，由关键点坐标变化而来；将等式(8)两边取平方，可整理为：

δ_i为与k_i、l_i有关的系数，其奇数项系数为0；至此，PnP问题转变为多项式求解问题，对方程求解可得θ，并依次解得β和α；

将旋转矩阵代入式子(2)，得^CL_1,2＝[A_1,2,B_1,2,0]^T,当选取的6对点中存在点k，^CP_k＝[x_k,y_k,0]^T，满足x_k·A_1,2+y_kB_1,2＝0，即点l_k垂直于^CL_1,2时，则从备选点中选一个与l_k不共线的点作为新点k，重复步骤S2，S3，解算旋转矩阵；

S4:根据所求旋转矩阵，求解位移向量，得到世界坐标系与相机坐标系的相对关系。

2.根据权利要求1所述的一种焦距未知的P6P相机位姿估计方法，其特征在于：所述关键点选取的方法为：

根据图像匹配结果，按照匹配度从大到小排列匹配点，选择前8个点作为备选点；

以图像原点为起点，与各备选点组成向量l_i，将夹角最大的两个向量分别选为l₁和l₂，优先选取与l₂向量夹角大于90度且与l₁夹角小于90度的点，或与l₂向量夹角小于90度且与l₁夹角大于90度的点作为其余四个关键点。

3.根据权利要求1所述的一种焦距未知的P6P相机位姿估计方法，其特征在于：所述n≥6。

Images