CN109903313A - 一种基于目标三维模型的实时位姿跟踪方法 - Google Patents

Abstract

本发明涉及一种基于目标三维模型的实时位姿跟踪方法。本发明基于目标的三维模型，利用轮廓特征，建立轮廓部件模型，依据局部轮廓辨识度将轮廓分割成若干局部轮廓段；针对每一轮廓段，在梯度域基于相位匹配准则建立二维‑三维对应；利用刚体目标各部件之间的几何关系，消除错误匹配对；基于建立的二维‑三维匹配对，通过RPNP算法求解位姿参数，迭代整个过程直至收敛，实现序列图像中目标位姿跟踪。本发明算法步骤简单明了，易于应用，且由于引入目标的三维模型，可实现全程无漂移跟踪。

Description

一种基于目标三维模型的实时位姿跟踪方法

技术领域

本发明主要涉及计算机视觉、摄像测量、视觉导航等领域，特指一种基于目标三维模型的实时目标位姿跟踪方法。

技术背景

在航天、工业机器人等领域自主操作应用中，准确、可靠及高效的目标位姿跟踪是其中的关键技术之一。随着计算机视觉、摄像测量等技术的发展，基于视觉的目标位姿跟踪方法受到了越来越多的重视，基于视觉的位姿跟踪方法具有精度高、成本低等优势。

已有基于视觉的目标位姿跟踪方法主要是建立二维-三维之间的特征匹配集合，基于建立的特征匹配集合，完成对目标位姿参数的解算，进而实现目标位姿跟踪。点、线及轮廓等特征被应用于已有基于视觉的目标位姿跟踪研究中。

Pauwels等人提出一种针对任意外形的刚体目标的实时位姿估计与跟踪算法，算法基于双目像机获取的图像，利用目标纹理及几何结构信息，采用SIFT特征及光流提取立体及运动信息，并基于此构建目标函数，求解得到位姿跟踪结果，该算法需要利用目标的纹理信息，不能很好地适应复杂光照、纹理变化等情况。Prisacariu等人将分割与位姿跟踪同时考虑，指出二维图像中目标区域与其位姿存在对应关系，提出基于区域的实时位姿跟踪方法，通过引入水平集分割的思想，基于像素级后验概率构建目标函数，在位姿参数张成的空间中采用梯度下降法求解，实现序列图像中目标的位姿跟踪，该算法无需事先获取目标的纹理信息。Hexner等人针对传统方法不能很好地处理具有复杂结构的物体或复杂背景，受局部主动轮廓研究的启发，提出利用多个局部区域信息的位姿求解算法。Wang等人针对当前基于轮廓采样点算法极易陷入局部极值的缺陷进行研究，提出基于全局优化的对应关系搜索方法，取得了不错的实验结果，但该算法计算复杂度较高。

深度学习技术也被应用于目标位姿跟踪研究中，相关方法可归结于以下两个主要类别：(1)类似于传统方法，深度学习技术用于学习特征描述，建立二维-三维对应，进而采用传统位姿参数解算方法求取相对关系参数；(2)充分发挥深度学习技术端到端的优势，直接基于图像信息，输出目标位姿参数，无需显式地建立二维-三维对应。总结起来，需要大量的标注数据、计算效率低及位姿跟踪精度不高等因素限制了深度学习相关目标位姿跟踪方法的推广应用。

发明内容

本专利针对高效的目标位姿跟踪问题，发明了一种基于目标三维模型的实时位姿跟踪方法：基于目标的三维模型及初始位姿估计投影得到二值图像(目标区域为1，背景区域为0)，提取目标轮廓，利用轮廓特征，建立轮廓部件模型，依据局部轮廓辨识度将轮廓分割成若干局部轮廓段；针对每一轮廓段，在梯度域基于相位匹配准则建立二维-三维对应；利用刚体目标各部件之间的几何关系，消除错误匹配对；基于建立的二维-三维匹配点对，通过RPNP(Robust Perspective-N-Point，RPNP，鲁棒的N点透视)算法求解位姿参数，迭代整个过程直至收敛，实现序列图像中目标位姿跟踪。

1、本发明的一种实时的基于目标三维模型的目标位姿跟踪方法的实施过程

本发明实施过程如附图1所示，具体如下：

(1)轮廓部件模型生成：基于目标的三维模型，结合初始位姿估计，投影生成二值图像，提取目标轮廓，基于局部轮廓特征值定义辨识度，将目标轮廓分割成若干辨识度较高的轮廓段，构成目标轮廓部件模型，并保存轮廓部件模型中心点及对应目标三维模型上的点组成的点对集合；

(2)梯度域相位匹配：将投影得到的二值图像及输入图像转换到梯度域，求取梯度相位信息，针对轮廓部件模型中每一个轮廓段，采用相位匹配准则，在其一定尺寸邻域内搜索最佳匹配位置，建立匹配对；

(3)误匹配剔除：依据目标的刚体特性，利用轮廓部件之间的几何位置关系，剔除误匹配，并结合步骤(1)中建立的到目标三维模型的点对应关系，得到误匹配点剔除后的二维-三维匹配点对集合；

(4)目标位姿参数求解：依据步骤(2)～(3)建立的二维-三维匹配点对集合，采用RPNP算法求解位姿参数，迭代整个过程直至收敛，实现目标三维位姿跟踪。

2、本发明的一种实时的基于目标三维模型的目标位姿跟踪方法，具体步骤：

(1)轮廓部件模型生成

利用目标的轮廓特征建立投影图像与输入图像之间匹配对的建立。本发明采用的目标三维模型采用的是顶点和三角面片的数据格式，利用目标的三维模型及目标的初始位姿估计，依据如图2所示的成像模型，采用公式(1)将目标三维模型中的每个顶点投影到图像上，

式中R及T分别为描述刚体运动的旋转矩阵及平移向量，[X_W Y_W Z_W]^T、[X_C Y_C Z_C]^T及[uv]^T分别为顶点在世界坐标系、相机坐标系及图像坐标系中的坐标，F_x、F_y、C_x及C_y为相机焦距及主点参数，事先通过标定获得。依据顶点投影结果，计算面片对应的投影区域，最终确定在投影图像中整个目标对应的区域，将目标对应的区域置1，背景区域置0，得到二值投影图像，记为T；

采用腐蚀数学形态学操作算子对二值投影图像进行处理，其中核元素为3*3的十字模板，再与原始二值图像做差，得到目标投影区域的最***轮廓，对应的顺序点集记为N为轮廓长度；

在原始二值图像中，对于轮廓中每一个轮廓点对应的位置s_i，计算其对应的Shi-Tomasi角点响应值，记为v_i，角点响应值越大，说明对应位置的轮廓走向存在较大改变，更具辨识度，依据{v_i}_N从大到小对所有轮廓点进行排序，并采用非最大值抑制方法选出Q个辨识度高的轮廓点，记为Q为选取的轮廓点个数。

针对以其为中心选择长度L的轮廓段S_j，则{S_j}_Q即为得到的目标在当前位姿参数下的轮廓部件模型，对应S_j，在投影计算的过程中保存深度信息，依据相机参数可反求出对应的物体上的点的三维坐标，记为轮廓部件模型的点对集合为

(2)梯度域相位匹配

为适应纹理、光照差异等干扰因素，本发明在梯度域进行匹配操作，旨在建立可靠的匹配对。采用3*3大小的Sobel算子求取投影到得到的二值图像T及输入图像I对应的梯度图，3*3大小的Sobel算子如图3所示；

针对每一个轮廓段S_j，基于梯度相位信息完成轮廓段的匹配，本发明基于余弦和准则定义梯度相位相似性度量准则。记T中轮廓段S_j对应的图像子块为T'，在输入图像中待匹配图像子块记为I'，(dx_pT,dy_pT)及(dx_pI,dy_pI)分别为T'及I'中的像素点梯度向量，定义梯度相位角为梯度向量与图像坐标系轴正方向的夹角，对应的梯度相位角度记为θ_pT及θ_pI，梯度相位角的范围为[0,2π]，则基于余弦和准则的梯度相位匹配准则如公式(2)所示

式中X和Y为图像子块的尺寸，Score为相似度值。(θ_pT-θ_pI)即为梯度向量之间的夹角，将梯度向量归一化

式中norm(g)求取向量的模值，则

cos(θ_pT-θ_pI)＝dx'_pT*dx'_pI+dy'_pT*dy'_pI (4)

在实践应用中，由于光照变化、纹理差异等因素的影响，可能使得图像中出现梯度反转的情况，本发明认为向量方向相同或相反完全等价，即(θ_pT-θ_pI)＝0与(θ_pT-θ_pI)＝π等价，改写公式(2)中相似度准则为

有二倍角公式知cos(2(θ_pT-θ_pI))＝2cos(θ_pT-θ_pI)²-1，则相似度准则可进一步转化为

将公式(4)带入公式(6)得

在投影二值图像中，本发明仅提取最***目标轮廓，为消除实时输入图像中目标***轮廓附近的梯度造成干扰，采用原始二值图像子块T'对应的梯度幅值对匹配准则进行加权，即

其中w_pT＝norm((dx_pT,dy_pT))，Score的取值范围为[0,1]。公式(8)中包含水平、垂直方向上的卷积及中间项卷积，利用卷积定理，本发明采用傅里叶变换加速卷积计算，提升位姿跟踪效率；

基于公式(8)定义的梯度相位匹配准则，针对每一个轮廓段S_j，在其初始位置附近一定尺寸邻域内采用滑动窗口策略搜索最佳匹配位置，若邻域内最大相似度值大于预先设定的阈值Threshold，则认为成功匹配，否则此轮廓段匹配失败。以轮廓段中心表示匹配对，则最终得到的成功匹配对集合记为K为成功匹配对数；

(3)误匹配剔除

遮挡、视角变化等因素的存在势必会影响匹配的可靠性，造成误匹配的存在。本发明针对的刚体目标的位姿跟踪，刚体目标不存在形变，自身各部件间相对几何位置关系固定，利用刚体目标这一特性，采用局部结构保持的匹配策略，剔除误匹配点；

针对依据结构保持策略，建立如下目标函数

式中P＝{p_k}_K,p_k∈{0,1}为标识向量，p_k＝1表示匹配点正确，反之为误匹配点，N_p为点p的邻域，以欧氏距离度量，λ为控制参数。通过最小化公式(9)中所示目标函数，即可求得标识向量P，进而剔除误匹配点，得到正确的匹配点对集合，记为结合目标三维模型投影中得到的点对集合记为这就建立了输入图像与目标三维模型之间的二维-三维对应关系，对应的二维-三维匹配点对集合为

(4)位姿参数求解

基于得到的二维-三维匹配点对集合采用RPNP算法求解目标位姿参数，迭代整个过程，并依据迭代中位姿参数更新量判断迭代是否终止，输出最终的位姿更新结果。

3、本发明达到的技术效果

与现有技术相比，本发明的优点在于：

(1)本发明利用目标的三维模型，仅基于单目图像完成目标位姿跟踪，算法步骤简单明了，易于应用，且由于引入目标的三维模型，可实现全程无漂移跟踪；

(2)本发明通过选取辨识度较少的轮廓段构建目标轮廓部件模型，各部件单独计算，并可根据相似度值及基于结构保持的误匹配剔除策略剔除误匹配，可有效提升算法对遮挡的适应能力；

(3)本发明采用梯度相位信息实现轮廓部件的匹配，仅利用目标最***轮廓的相位信息，可有效提升算法对光照、纹理差异等干扰的鲁棒性；

(4)本发明仅选取辨识度较高的若干轮廓段，并采用傅里叶变换加速梯度相位匹配准则的计算，可高效解算目标位姿参数，实现实时目标位姿跟踪。

附图说明

图1一种基于目标三维模型的实时位姿跟踪方法流程图；

图2成像模型；

图3 3*3Sobel算子。

具体实施方式

以下对本发明的实施方式做进一步详细说明。

(1)轮廓部件模型生成

利用目标的三维模型，结合目标位姿初始估计，针对模型中的顶点，依据如图2所示的成像模型，采用公式(1)投影到图像上，式中R及T分别为描述刚体运动的旋转矩阵及平移向量，式中[X_W Y_W Z_W]^T、[X_C Y_C Z_C]^T及[u v]^T分别为顶点在世界坐标系、相机坐标系及图像坐标系中的坐标，F_x、F_y、C_x及C_y为相机焦距及主点参数，事先通过标定获得。依据顶点投影结果，计算面片对应的投影区域，最终确定在投影图像中整个目标对应的区域，将目标对应的区域置1，背景区域置0，得到二值投影图像，记为T；

采用腐蚀数学形态学操作算子对二值投影图像进行处理，其中核元素为3*3的十字模板，再与原始二值图像做差，得到目标投影区域的最***轮廓，对应的顺序点集记为N为轮廓长度；

在原始二值图像中，对于轮廓中每一个轮廓点对应的位置s_i，计算其对应的Shi-Tomasi角点响应值，记为v_i，角点响应值越大，说明对应位置的轮廓走向存在较大改变，更具辨识度，依据{v_i}_N从大到小对所有轮廓点进行排序，并采用非最大值抑制方法选出Q个辨识度高的轮廓点，Q为选取的轮廓点个数，记为针对以其为中心选择长度L为的轮廓段S_j，则{S_j}_Q即为得到的目标在当前位姿参数下的轮廓部件模型，对应S_j，在投影计算的过程中保存深度信息，依据相机参数可反求出对应的物体上的点的三维坐标，记为轮廓部件模型的点对集合为

(2)梯度域相位匹配

为适应纹理、光照差异等干扰因素，本发明在梯度域进行匹配操作，旨在建立可靠的匹配对。采用3*3大小的Sobel算子求取投影到得到的二值图像T及输入图像I对应的梯度图，3*3大小的Sobel算子如图3所示；

针对每一个轮廓段S_j，基于梯度相位信息完成轮廓段的匹配，本发明基于余弦和准则定义梯度相位相似性度量准则。记T中轮廓段S_j对应的图像子块为T'，在输入图像中待匹配图像子块记为I'，(dx_pT,dy_pT)及(dx_pI,dy_pI)分别为T'及I'中的像素点梯度向量，定义梯度相位角为梯度向量与图像坐标系轴正方向的夹角，对应的梯度相位角度记为θ_pT及θ_pI，梯度相位角的范围为[0,2π]，则基于余弦和准则的梯度相位匹配准则如公式(2)所示

式中X和Y为图像子块的尺寸，Score为相似度值。(θ_pT-θ_pI)即为梯度向量之间的夹角，将梯度向量归一化

式中norm(g)求取向量的模值，则

在实践应用中，由于光照变化、纹理差异等因素的影响，可能使得图像中出现梯度反转的情况，本发明认为向量方向相同或相反完全等价，即(θ_pT-θ_pI)＝0与(θ_pT-θ_pI)＝π等价，改写公式(2)中相似度准则为

有二倍角公式知cos(2(θ_pT-θ_pI))＝2cos(θ_pT-θ_pI)²-1，则相似度准则可进一步转化为

将公式(4)带入公式(6)得

在投影二值图像中，本发明仅提取最***目标轮廓，为消除实时输入图像中目标***轮廓附近的梯度造成干扰，采用原始二值图像子块T'对应的梯度幅值对匹配准则进行加权，即

其中w_pT＝norm((dx_pT,dy_pT))，Score的取值范围为[0,1]。公式(8)中包含水平、垂直方向上的卷积及中间项卷积，利用卷积定理，本发明采用傅里叶变换加速卷积计算，提升位姿跟踪效率；

基于公式(8)定义的梯度相位匹配准则，针对每一个轮廓段S_j，在其初始位置附近一定尺寸邻域内采用滑动窗口策略搜索最佳匹配位置，若邻域内最大相似度值大于预先设定的阈值Threshold，则认为成功匹配，否则此轮廓段匹配失败。以轮廓段中心表示匹配对，则最终得到的成功匹配对集合记为K为成功匹配对数；

(3)误匹配剔除

遮挡、视角变化等因素的存在势必会影响匹配的可靠性，造成误匹配的存在。本发明针对的刚体目标的位姿跟踪，刚体目标不存在形变，自身各部件间相对几何位置关系固定，利用刚体目标这一特性，采用局部结构保持的匹配策略，剔除误匹配点；

针对依据结构保持策略，建立如下目标函数

式中p＝{p_k}_K,p_k∈{0,1}为标识向量，p_k＝1表示匹配点正确，反之为误匹配点，N_p为点p的邻域，以欧氏距离度量，λ为控制参数。通过最小化公式(9)中所示目标函数，即可求得标识向量p，进而剔除误匹配点，得到正确的匹配点对集合，记为结合目标三维模型投影中得到的点对集合记为这就建立了输入图像与目标三维模型之间的二维-三维对应关系，对应的二维-三维匹配点对集合为

(4)位姿参数求解

基于得到的二维-三维匹配点对集合采用RPNP算法求解目标位姿参数，迭代整个过程，并依据迭代中位姿参数更新量判断迭代是否终止，输出最终的位姿更新结果。

以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。

Claims (1)

1.一种基于目标三维模型的实时位姿跟踪方法，其特征在于：基于目标的三维模型及初始位姿估计投影得到二值图像，目标区域为1，背景区域为0，提取目标轮廓，利用轮廓特征，建立轮廓部件模型，依据局部轮廓辨识度将轮廓分割成若干局部轮廓段；针对每一轮廓段，在梯度域基于相位匹配准则建立二维-三维对应；利用刚体目标各部件之间的几何关系，消除错误匹配对；基于建立的二维-三维匹配点对，通过RPNP鲁棒的N点透视算法求解位姿参数，迭代整个过程直至收敛，实现序列图像中目标位姿跟踪，

其步骤为：

(1)轮廓部件模型生成

利用目标的轮廓特征建立投影图像与输入图像之间匹配对的建立，利用目标三维模型采用的是顶点和三角面片的数据格式，利用目标的三维模型及目标的初始位姿估计，采用公式(1)将目标三维模型中的每个顶点投影到图像上，

式中R及T分别为描述刚体运动的旋转矩阵及平移向量，[X_W Y_W Z_W]^T、[X_C Y_C Z_C]^T及[uv]^T分别为顶点在世界坐标系、相机坐标系及图像坐标系中的坐标，F_x、F_y、C_x及C_y为相机焦距及主点参数，通过标定获得，依据顶点投影结果，计算面片对应的投影区域，最终确定在投影图像中整个目标对应的区域，将目标对应的区域置1，背景区域置0，得到二值投影图像，记为T；

采用腐蚀数学形态学操作算子对二值投影图像进行处理，其中核元素为3*3的十字模板，再与原始二值图像做差，得到目标投影区域的最***轮廓，对应的顺序点集记为N为轮廓长度；

在原始二值图像中，对于轮廓中每一个轮廓点对应的位置s_i，计算其对应的Shi-Tomasi角点响应值，记为v_i，角点响应值越大，说明对应位置的轮廓走向存在较大改变，更具辨识度，依据{v_i}_N从大到小对所有轮廓点进行排序，并采用非最大值抑制方法选出Q个辨识度高的轮廓点，记为Q为选取的轮廓点个数，

针对以其为中心选择长度L的轮廓段S_j，则{S_j}_Q即为得到的目标在当前位姿参数下的轮廓部件模型，对应S_j，在投影计算的过程中保存深度信息，依据相机参数可反求出对应的物体上的点的三维坐标，记为轮廓部件模型的点对集合为

(2)梯度域相位匹配

在梯度域进行匹配操作，旨在建立可靠的匹配对，采用3*3大小的Sobel算子求取投影到得到的二值图像T及输入图像I对应的梯度图；

针对每一个轮廓段S_j，记T中轮廓段S_j对应的图像子块为T'，在输入图像中待匹配图像子块记为I'，(dx_pT,dy_pT)及(dx_pI,dy_pI)分别为T'及I'中的像素点梯度向量，定义梯度相位角为梯度向量与图像坐标系轴正方向的夹角，对应的梯度相位角度记为θ_pT及θ_pI，梯度相位角的范围为[0,2π]，则基于余弦和准则的梯度相位匹配准则如公式(2)所示

式中X和Y为图像子块的尺寸，Score为相似度值，(θ_pT-θ_pI)即为梯度向量之间的夹角，将梯度向量归一化

式中norm(g)求取向量的模值，则

cos(θ_pT-θ_pI)＝dx'_pT*dx'_pI+dy'_pT*dy'_pI (4)

本发明认为向量方向相同或相反完全等价，即(θ_pT-θ_pI)＝0与(θ_pT-θ_pI)＝π等价，改写公式(2)中相似度准则为

有二倍角公式知cos(2(θ_pT-θ_pI))＝2cos(θ_pT-θ_pI)²-1，则相似度准则进一步转化为

将公式(4)带入公式(6)得

在投影二值图像中，仅提取最***目标轮廓，为消除实时输入图像中目标***轮廓附近的梯度造成干扰，采用原始二值图像子块T'对应的梯度幅值对匹配准则进行加权，即

其中w_pT＝norm((dx_pT,dy_pT))，Score的取值范围为[0,1]；

针对每一个轮廓段S_j，在其初始位置附近一定尺寸邻域内采用滑动窗口策略搜索最佳匹配位置，若邻域内最大相似度值大于预先设定的阈值，则认为成功匹配，否则此轮廓段匹配失败，以轮廓段中心表示匹配对，则最终得到的成功匹配对集合记为K为成功匹配对数；

(3)误匹配剔除

针对的刚体目标的位姿跟踪，刚体目标不存在形变，自身各部件间相对几何位置关系固定，采用局部结构保持的匹配策略，剔除误匹配点；

针对依据结构保持策略，建立如下目标函数

式中P＝{p_k}_K,p_k∈{0,1}为标识向量，p_k＝1表示匹配点正确，反之为误匹配点，N_p为点p的邻域，以欧氏距离度量，λ为控制参数，通过最小化公式(9)中所示目标函数，即求得标识向量P，进而剔除误匹配点，得到正确的匹配点对集合，记为结合目标三维模型投影中得到的点对集合记为建立输入图像与目标三维模型之间的二维-三维对应关系，对应的二维-三维匹配点对集合为

(4)位姿参数求解

基于得到的二维-三维匹配点对集合采用RPNP鲁棒的N点透视算法求解目标位姿参数，迭代整个过程，并依据迭代中位姿参数更新量判断迭代是否终止，输出最终的位姿更新结果。