CN101907459A - 基于单目视频的实时三维刚体目标姿态估计与测距方法 - Google Patents

Abstract

基于单目视频的实时三维刚体目标姿态估计与测距方法，首先通过光学观测设备采集目标的观测视频，将采集得到的图像序列送入目标分割模块以获得目标的二值分割图像和轮廓图像，提取目标轮廓点上的特征向量，生成多特征驱动距离图，建立输入的二维图像序列与目标的三维模型之间的试探性同名特征对应关系，对图像中目标的三维姿态和距离参数进行反演，将反演得到的目标的三维姿态和距离参数反馈，对二维图像序列与目标的三维模型之间的试探性同名特征对应关系进行修正更新，直至满足迭代停止条件，本发明无需立体成像设备，具有不损伤被观测目标、隐蔽性好的优点，同时具有成本低、自动化程度高的优点。

Description

基于单目视频的实时三维刚体目标姿态估计与测距方法

技术领域

本发明属于目标姿态估计与测距技术领域，具体涉及基于单目视频的实时三维刚体目标姿态估计与测距方法。

背景技术

三维刚体目标的姿态和距离参数测量是众多应用领域的一个基本问题，在计算机图形学、摄影测量、机器人导航、建筑施工、交通控制、军事侦察中均有着重要的应用价值。传统的测量方法主要包括以下两种：一是手工测量，这种方法受测量现场和测量设备限制，一般只能应用在比较简单的场合，应用限制大、精度低；二是采用激光、声纳、测距雷达等有源装置进行主动测量，这种测量方式除了成本高外，被测目标都会受到测量仪器发出某种形式的能量影响，不利于被测目标(如古文物)的保护和测量活动(如军事侦察)的隐蔽。另外，这两种测量方式都只能在现场进行测量，不具有时间或空间的可重复性。而基于计算机视觉的测量方法则具有设备简单、成本低、测量方便、不要求测量的现场性等优点而得到越来越多的关注和发展。

根据系统所要求的图像采集镜头的数量，基于计算机视觉的测量方法又可分为基于多目观测的测量方式和基于单目观测的测量方式。基于多目观测的测量方式又称为基于立体像对的目标测量，是基于计算机视觉的目标测量研究中最为成熟的一种方法，它利用立体像对的三角交会原理进行距离测量，具有算法设计简单的优点；其缺点是图像采集需要专业立体成像设备，复杂昂贵，每个采集镜头成像参数以及多个镜头之间的相对姿态参数必须事先经过严格校正才能保证测量的准确性；为了保证测距精度，两个采集镜头之间的基线相对于测量目标的尺寸比必须足够大，造成设备的应用限制较大；而且基于立体像对的目标测量方法没有充分挖掘单个镜头所采集数据中所包含的潜在信息，只能获取目标的距离参数，测量参数单一。与之相比，基于单目观测的目标测量方式对成像设备的要求大大降低，无需专业立体成像设备，任何具有成像能力的设备如普通的监控摄像机、家用DV等都可以做为系统的图像采集镜头，同时也不存在立体成像设备的基线要求限制；在充分挖掘目标单目观测视频所包含的信息的基础上，除了能够获取目标的距离参数外，还可以获取目标的姿态、运动矢量、加速度矢量等参数；所付出的代价是算法的复杂性增高，对系统的计算能力要求较高。

目前对基于单目观测的目标测量方法研究仍集中在二维平面场景的测量和目标同名特征已知情况下的三维刚体目标测量，应用范围限制较大，限定应用场景或限定目标类型，自动化程度较低，需要用户手工标定目标同名特征信息；对在事先未知目标同名特征情况下的三维刚体目标测量的研究并不多见，而这正是实际应用中最为需要的。

发明内容

为了克服上述现有技术的缺点，本发明的目的在于提供了基于单目视频的实时三维刚体目标姿态估计与测距方法，无需立体成像设备，具有不损伤被观测目标、隐蔽性好的优点，同时具有成本低、自动化程度高的优点。

为了达到上述目的，本发明采用的技术方案是：

基于单目视频的实时三维刚体目标姿态估计与测距方法，包括以下步骤：

步骤1，通过光学观测设备采集目标的观测视频，光学观测设备为单目观测镜头；

步骤2，将采集得到的图像序列送入目标分割模块以获得目标的二值分割图像和轮廓图像，采用的方法是基于水平集的改进型区域竞争方法，分割模型方程如下所示：

$E\left(C\right)=\mathrm{\α}\·\mathrm{Length}\left(C\right)+c\·\mathrm{Area}\left(\mathrm{inside}\left(C\right)\right)$

$+{\mathrm{\λ}}_1\underset{\mathrm{inside}\left(C\right)}{\∫}|I-c_1|\mathrm{dxdy}$

$+{\mathrm{\λ}}_2\underset{\mathrm{outside}\left(C\right)}{\∫}|I-c_2|\mathrm{dxdy}---\left(1\right)$

其中，I表示待分割图像，C表示零水平集轮廓，c₁表示目标区域的平均灰度，c₂表示背景区域的平均灰度；α，c，λ₁，λ₂均为加权系数；

步骤3，根据目标的二值分割图像和轮廓图像，提取目标轮廓点上的特征向量，由轮廓特征提取模块对目标轮廓上的点进行特征矢量提取，轮廓特征提取模块对目标轮廓上的点，提取如下的三维特征矢量：

$\stackrel{V}{f}={(x,y,\mathrm{curv})}^T---\left(2\right)$

x、y表示轮廓点在像平面的横纵坐标，curv表示目标轮廓在该点的曲率，采用如下步骤来计算目标轮廓在给定点的曲率：令Mask表示一个半径为r的圆形掩模，对于给定点p，记其灰度值为I(p)；以该p点为掩模中心，计算掩模Mask范围内的图像像素点与掩模中心点的灰度差异，则p点对应的曲率计算公式为：

$\mathrm{curv}=\underset{q\∈\mathrm{Mask}}{\mathrm{\Σ}}\mathrm{\Φ}(I\left(q\right)-I\left(p\right))---\left(3\right)$

其中，Φ(·)为隶属度模糊函数，方程如下：

$\mathrm{\&Phi;}\left(x\right)=\left\{\begin{array}{cc}1& \mathrm{if}\left|x\right|\&le;t_1\\ \frac{1}{1-t_1}\left(\frac{t_2-\left|x\right|}{t_2}\right)& \mathrm{if}{t}_1<\left|x\right|\&le;t_2\\ 0& \mathrm{if}\left|x\right|>t_2\end{array}\right.---\left(4\right)$

其中，x为自变量，t₁、t₂为阈值，t₁＜t₂。

步骤4，根据目标的轮廓图像和目标轮廓点上的特征向量，生成多特征驱动距离图，多特征驱动距离图是指使用加权多特征距离作为距离度量范数的距离图，其数学描述如下：

令G表示N维特征空间中的均匀采样网格，

表示目标采样点集合，多特征驱动距离图描述的是网格G中的每一点到目标点集合C的最短距离；对采样点x∈G，其距离值定义为：

$\mathrm{MFDM}\left(x\right)=\underset{y}{\min }\left|\right|x-y\left|\right|,\&ForAll;y\&Element;C---\left(5\right)$

其中，距离度量||·||采用的是加权多特征距离范数，

“加权多特征距离范数”是指，设N维特征空间中的两点

则该两点之间的加权多特征驱动距离定义为

$\mathrm{MFD}(\stackrel{V}{p},\stackrel{V}{q})=w_1{(x_1-x_1^{\&prime;})}^2+w_2{(x_2-x_2^{\&prime;})}^2+L{w}_N{(x_N-x_N^{\&prime;})}^2---\left(6\right)$

其中w₁L w_N为各特征维的加权系数，公式(6)所定义的加权多特征驱动距离提供了一种可以容纳任意个数的特征及其组合方式的算法框架，在加权多特征驱动距离框架下，可以添加任何有效的特征，并通过调整各特征维的加权系数来提高同名点的匹配精度，

多特征驱动距离图是采用如下的加权快速距离变换来生成多特征驱动距离图：

1)一维情况下的加权快速距离变换

在一维情况下，加权距离变换定义式为：

$D_f\left(x\right)=\underset{q\&Element;C}{\min }(w\&CenterDot;||x-q||+f\left(q\right))---\left(7\right)$

其中f(·)为初始距离函数，C为其定义域，w为标量加权系数。当度量范数采用2范数时，上式可以重写为

$D_f\left(x\right)=\underset{q\&Element;C}{\min }(w\&CenterDot;{(x-q)}^2+f\left(q\right))---\left(8\right)$

(8)式可以看成是以x为自变量的二次函数，其形状为抛物线；从(8)式可以得出如下结论：距离变换D_f(x)等于所有不同q值的抛物线y＝w·(x-q)²+f(q)的下包迹的值，

由于抛物线方程已知，要求取其下包迹只需要再知道各抛物线之间的交点坐标；对于q＝q₁和q＝q₂的两条抛物线，其交点坐标为：

$s=\frac{f\left(q_1\right)-f\left(q_2\right)}{2w(q_1-q_2)}+q_1+q_2---\left(9\right)$

2)N维情况下的加权快速距离变换

在N维情况下，加权距离变换定义式为

$D_f(x_1,x_{2}L{x}_N)=\underset{(x_1^{\&prime;},x_2^{\&prime;}L{x}_N^{\&prime;})\&Element;C}{\min }\left(\begin{array}{c}{w}_1{(x_1-x_1^{\&prime;})}^2+w_2{(x_2-x_2^{\&prime;})}^2+L\\ w_N{(x_N-x_N^{\&prime;})}^2+f(x_1^{\&prime;},x_2^{\&prime;}L{x}_N^{\&prime;})\end{array}\right)$

                                (10)

其中w₁L w_N为各维的加权系数；上式可重写为

$D_f(x_1,x_{2}L{x}_N)=\underset{x_1^{\&prime;}}{\min }\left(\begin{array}{c}{w}_1{(x_1-x_1^{\&prime;})}^2+\\ \underset{x_2^{\&prime;}}{\min }\left(\begin{array}{c}{w}_2{(x_2-x_2^{\&prime;})}^2+L\\ \underset{x_N^{\&prime;}}{\min }(w_N{(x_N-x_N^{\&prime;})}^2+f(x_1^{\&prime;},x_2^{\&prime;}L{x}_N^{\&prime;}))\end{array}\right)\end{array}\right)---\left(11\right)$

(11)式的重要含义在于：对于N维加权距离变换，可以通过逐维进行一维加权距离变换求得；该加权快速距离变换算法的复杂度为O(L)，其中L为采样点的总数目；

步骤5，根据多特征驱动距离图以及从模型库中获得的目标的三维模型数据，建立输入的二维图像序列与目标的三维模型之间的试探性同名点特征对应关系，采用如下的两步方法建立二维图像与目标三维模型之间的试探性同名点特征对应关系：

1)2D-to-2D点对应关系的确定

首先将目标的三维模型进行投影得到相应的投影图像并从中提取目标的轮廓，然后利用多特征驱动距离图建立输入图像轮廓与投影图像轮廓之间的2D-to-2D点对应关系，使用步骤4所给出的多特征驱动距离图建立输入图像轮廓与投影图像轮廓之间的2D-to-2D点对应关系，根据多特征驱动距离图的定义，给定采样点x∈G，其多特征驱动距离图值描述的是从该点到目标点集C的最小距离，因此具有相同距离图值的采样点形成一个个封闭和连续的曲面或称为等高面，为了寻找给定采样点x在目标点集C中的最邻近点，只需要从采样点x出发沿着距离图的梯度下降方向向下搜寻，在搜寻路径上遇到的第一个点x′∈C即为采样点x在目标点集C中的最邻近点，可以将寻找两轮廓上同名点的计算复杂度从O(MN)显著降低到O(1)，其中M、N分别为两轮廓上点的数目，

2)2D-to-3D点对应关系的确定

把建立的2D-to-2D点对应关系反向投影到目标的三维模型上去，以建立二维输入图像与目标三维模型间的2D-to-3D点对应关系，采用如下的面片染色方案，即在使用目标的三维模型进行投影时，赋予模型上每个三角面片不同的颜色，然后以此颜色为索引，可以从模型的几千个三角面片中找出对应于投影图像轮廓的三角面片，在找到对应于投影图像轮廓的三角面片后，即可求取投影图像轮廓同名点在模型表面上所对应点的准确三维坐标，设相机坐标系与目标自坐标系之间不存在旋转和平移，若一个三角面片的三个顶点坐标为

则由该面片所定义的平面方程可表示为：

$P=\left(\begin{array}{c}({\stackrel{V}{x}}_{v1}-{\stackrel{V}{x}}_{v3})\&times;({\stackrel{V}{x}}_{v2}-{\stackrel{V}{x}}_{v3})\\ -{\stackrel{V}{x}}_{v3}\&CenterDot;({\stackrel{V}{x}}_{v1}\&times;{\stackrel{V}{x}}_{v2})\end{array}\right)---\left(12\right)$

令

表示投影轮廓同名点对应的模型上的点的三维坐标，则可按下式计算其值：

${\stackrel{V}{x}}_{\mathrm{vg}}=(-P\left(4\right)/{\stackrel{V}{x}}_g\&CenterDot;P(1:3)){\stackrel{V}{x}}_g---\left(13\right)$

步骤6，根据试探性同名点特征对应关系对图像中目标的三维姿态和距离参数进行反演，令

表示目标轮廓图像上某点在像平面坐标系的坐标，

表示该点在相机坐标系中的对应坐标，有

${\stackrel{V}{v}}_i={({\stackrel{V}{x}}_{p,i}^T,f)}^T---\left(14\right)$

定义投影矩阵

$V_i=\frac{{\stackrel{V}{v}}_i\&CenterDot;{\stackrel{V}{v}}_i^T}{{\stackrel{V}{v}}_i^T\&CenterDot;{\stackrel{V}{v}}_i}---\left(15\right)$

目标姿态/距离参数反演模块的特征之一，是使用如下的目标代价函数来对目标的三维姿态和距离参数进行反演：

$e=\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{\left|\right|(I-V_i)(R{\stackrel{V}{x}}_{v,i}+\stackrel{V}{t})\left|\right|}^2---\left(16\right)$

步骤7，将步骤6反演得到的目标的三维姿态和距离参数反馈到步骤5，对二维图像序列与目标的三维模型之间的试探性同名点特征对应关系进行修正更新；

步骤8，重复步骤5～步骤7的过程直至满足迭代停止条件。

由于本发明对三维刚体目标的姿态和距离参数反演，只需要获取目标的单目观测视频，而不需要昂贵复杂的立体成像设备，任何具有光学成像能力的设备，均可以用来作为本系统的视频采集设备，具有不损伤被观测目标、隐蔽性好的优点，同时具有成本低的优点；由于本发明在系统参数设置完毕后，测量过程中不需要用户进行手工干预，不需要用户事先指定观测目标的同名特征，具有自动化程度高的优点。

具体实施方式

下面结合实施例对本发明做详细描述。

基于单目视频的实时三维刚体目标姿态估计与测距方法，包括以下步骤：

步骤1，通过光学观测设备采集目标的观测视频，光学观测设备为单目观测镜头；

步骤2，将采集得到的图像序列送入目标分割模块以获得目标的二值分割图像和轮廓图像，采用的方法是基于水平集的改进型区域竞争方法，分割模型方程如下所示：

$E\left(C\right)=\mathrm{\&alpha;}\&CenterDot;\mathrm{Length}\left(C\right)+c\&CenterDot;\mathrm{Area}\left(\mathrm{inside}\left(C\right)\right)$

$+{\mathrm{\&lambda;}}_1\underset{\mathrm{inside}\left(C\right)}{\&Integral;}|I-c_1|\mathrm{dxdy}$

$+{\mathrm{\&lambda;}}_2\underset{\mathrm{outside}\left(C\right)}{\&Integral;}|I-c_2|\mathrm{dxdy}---\left(1\right)$

其中，I表示待分割图像，C表示零水平集轮廓，c₁表示目标区域的平均灰度，c₂表示背景区域的平均灰度；α，c，λ₁，λ₂均为加权系数，

假定在所获取的观测视频中目标姿态、距离参数的变化是平滑的，相邻的前后两帧之间目标图像不存在跳跃性的变化，这符合大多数刚体目标的运动特征，利用视频图像序列的帧间相关性，图像分割模块将前一帧的目标分割结果做为后一帧目标分割的零水平集轮廓初始位置，从而显著降低了图像分割所耗用的时间，以满足系统实时处理的要求；

步骤3，根据目标的二值图像和轮廓图像，提取目标轮廓点上的特征向量，使用目标观测图像的轮廓特征来完成三维刚体目标姿态估计与测距的任务，在由图像分割模块分割得到被测目标的二值图像和轮廓图像后，由轮廓特征提取模块对目标轮廓上的点进行特征矢量提取，作为算法复杂度和参数测量精度之间的权衡，轮廓特征提取模块对目标轮廓上的点，提取如下的3维特征矢量：

$\stackrel{V}{f}={(x,y,\mathrm{curv})}^T---\left(2\right)$

x、y表示轮廓点在像平面的横纵坐标，curv表示目标轮廓在该点的曲率，

由于标准曲率具有噪声敏感的缺点，本发明采用如下步骤来计算目标轮廓在给定点的曲率：令Mask表示一个半径为r的圆形掩模，对于给定点p，记其灰度值为I(p)；以该p点为掩模中心，计算掩模Mask范围内的图像像素点与掩模中心点的灰度差异，则p点对应的曲率计算公式为：

$\mathrm{curv}=\underset{q\&Element;\mathrm{Mask}}{\mathrm{\&Sigma;}}\mathrm{\&Phi;}(I\left(q\right)-I\left(p\right))---\left(3\right)$

其中，Φ(·)为隶属度模糊函数，方程如下：

$\mathrm{\&Phi;}\left(x\right)=\left\{\begin{array}{cc}1& \mathrm{if}\left|x\right|\&le;t_1\\ \frac{1}{1-t_1}\left(\frac{t_2-\left|x\right|}{t_2}\right)& \mathrm{if}{t}_1<\left|x\right|\&le;t_2\\ 0& \mathrm{if}\left|x\right|>t_2\end{array}\right.---\left(4\right)$

其中，x为自变量，t₁、t₂为阈值，t₁＜t₂。

由以上计算得到的轮廓曲率与标准曲率一样反映了轮廓线的弯曲程度，同时具有很好的噪声鲁棒性与尺度不变性的特点；

步骤4，根据目标的轮廓图像和目标轮廓点上的特征向量，生成多特征驱动距离图，不要求用户事先给定二维输入图像与目标的三维模型之间的同名特征对应关系，而是计算在进行目标三维姿态和距离参数反演的同时逐步确定出二维输入像与目标的三维模型之间正确的同名特征对应关系，即是要根据步骤2图像分割模块得到的目标轮廓图像和步骤3轮廓特征提取模块得到的目标轮廓点的特征矢量生成多特征驱动距离图，多特征驱动距离图是指使用加权多特征距离作为距离度量范数的距离图，其数学描述如下：

令G表示N维特征空间中的均匀采样网格，

表示目标采样点集合，多特征驱动距离图描述的是网格G中的每一点到目标点集合C的最短距离。对采样点x∈G，其距离值定义为：

$\mathrm{MFDM}\left(x\right)=\underset{y}{\min }\left|\right|x-y\left|\right|,\&ForAll;y\&Element;C---\left(5\right)$

其中，距离度量||·||采用的是加权多特征距离范数，

“加权多特征距离范数”是指，设N维特征空间中的两点则该两点之间的加权多特征驱动距离定义为

$\mathrm{MFD}(\stackrel{V}{p},\stackrel{V}{q})=w_1{(x_1-x_1^{\&prime;})}^2+w_2{(x_2-x_2^{\&prime;})}^2+L{w}_N{(x_N-x_N^{\&prime;})}^2---\left(6\right)$

其中w₁L w_N为各特征维的加权系数，公式(6)所定义的加权多特征驱动距离提供了一种可以容纳任意个数的特征及其组合方式的算法框架，在加权多特征驱动距离框架下，可以添加任何有效的特征，并通过调整各特征维的加权系数来提高同名点的匹配精度，这在特征异质的情况下是非常必要的，

多特征驱动距离图是采用如下的加权快速距离变换来生成多特征驱动距离图：

1)一维情况下的加权快速距离变换

在一维情况下，加权距离变换定义式为：

$D_f\left(x\right)=\underset{q\&Element;C}{\min }(w\&CenterDot;||x-q||+f\left(q\right))---\left(7\right)$

其中f(·)为初始距离函数，C为其定义域，w为标量加权系数。当度量范数采用2范数时，上式可以重写为

$D_f\left(x\right)=\underset{q\&Element;C}{\min }(w\&CenterDot;{(x-q)}^2+f\left(q\right))---\left(8\right)$

(8)式可以看成是以x为自变量的二次函数，其形状为抛物线。从(8)式可以得出如下结论：距离变换D_f(x)等于所有不同q值的抛物线y＝w·(x-q)²+f(q)的下包迹的值，

由于抛物线方程已知，要求取其下包迹只需要再知道各抛物线之间的交点坐标。对于q＝q₁和q＝q₂的两条抛物线，其交点坐标为

$s=\frac{f\left(q_1\right)-f\left(q_2\right)}{2w(q_1-q_2)}+q_1+q_2---\left(9\right)$

2)N维情况下的加权快速距离变换

在N维情况下，加权距离变换定义式为

$D_f(x_1,x_{2}L{x}_N)=\underset{(x_1^{\&prime;},x_2^{\&prime;}L{x}_N^{\&prime;})\&Element;C}{\min }\left(\begin{array}{c}{w}_1{(x_1-x_1^{\&prime;})}^2+w_2{(x_2-x_2^{\&prime;})}^2+L\\ w_N{(x_N-x_N^{\&prime;})}^2+f(x_1^{\&prime;},x_2^{\&prime;}L{x}_N^{\&prime;})\end{array}\right)---\left(10\right)$

其中w₁L w_N为各维的加权系数。上式可重写为

$D_f(x_1,x_{2}L{x}_N)=\underset{x_1^{\&prime;}}{\min }\left(\begin{array}{c}{w}_1{(x_1-x_1^{\&prime;})}^2+\\ \underset{x_2^{\&prime;}}{\min }\left(\begin{array}{c}{w}_2{(x_2-x_2^{\&prime;})}^2+L\\ \underset{x_N^{\&prime;}}{\min }(w_N{(x_N-x_N^{\&prime;})}^2+f(x_1^{\&prime;},x_2^{\&prime;}L{x}_N^{\&prime;}))\end{array}\right)\end{array}\right)---\left(11\right)$

(11)式的重要含义在于：对于N维加权距离变换，可以通过逐维进行一维加权距离变换求得。该加权快速距离变换算法的复杂度为O(L)，其中L为采样点的总数目；

步骤5，根据多特征驱动距离图以及从模型库中获得的目标的三维模型数据，建立输入的二维图像序列与目标的三维模型之间的试探性同名特征对应关系，不要求用户事先给定二维输入图像与目标三维模型间的同名特征对应关系，而是在进行目标三维姿态和距离参数反演的同时，逐步确定出二维输入像与目标的三维模型之间正确的同名特征对应关系，自动化程度高，

“建立输入的二维图像与目标三维模型之间的试探性同名点特征对应关系”中的“试探性”是指，在算法迭代初期，所建立输入的二维图像与目标三维模型之间的同名点特征对应关系可能并不完全准确，而是处在算法收敛半径所允许的误差邻域中，需要在后续迭代过程中进行逐步更新修正；

“建立输入的二维图像与目标三维模型之间的试探性同名点特征对应关系”中的“点特征”是指，本模块建立的特征对应关系是输入图像中目标轮廓上的像点与目标三维模型上三维实体点之间的对应关系，

采用如下的两步方法建立二维图像与目标三维模型之间的同名点特征对应关系：

1)2D-to-2D点对应关系的确定

首先将目标的三维模型进行投影得到相应的投影图像并从中提取目标的轮廓，然后利用多特征驱动距离图建立输入图像轮廓与投影图像轮廓之间的2D-to-2D点对应关系，使用步骤4“多特征驱动距离图生成模块”所给出的多特征驱动距离图建立输入图像轮廓与投影图像轮廓之间的2D-to-2D点对应关系，根据多特征驱动距离图的定义，给定采样点x∈G，其多特征驱动距离图值描述的是从该点到目标点集C的最小距离，因此具有相同距离图值的采样点形成一个个封闭和连续的曲面或称为等高面，为了寻找给定采样点x在目标点集C中的最邻近点，只需要从采样点x出发沿着距离图的梯度下降方向向下搜寻，在搜寻路径上遇到的第一个点x′∈C即为采样点x在目标点集C中的最邻近点。与通常的线性搜索相比，利用多特征驱动距离图的帮助，可以将寻找两轮廓上同名点的计算复杂度从O(MN)显著降低到O(1)，其中M、N分别为两轮廓上点的数目，

2)2D-to-3D点对应关系的确定

把建立的2D-to-2D点对应关系反向投影到目标的三维模型上去，以建立二维输入图像与目标三维模型间的2D-to-3D点对应关系，采用如下的面片染色方案，即在使用目标的三维模型进行投影时，赋予模型上每个三角面片不同的颜色，然后以此颜色为索引，可以从模型的几千个三角面片中找出对应于投影图像轮廓的三角面片，在找到对应于投影图像轮廓的三角面片后，即可求取投影图像轮廓同名点在模型表面上所对应点的准确三维坐标，设相机坐标系与目标自坐标系之间不存在旋转和平移，若一个三角面片的三个顶点坐标为

则由该面片所定义的平面方程可表示为：

$P=\left(\begin{array}{c}({\stackrel{V}{x}}_{v1}-{\stackrel{V}{x}}_{v3})\&times;({\stackrel{V}{x}}_{v2}-{\stackrel{V}{x}}_{v3})\\ -{\stackrel{V}{x}}_{v3}\&CenterDot;({\stackrel{V}{x}}_{v1}\&times;{\stackrel{V}{x}}_{v2})\end{array}\right)---\left(12\right)$

令表示投影轮廓同名点对应的模型上的点的三维坐标，则可按下式计算其值：

${\stackrel{V}{x}}_{\mathrm{vg}}=(-P\left(4\right)/{\stackrel{V}{x}}_g\&CenterDot;P(1:3)){\stackrel{V}{x}}_g---\left(13\right)$

步骤6，根据试探性同名特征对应关系对图像中目标的三维姿态和距离参数进行反演，令

表示目标轮廓图像上某点在像平面坐标系的坐标，表示该点在相机坐标系中的对应坐标，有

${\stackrel{V}{v}}_i={({\stackrel{V}{x}}_{p,i}^T,f)}^T---\left(14\right)$

定义投影矩阵

$V_i=\frac{{\stackrel{V}{v}}_i\&CenterDot;{\stackrel{V}{v}}_i^T}{{\stackrel{V}{v}}_i^T\&CenterDot;{\stackrel{V}{v}}_i}---\left(15\right)$

目标姿态/距离参数反演模块的特征之一，是使用如下的目标代价函数来对目标的三维姿态和距离参数进行反演：

$e=\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{\left|\right|(I-V_i)(R{\stackrel{V}{x}}_{v,i}+\stackrel{V}{t})\left|\right|}^2---\left(16\right)$

步骤7，将步骤6反演得到的目标的三维姿态和距离参数反馈到步骤5，对二维图像序列与目标的三维模型之间的试探性同名特征对应关系进行修正更新；

步骤8，重复步骤5～步骤7的过程直至满足迭代停止条件。

Claims (1)

1.基于单目视频的实时三维刚体目标姿态估计与测距方法，其特征在于：包括以下步骤：

步骤1，通过光学观测设备采集目标的观测视频，光学观测设备为单目观测镜头；

步骤2，将采集得到的图像序列送入目标分割模块以获得目标的二值分割图像和轮廓图像，采用的方法是基于水平集的改进型区域竞争方法，分割模型方程如下所示：

$E\left(C\right)=\mathrm{\&alpha;}\&CenterDot;\mathrm{Length}\left(C\right)+c\&CenterDot;\mathrm{Area}\left(\mathrm{inside}\left(C\right)\right)$

$+{\mathrm{\&lambda;}}_1\underset{\mathrm{inside}\left(C\right)}{\&Integral;}|I-c_1|\mathrm{dxdy}$

$+{\mathrm{\&lambda;}}_2\underset{\mathrm{outside}\left(C\right)}{\&Integral;}|I-c_2|\mathrm{dxdy}---\left(1\right)$

其中，I表示待分割图像，C表示零水平集轮廓，c₁表示目标区域的平均灰度，c₂表示背景区域的平均灰度；α，c，λ₁，λ₂均为加权系数；

步骤3，根据目标的二值图像和轮廓图像，提取目标轮廓点上的特征向量，由轮廓特征提取模块对目标轮廓上的点进行特征矢量提取，轮廓特征提取模块对目标轮廓上的点，提取如下的三维特征矢量：

$\stackrel{V}{f}={(x,y,\mathrm{curv})}^T---\left(2\right)$

x、y表示轮廓点在像平面的横纵坐标，curv表示目标轮廓在该点的曲率，采用如下步骤来计算目标轮廓在给定点的曲率：令Mask表示一个半径为r的圆形掩模，对于给定点p，记其灰度值为I(p)；以该p点为掩模中心，计算掩模Mask范围内的图像像素点与掩模中心点的灰度差异，则p点对应的曲率计算公式为：

$\mathrm{curv}=\underset{q\&Element;\mathrm{Mask}}{\mathrm{\&Sigma;}}\mathrm{\&Phi;}(I\left(q\right)-I\left(p\right))---\left(3\right)$

其中，Φ(·)为隶属度模糊函数，方程如下：

$\mathrm{\&Phi;}\left(x\right)=\left\{\begin{array}{cc}1& \mathrm{if}\left|x\right|\&le;t_1\\ \frac{1}{1-t_1}\left(\frac{t_2-\left|x\right|}{t_2}\right)& \mathrm{if}{t}_1<\left|x\right|\&le;t_2\\ 0& \mathrm{if}\left|x\right|>t_2\end{array}\right.---\left(4\right)$

其中，x为自变量，t₁、t₂为阈值，t₁＜t₂；

步骤4，根据目标的轮廓图像和目标轮廓点上的特征向量，生成多特征驱动距离图，多特征驱动距离图是指使用加权多特征距离作为距离度量范数的距离图，其数学描述如下：

令G表示N维特征空间中的均匀采样网格，

表示目标采样点集合，多特征驱动距离图描述的是网格G中的每一点到目标点集合C的最短距离，对采样点x∈G，其距离值定义为：

$\mathrm{MFDM}\left(x\right)=\underset{y}{\min }\left|\right|x-y\left|\right|,\&ForAll;y\&Element;C---\left(5\right)$

其中，距离度量||·||采用的是加权多特征距离范数，

“加权多特征距离范数”是指，设N维特征空间中的两点

则该两点之间的加权多特征驱动距离定义为

$\mathrm{MFD}(\stackrel{V}{p},\stackrel{V}{q})=w_1{(x_1-x_1^{\&prime;})}^2+w_2{(x_2-x_2^{\&prime;})}^2+L{w}_N{(x_N-x_N^{\&prime;})}^2---\left(6\right)$

其中w₁L w_N为各特征维的加权系数，公式(6)所定义的加权多特征驱动距离提供了一种可以容纳任意个数的特征及其组合方式的算法框架。在加权多特征驱动距离框架下，可以添加任何有效的特征，并通过调整各特征维的加权系数来提高同名点的匹配精度，这在特征异质的情况下是非常必要的，

多特征驱动距离图是采用如下的加权快速距离变换来生成多特征驱动距离图：

1)一维情况下的加权快速距离变换

在一维情况下，加权距离变换定义式为：

$D_f\left(x\right)=\underset{q\&Element;C}{\min }(w\&CenterDot;||x-q||+f\left(q\right))---\left(7\right)$

其中f(·)为初始距离函数，C为其定义域，w为标量加权系数。当度量范数采用2范数时，上式可以重写为

$D_f\left(x\right)=\underset{q\&Element;C}{\min }(w\&CenterDot;{(x-q)}^2+f\left(q\right))---\left(8\right)$

(8)式可以看成是以x为自变量的二次函数，其形状为抛物线。从(8)式可以得出如下结论：距离变换D_f(x)等于所有不同q值的抛物线y＝w·(x-q)²+f(q)的下包迹的值，

由于抛物线方程已知，要求取其下包迹只需要再知道各抛物线之间的交点坐标。对于q＝q₁和q＝q₂的两条抛物线，其交点坐标为

$s=\frac{f\left(q_1\right)-f\left(q_2\right)}{2w(q_1-q_2)}+q_1+q_2---\left(9\right)$

2)N维情况下的加权快速距离变换

在N维情况下，加权距离变换定义式为

$D_f(x_1,x_{2}L{x}_N)=\underset{(x_1^{\&prime;},x_2^{\&prime;}L{x}_N^{\&prime;})\&Element;C}{\min }\left(\begin{array}{c}{w}_1{(x_1-x_1^{\&prime;})}^2+w_2{(x_2-x_2^{\&prime;})}^2+L\\ w_N{(x_N-x_N^{\&prime;})}^2+f(x_1^{\&prime;},x_2^{\&prime;}L{x}_N^{\&prime;})\end{array}\right)---\left(10\right)$

其中w₁L w_N为各维的加权系数。上式可重写为

$D_f(x_1,x_{2}L{x}_N)=\underset{x_1^{\&prime;}}{\min }\left(\begin{array}{c}{w}_1{(x_1-x_1^{\&prime;})}^2+\\ \underset{x_2^{\&prime;}}{\min }\left(\begin{array}{c}{w}_2{(x_2-x_2^{\&prime;})}^2+L\\ \underset{x_N^{\&prime;}}{\min }(w_N{(x_N-x_N^{\&prime;})}^2+f(x_1^{\&prime;},x_2^{\&prime;}L{x}_N^{\&prime;}))\end{array}\right)\end{array}\right)---\left(11\right)$

(11)式的重要含义在于：对于N维加权距离变换，可以通过逐维进行一维加权距离变换求得。该加权快速距离变换算法的复杂度为O(L)，其中L为采样点的总数目；

步骤5，根据多特征驱动距离图以及从模型库中获得的目标的三维模型数据，建立输入的二维图像序列与目标的三维模型之间的试探性同名点特征对应关系，采用如下的两步方法建立二维图像与目标三维模型之间的试探性同名点特征：

1)2D-to-2D点对应关系的确定

首先将目标的三维模型进行投影得到相应的投影图像并从中提取目标的轮廓，然后利用多特征驱动距离图建立输入图像轮廓与投影图像轮廓之间的2D-to-2D点对应关系，使用步骤4“多特征驱动距离图生成模块”所给出的多特征驱动距离图建立输入图像轮廓与投影图像轮廓之间的2D-to-2D点对应关系，根据多特征驱动距离图的定义，给定采样点x∈G，其多特征驱动距离图值描述的是从该点到目标点集C的最小距离，因此具有相同距离图值的采样点形成一个个封闭和连续的曲面或称为等高面，为了寻找给定采样点x在目标点集C中的最邻近点，只需要从采样点x出发沿着距离图的梯度下降方向向下搜寻，在搜寻路径上遇到的第一个点x′∈C即为采样点x在目标点集C中的最邻近点，可以将寻找两轮廓上同名点的计算复杂度从O(MN)显著降低到O(1)，其中M、N分别为两轮廓上点的数目，

2)2D-to-3D点对应关系的确定

把建立的2D-to-2D点对应关系反向投影到目标的三维模型上去，以建立二维输入图像与目标三维模型间的2D-to-3D点对应关系，采用如下的面片染色方案，即在使用目标的三维模型进行投影时，赋予模型上每个三角面片不同的颜色，然后以此颜色为索引，可以从模型的几千个三角面片中找出对应于投影图像轮廓的三角面片，在找到对应于投影图像轮廓的三角面片后，即可求取投影图像轮廓同名点在模型表面上所对应点的准确三维坐标，设相机坐标系与目标自坐标系之间不存在旋转和平移，若一个三角面片的三个顶点坐标为

则由该面片所定义的平面方程可表示为：

$P=\left(\begin{array}{c}({\stackrel{V}{x}}_{v1}-{\stackrel{V}{x}}_{v3})\&times;({\stackrel{V}{x}}_{v2}-{\stackrel{V}{x}}_{v3})\\ -{\stackrel{V}{x}}_{v3}\&CenterDot;({\stackrel{V}{x}}_{v1}\&times;{\stackrel{V}{x}}_{v2})\end{array}\right)---\left(12\right)$

令

表示投影轮廓同名点对应的模型上的点的三维坐标，则可按下式计算其值：

${\stackrel{V}{x}}_{\mathrm{vg}}=(-P\left(4\right)/{\stackrel{V}{x}}_g\&CenterDot;P(1:3)){\stackrel{V}{x}}_g---\left(13\right)$

步骤6，根据试探性同名特征对应关系对图像中目标的三维姿态和距离参数进行反演，令表示目标轮廓图像上某点在像平面坐标系的坐标，

表示该点在相机坐标系中的对应坐标，有

${\stackrel{V}{v}}_i={({\stackrel{V}{x}}_{p,i}^T,f)}^T---\left(14\right)$

定义投影矩阵

$V_i=\frac{{\stackrel{V}{v}}_i\&CenterDot;{\stackrel{V}{v}}_i^T}{{\stackrel{V}{v}}_i^T\&CenterDot;{\stackrel{V}{v}}_i}---\left(15\right)$

目标姿态/距离参数反演模块的特征之一，是使用如下的目标代价函数来对目标的三维姿态和距离参数进行反演：

$e=\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{\left|\right|(I-V_i)(R{\stackrel{V}{x}}_{v,i}+\stackrel{V}{t})\left|\right|}^2---\left(16\right)$

步骤7，将步骤6反演得到的目标的三维姿态和距离参数反馈到步骤5，对二维图像序列与目标的三维模型之间的试探性同名特征对应关系进行修正更新；

步骤8，重复步骤5～步骤7的过程直至满足迭代停止条件。