CN105956074A - 邻近位姿融合引导的单幅图像场景六自由度定位方法 - Google Patents

Abstract

本发明公开了邻近位姿融合引导的单幅图像场景六自由度定位方法，首先从已标记内参和位置信息的参考图像库中检索出输入图像的一组邻近图像集；然后估计出输入图像与每一个邻近图像之间的相对位姿，并结合邻近图像的位姿，得到输入图像的一组候选位姿集；最后通过融合输入图像的候选位姿集，得到输入图像的六自由度位姿。为此，在估计输入图像与相似图像之间的相对位姿时，本发明提出了一种基于奇异值分解的快速估计算法。在融合候选位姿集时，定义了一个有效的几何误差函数，通过最小化误差函数值，得到输入图像的位置信息，其姿态信息由候选位姿的姿态信息平均计算得到。

Description

邻近位姿融合引导的单幅图像场景六自由度定位方法

技术领域

本发明涉及计算机视觉、计算机图像处理、增强现实等领域，尤其涉及邻近位姿融合引导的单幅图像场景六自由度定位方法。

背景技术

图像的位姿信息对于很多基于位置的服务非常关键，例如旅行路线推荐、图像导览、基于位置的图像检索、增强现实场景生成、自动驾驶等。图像的六自由度位姿信息包括三个自由度的位置信息和三个自由度的姿态信息。标注图像的六自由度位姿信息是非常耗时和困难的，因此研究图像六自由度位姿信息的自动计算方法具有非常重要的意义。

针对图像六自由度位姿信息的计算，现有的文献中提出了很多方法。一些方法利用了场景的三维点云模型，场景的三维点云模型往往通过在场景的大量图像上应用运动恢复结构算法重建得到。利用场景三维点云模型，这些方法将图像定位问题建模为二维图像到三维场景间的注册问题。从而通过查找输入图像上特征点与三维点云模型之间的匹配点对，并应用直接线性变换算法(direct linear transformation algorithm，简称DLT)估计出输入图像的位姿信息。在此方法流程下，这些方法主要目的在于找到尽量多的稳定的匹配点对。受益于预先重建好的三维点云模型，这些方法常常可以得到较好的结果。但是如果场景三维点云模型不存在的话，这些方法将无法发挥作用。同时重建场景的三维点云模型也是一个很耗时的过程，因而无法及时使用最新的含有位姿信息的图片数据，方法的灵活性不够。

除了基于三维点云模型的工作，另外一些方法通过识别输入图像中的地标，并将地标的位置信息迁移到输入图像上，从而定位输入图像。或者检索出输入图像的一组邻近图像，并综合考虑这组邻近图像的位置信息，得到输入图像的最终位置信息。受益于识别方法或者检索方法可以处理大规模的数据，这种方法通常也可以处理大规模数据，但是这种方法只能计算出输入图像的位置信息，很难计算出输入图像的六自由度信息。

发明内容

本发明技术解决问题：针对上述技术存在的问题，提供邻近位姿融合引导的单幅图像场景六自由度定位方法，该方法直接使用包含有位姿信息的图像数据，不需要重建场景的三维点云模型，因而具有很好的灵活性。

本发明技术解决方案：邻近位姿融合引导的单幅图像场景六自由度定位方法，对于一幅输入图像I，应用基于内容的相似图像检索方法从参考图像集中检索出一组输入图像的邻近图像集。其中参考图像集中的每张图像均标注了六自由度的位姿信息，并且已经标定了相机内参。然后，通过已标定的参考图像标定，估计出输入图像I与每个邻近图像N之间的相对位姿，结合邻近图像的位姿，得到输入图像的一组候选位姿集。最后，定义了一个几何误差函数，通过最小化该函数，融合输入图像的候选位姿集，得到输入图像的最终位置信息。输入图像的位置确定后，其姿态信息由两个候选姿态信息平均计算得到，从而完成输入图像的六自由度定位。

本发明主要通过以下技术方案实现，如图1所示：对于一幅输入图像，首先应用视觉词袋检索算法从参考图像集中检索出一组输入图像的邻近图像集。具体来说，提取参考图像集中每个图像上的尺度变换不变特征作为局部特征，并应用k均值聚类算法计算得到视觉单词树，且建立的视觉单词树包含20000～40000个视觉描述符。利用所得到的视觉单词树，计算输入图像和参考图像集中每个图像的视觉描述文本，即每个图像的所有局部特征对于视觉单词树的每个视觉单词的统计直方图。通过计算输入图像与参考图像集中每个图像的视觉描述文本的差异，选取差异最小的15～30个参考图像作为输入图像的邻近图像。然后，估计输入图像I与每个邻近图像N之间的相对位姿，结合邻近图像的位姿，得到输入图像的候选位姿。具体来说：a)提取输入图像与邻近图像中的所有尺度变换不变特征，并寻找输入图像上的特征点与邻近图像特征点之间的匹配，从而得到一组匹配点对。b)在匹配点对中随机选取11对特征点对，使用这些特征点对，建立线性方程组，通过奇异值分解算法求解线性方程组，得到输入图像与邻近图像之间的基础矩阵。c)从得到的基础矩阵中估计出输入图像的焦距，从而得到输入图像与邻近图像之间的本征矩阵。d)使用5点算法从得到的本征矩阵中估计出输入图像与邻近图像的相对位姿，结合邻近图像的位姿，从而得到输入图像的一个候选位姿。e)重复b)到d)的过程200次，选取最好的相对位姿计算结果，选取准则为：在每个得到的相对位姿基础上，计算输入图像与邻近图像满足外极几何约束的特征匹配点对个数，个数最多的作为最终的结果。在输入图像与每个邻近图像的相对位姿基础上，比较满足外极几何约束的匹配点对个数，只选择匹配点对个数最多的两个作为输入图像的候选位姿。最后，通过定义并最小化一个几何误差，得到输入图像的最终位姿，几何误差的定义方法是：由于候选位姿只是输入图像与邻近图像的相对位姿，因此一个候选位姿只能决定输入图像的位置应该在一条直线上，多个候选位姿对应多条直线，所定义的几何误差即为输入图像位置到直线的距离平方之和；同时由于输入图像与邻近图像的位置较为接近，为了方法的稳定，增加了输入图像位置到邻近图像位置的距离平方和作为正则项。输入图像的位置确定后，其姿态信息由两个候选位姿的姿态信息平均计算得到，从而实现输入图像的六自由度定位。

本发明与现有技术相比的优点在于：本发明能够直接利用含有位姿信息的图片定位输入图像，不需要重建场景的三维点云模型，具有良好的灵活性，并且所定义的几何误差函数能够有效的从一组候选位姿集中得到输入图像的最终位姿信息，从而实现单幅图像场景的六自由度定位。

附图说明

图1为本发明的总体流程示意图；

图2为输入图像的邻近图像检索结果示例图；

图3为通过多个融合候选位姿得到输入图像最终位姿的方法示意图；

图4为部分定位结果的在三维空间中的可视化结果。

具体实施方式

下面结合附图对本发明做进一步的详细说明，以令本领域技术人员参照说明书文字能够据以实施。

本发明假设已经存在一个标记好内参和六自由度位姿信息的参考图像集，对于输入图像，在参考图像集的基础上，计算其六自由度位姿信息。同时，为了能够提高方法的适用性，假设输入图像的内参未标定，且假设输入图像的内参包括焦距f和一阶径向畸变λ两个参数。

在上述条件下，如图1所示，对于一幅输入图像，(1)首先使用视觉词袋检索算法从参考图像集中检索出一组输入图像的邻近图像，检索方法的具体过程包括：

(11)提取参考图像集中每个图像上的尺度变换不变特征作为局部特征，并应用k均值聚类算法计算得到视觉单词树，且建立的视觉单词树包含30000个视觉描述符。

(12)利用所得到的视觉单词树，计算输入图像和参考图像集中每个图像的视觉描述文本，即每个图像的所有局部特征关于视觉单词树中每个视觉单词的统计直方图。

(13)通过计算输入图像与参考图像集中每个图像的视觉描述文本的差异，选取差异最小的20个参考图像作为输入图像的邻近图像。如图2所示，为3幅输入图像，及其部分邻近图像检索结果的示例图。

(2)接着，在邻近图像检索结果上，估计输入图像与每个邻近图像之间的相对位姿。(21)为此，对于输入图像和每个邻近图像，先寻找他们之间的匹配点对。分别在其中提取尺度变换不变特征，对于输入图像上的一个特征点假设邻近图像上与其特征描述符的欧式距离最近的特征点为第二近的特征点为如果：

那么将作为输入图像和邻近图像之间的一对匹配点，其中是特征和描述符之间的欧式距离。

(22)通过该方法，可以获得到一组输入图像与邻近图像之间的匹配点对，记为其中i为匹配点对下标，m为匹配点对个数。对于一对匹配点他们的齐次坐标分别记为p∝(x_p,y_p,1)^T和q∝(x_q,y_q,1)^T，其中x_p和x_q分别是点和点的横坐标，y_p和y_q分别是点和点的纵坐标。由于本发明假设输入图像具有一阶径向畸变参数λ，因此特征点的无畸变齐次坐标可以记为q_u∝(x_q,y_q,1+r²)^T，其中是特征点到畸变中心的欧式距离，u和v分别为畸变中心的横坐标和纵坐标，并且假设畸变中心在图像中心。根据外极几何理论，一对匹配点的无畸变齐次坐标应该满足外极几何约束方程：

p^TFq_u＝0， (2)

其中F是输入图像与邻近图像之间的基础矩阵。

由于p∝(x_p,y_p,1)^T，q_u∝(x_q,y_q,1+r²)^T，因此外极几何约束方程可以变形为：

$p^{T}F\left(\begin{array}{c}{x}_q\\ y_q\\ 1+{\mathrm{\λr}}^2\end{array}\right)=p^T\left[\begin{array}{cccc}{f}_1& f_2& f_3& {\mathrm{\λf}}_3\end{array}\right]\left(\begin{array}{c}{x}_q\\ y_q\\ 1\\ \mathrm{\λ}{r}^2\end{array}\right)=p^{T}V\left(\begin{array}{c}{x}_q\\ y_q\\ 1\\ \mathrm{\λ}{r}^2\end{array}\right)=0,---\left(3\right)$

其中，f_i是基础矩阵F的第i个列向量，即F＝[f ₁f₂ f₃]。同时，为了和传统的基础矩阵相区别，将矩阵[f₁ f₂ f₃ λf₃]记为V，并称之为单边径向基础矩阵。

受估计基础矩阵的8点算法启发，本发明提出了使用11对匹配点估计单边径向基础矩阵V，将算法简称为11点算法。通过分析外极几何约束方程(3)，可以发现，该方程对于单边径向基础矩阵V中的每个元素都是线性的，且常数项为0，因此为齐次线性方程。而且单边径向基础矩阵V是非零的，由于V中包含12个元素，因此可以通过构建包含11个方程的齐次线性方程组，并应用奇异值分解算法求解该齐次线性方程组，得到的非零解即对应单边径向基础矩阵V。由于每个匹配点对可以得到一个齐次线性方程，因此需要11对匹配点对，得到的齐次线性方程组为：

Av＝0， (4)

其中v是单边径向基础矩阵V按行优先的向量形式，A为系数矩阵。假设第i对匹配点对对应于第i个齐次线性方程，对应于系数矩阵A的第i行，其对应的系数向量为通过对A进行奇异值分解，对应于最小奇异值的右奇异向量即为v的解。

由于基础矩阵F＝[f₁ f₂ f₃]为秩为2的矩阵，且V＝[f₁ f₂ f₃ λf₃]的每一列都是矩阵F的列向量的线性组合，因此V的秩也为2，但是通过奇异值分解求得的矩阵不一定满足此约束，因此需要强制初步得到的矩阵V满足此约束。由于通过奇异值分解算法可以找到矩阵在弗罗贝尼乌斯范数下的最相似矩阵，本发明再次应用奇异值分解算法优化矩阵V。为此，首先对初始矩阵V后两列组成的矩阵应用奇异值分解，并且只保留最大的奇异值，其余奇异值设置为0，从而得到后两列的秩为1的最相似矩阵，通过求解这两列的线性相关系数，即可得到一阶径向畸变参数λ。然后，对初始矩阵V的前三列做奇异值分解，并保留最大的前两个奇异值，其余奇异值设置为0，从而得到秩为2的最相似矩阵，该矩阵即为估计得到的F。从而得到最终的V＝[f₁ f₂ f₃ λf₃]。

(23)在估计得到的基础矩阵F、一阶径向畸变参数λ、单边径向基础矩阵V的基础上进一步估计输入图像与邻近图像之间的本征矩阵，从而估计他们之间的相对位姿。由于邻近图像的内参已知，因此为了推导方便，可以假设邻近图像的内参矩阵为单位矩阵，因此输入图像与邻近图像之间的本征矩阵E为：

E＝FK (5)

其中，K为输入图像的内参矩阵，因为输入图像的一阶径向畸变参数λ已经估计得到，因此可以认为输入图像仅有一个内参，即焦距f，那么相应的K是一个对角矩阵，对角线的元素依次为f，f，1。由于基础矩阵的性质，其具有两个相等的非零奇异值，也就是说，基础矩阵满足：

2EE^TE-tr(EE^T)E＝0， (6)

其中tr()为求矩阵的迹，将方程(5)带入(6)可得：

2FKK^TF^TF-tr(FKK^TF^T)F＝0， (7)

方程(7)实际为一个矩阵方程，因此可以得到9个关于焦距平方f²的线性方程，我们以第一行第一列为例，可以得到：

$f^2(F_{11}{f}_1^T{f}_1+2F_{12}{f}_1^T{f}_2-F_{11}{f}_2^T{f}_2)=F_{11}{f}_3^T{f}_3-2F_{13}{f}_1^T{f}_3,---\left(8\right)$

由于f>0，可以得到：

$f=\sqrt{\frac{F_{11}{f}_3^T{f}_3-2F_{13}{f}_1^T{f}_3}{2F_{12}{f}_1^T{f}_2+F_{11}(f_1^T{f}_1-f_2^T{f}_2)}},---\left(9\right)$

其中F_ij是矩阵F的第i行第j列个元素，f_i是矩阵F的第i列的列向量。通过分别求解这9个方程，可以得到9个焦距值，取平均即可得到最终的焦距值。

(24)得到焦距值后，应用方程(5)即可得到基础矩阵E，在此基础上，通过计算机视觉领域经典的5点算法即可从基础矩阵E中估计出输入图像与参考图像的相对位姿。特别的，定义一个投影矩阵为P＝[R t]，其中R为3×3的旋转矩阵，t为平移向量。定义P_n＝[R_nt_n]为邻近图像在世界坐标系中的投影矩阵，P_rn＝[R_rn t_rn]为输入图像相对于邻近图像坐标系的投影矩阵。因此，得到输入图像关于两个邻近图像的相对位姿，即两个这样的相对投影矩阵。

(25)由于随机选择的11个点对中有可能存在误差较大或者错误的匹配点，因此本发明从所有匹配点中随机选择200次，估计结果，并使用计算得到的单边径向基础矩阵检查所有的匹配点对是否满足外极几何约束，即计算方程(3)的左值，根据经验，小于9的即认为满足该约束，将满足约束的匹配点对称为内点对，不满足约束的称为外点对，并记录内点对的个数。将内点对最多的结果作为最终的结果。

(26)对于输入图像和每一个邻近图像，均使用上述算法估计单边径向基础矩阵，并记录内点对个数，留下两个内点对个数最多的邻近图像对应的相对位姿计算结果，其余结果则被舍弃。

(3)最后融合这两个相对位姿，得到输入图像最终的位姿。首先计算输入图像的姿态，即输入图像的旋转矩阵。定义邻近图像相对于世界坐标系的投影矩阵是P_n＝[R_n t_n]，输入图像相对于邻近图像坐标系的投影矩阵是P_rn＝[R_rn t_rn]，那么输入图像相对于世界坐标系的旋转矩阵R_r是：

R_r＝R_rnR_n， (10)

并且该旋转矩阵可以分解为三个围绕坐标轴旋转的基本旋转矩阵的连乘，每个基本旋转矩阵对应一个欧拉角。在本发明中，旋转矩阵分解为依次围绕z-x-y轴的三个旋转矩阵，对应于θ_z，θ_x，θ_y三个欧拉角。对于得到的两个候选旋转矩阵，分别求得其对应的三个欧拉角，并取欧拉角的平均，并计算平均后欧拉角对应的旋转矩阵，即可得到输入图像的旋转矩阵，即输入图像的姿态信息。

在得到输入图像的姿态信息后，计算输入图像的位置信息，即可完成输入图像的六自由度位置信息计算。由于相对位姿无法确定尺度信息，因此得到输入图像与一个邻近图像的相对位姿后，无法确定输入图像的准确位置信息，只能确定输入图像在一条直线L上。给定输入图像相对于邻近图像坐标系的投影矩阵，将其确定的直线L的方程记为：

$\frac{x-x_l}{x_d}=\frac{y-y_l}{y_d}=\frac{z-z_l}{z_d},---\left(11\right)$

其中直线上的特点可以选邻近图像的位置l，根据邻近图像的投影矩阵，可得：

$l=\left[\begin{array}{c}{x}_l\\ y_l\\ z_l\end{array}\right]=-R_n^{-1}{t}_n,---\left(12\right)$

直线的方向d可以根据邻近图像的投影矩阵P_n和输入图像相对于邻近图像坐标系的投影矩阵P_rn得到：

$d=\left[\begin{array}{c}{x}_d\\ y_d\\ z_d\end{array}\right]=-R_n^{-1}{R}_{rn}^{-1}{t}_{rn},---\left(13\right)$

在直线L确定后，不难发现，一条直线并不能确定输入图像的位置，因此本发明使用了两个邻近图像，因此能够得到两条直线，记为L₁和L₂，两条直线的交点，即为输入图像的位置。由于计算误差的存在，直线L₁和L₂常常并不相交，因此，为了计算稳定，本发明定义了一个几何误差函数：

G＝G_l+G_n， (14)

该几何误差函数包括输入图像到两条直线的距离平方之和G_l，且定义为：

其中，x＝[x,y,z]^T是输入图像的位置，是输入图像到直线L_i的距离平方，如图3所示为的示意图，定义

G_n为正则项，是输入图像到邻近图像距离的平方和：

其中，是输入图像到邻近图像l_i的距离平方：

由于几何误差函数G是关于输入图像位置x的凸二次函数，因此对G求关于x的每个元素的偏导，并令偏导为0，可得到关于图像位置x的线性方程组：

$\left\{\begin{array}{c}\frac{\∂G}{\∂x}=\underset{i=1}{\overset{2}{\mathrm{\Σ}}}\left(4\right(x-x_{l_i})+\frac{2x_{d_i}\left(x_{d_i}\right(x-x_{l_i})+y_{d_i}(y-y_{l_i})+z_{d_i}(z-z_{l_i}))}{x_{d_i}^2+y_{d_i}^2+z_{d_i}^2})=0\\ \frac{\∂G}{\∂y}=\underset{i=1}{\overset{2}{\mathrm{\Σ}}}\left(4\right(y-y_{l_i})+\frac{2y_{d_i}\left(x_{d_i}\right(x-x_{l_i})+y_{d_i}(y-y_{l_i})+z_{d_i}(z-z_{l_i}))}{x_{d_i}^2+y_{d_i}^2+z_{d_i}^2})=0\\ \frac{\∂G}{\∂z}=\underset{i=1}{\overset{2}{\mathrm{\Σ}}}\left(4\right(z-z_{l_i})+\frac{2z_{d_i}\left(x_{d_i}\right(x-x_{l_i})+y_{d_i}(y-y_{l_i})+z_{d_i}(z-z_{l_i}))}{x_{d_i}^2+y_{d_i}^2+z_{d_i}^2})=0\end{array}\right.,---\left(19\right)$

求解该线性方程组，即可得到输入图像的位置。

如图4所示，为使用本方法的完成单幅图像场景六自由度定位，并在三维空间中进行可视化的结果，其中每个图像对应的六自由度信息用一个四棱锥表示，锥顶表示图像的位置，椎体的方向表示图像对应的姿态信息。

Claims (4)

1.邻近位姿融合引导的单幅图像场景六自由度定位方法，其特征在于实现步骤如下：

步骤一、对于输入图像I，应用视觉词袋检索算法从参考图像集中检索出一组输入图像的邻近图像集，其中参考图像集中的每张图像均标注了六自由度的位姿信息，并且已经标定了相机内参；

步骤二、对邻近图像集中的每个邻近图像N标定输入图像的内参，在此基础上，估计出输入图像I与每个邻近图像N之间的相对位姿，结合邻近图像的位姿，从而得到输入图像的一组候选位姿集；

步骤三、定义一个几何误差函数，通过最小化几何误差函数，融合输入图像的候选位姿集，得到输入图像的最终位置信息；输入图像的位置确定后，其姿态信息由两个候选位姿的姿态信息平均计算得到，从而实现图像场景的六自由度定位。

2.根据权利要求1所述的邻近位姿融合引导的单幅图像场景六自由度定位方法，其特征在于：所述步骤一中视觉词袋检索算法的具体实现过程包括：

(11)提取参考图像集中每个图像上的尺度变换不变特征作为局部特征，并应用k均值聚类算法计算得到视觉单词树，且建立的视觉单词树包含20000～40000个视觉描述符；

(12)利用所得到的视觉单词树，计算输入图像和参考图像集中每个图像的视觉描述文本，即每个图像的所有局部特征在视觉单词树上的统计直方图；

(13)通过计算输入图像与参考图像集中每个图像的视觉描述文本的差异，选取差异最小的15～30个参考图像作为输入图像的邻近图像。

3.根据权利要求1所述的邻近位姿融合引导的单幅图像场景六自由度定位方法，其特征在于：所述步骤二中输入图像I与每个邻近图像N之间的相对位姿估计，结合邻近图像的位姿，从而得到输入图像的一组候选位姿集通过如下步骤实现的：

(21)提取输入图像与邻近图像中的所有尺度变换不变特征，并寻找输入图像上的特征点与邻近图像特征点之间的匹配，从而得到一组匹配点对；

(22)在匹配点对中随机选取11对特征点对，使用这些特征点对，建立线性方程组，通过奇异值分解算法求解线性方程组，得到输入图像与邻近图像之间的基础矩阵；

(23)从得到的基础矩阵中估计出输入图像的焦距，从而得到输入图像与邻近图像之间的本征矩阵；

(24)使用计算机视觉领域经典的5点算法从得到的本征矩阵中估计出输入图像与邻近图像的相对位姿，从而得到输入图像的一个候选位姿；

(25)重复(22)到(24)的过程，选取最好的相对位姿计算结果，选取准则为：在每个得到的相对位姿基础上，计算输入图像与邻近图像满足外极几何约束的特征匹配点对个数，个数最多的作为最终的结果；

(26)在估计输入图像与每个邻近图像的相对位姿基础上，比较满足外极几何约束的匹配点对个数，只选择匹配点对个数最多的两个作为输入图像的候选位姿。

4.根据权利要求1所述的邻近位姿融合引导的单幅图像场景六自由度定位方法，其特征在于：所述步骤三中几何误差的定义方法是：首先定义几何误差为输入图像位置到直线的距离平方之和；然后，由于输入图像与邻近图像的位置较为接近，为了使方法的结果更稳定，在几何误差中增加了输入图像位置到邻近图像位置的距离平方和作为正则项。