光学 精密工程  2018, Vol.26 Issue (4): 834-842   PDF    
测量误差不确定性加权的立体视觉位姿估计目标函数
霍炬1,2, 张贵阳1, 崔家山3, 杨明1     
1. 哈尔滨工业大学 航天学院, 黑龙江 哈尔滨 150001;
2. 哈尔滨工业大学 电气工程与自动化学院, 黑龙江 哈尔滨 150001;
3. 中国航天科工集团8357研究所, 天津 300308
摘要: 针对视觉测量过程中配合目标特征点在成像时灰度模式的各向异性且非独立同分布对位姿估计求解的影响,建立了基于特征点测量误差不确定性加权的位姿估计目标函数。利用协方差矩阵描述特征点的方向不确定性,分析成像特征点的不确定性对目标函数的作用权重,并将特征点测量误差的不确定性融入到空间共线性误差函数中,进而构造了基于成像特征点测量误差不确定性加权的新目标函数。该方法适应于特征点测量误差具有不同程度的方向不确定性的情况,最后通过广义正交迭代算法对该目标函数进行迭代优化求解。实验表明,当测量空间为2 300 mm×1 400 mm×1 400 mm,采用新目标函数得到靶标重投影后图像坐标的最大误差不超过0.11 pixel,立体视觉系统对标准杆的相对测量精度优于0.01%。所得的结果验证了新目标函数的位姿估计精度高、稳定性强,适用于实际的工程应用。
关键词: 立体视觉      不确定性      误差加权      目标函数      位姿估计     
A objective function with measuring error uncertainty weighted for pose estimation in stereo vision
HUO Ju1,2 , ZHANG Gui-yang1 , CUI Jia-shan3 , YANG Ming1     
1. School of Astronautics, Harbin Institute of Technology, Harbin 150001, China;
2. School of Electrical Engineering and Automation, Harbin Institute of Technology, Harbin 150001, China;
3. The 8357 Institute, China Aerospace Science & Industry Corp., Tianjin 300308, China
*Corresponding author: YANG Ming, E-mail:myang_csc@163.com
Abstract: In order to investigate the effect of anisotropic and correlated non-identical gray distributions of feature points on pose estimation, a novel objective function with error uncertainty weighted of feature points was proposed. In the method, the inverse covariance matrix was utilized to describe the directional uncertainty of feature points, and weighted contribution of uncertainty to the error objective function was analysed. By transforming the uncertainty into a covariance-weighted data space, the novel objective function was constructed, which was suitable for actual applications. Finally, the optimized solution to the novel objective function was obtained via generalized orthogonal iterative algorithm. The simulation and practical experiments show that the maximum error of re-projection image coordinates of the target is less than 0.11 pixels and the measurement relative error for standard gauges is superior to 0.01% within the space 2300 mm×1400 mm×1400 mm. The results verify the high accuracy and strong robustness of the proposed approach, and should therefore have potential for engineering applications.
Key words: stereo-vision     uncertainty     weighted measuring error     objective function     pose estimation    
1 引言

位姿估计是计算摄像学,摄影测量学和计算机图形学等学科领域的一个重要问题[1-3]。在视觉测量中,位姿估计是指利用目标在世界坐标系下的坐标与其经透视投影在相机成像平面上的投影坐标,利用多个点完成目标像点匹配。采用建立的目标投影成像模型对目标的相对运动状态进行求解,通常的做法是构造最小化的目标函数,并通过循环迭代求得最优解[4-5]

在当前的视觉位姿估计迭代优化求解中,一般是以摄像机光心与特征目标及成像点共线作为约束条件来构造目标函数。例如被广泛应用的图像空间误差目标函数[6],建立以空间点在成像平面上的像点坐标与经摄像机模型重投影后像点坐标误差向量的L2范数为优化目标函数。物空间误差目标函数[7]则建立包含空间点全部投影信息的共线性误差方程,将位姿估计问题转化为经摄像机重建后的空间点与原始点之间误差的最小化问题。这两种方法考虑的成像误差均呈高斯均匀分布,所得到的位姿解是在假设误差高斯均匀分布条件下的极大似然最优解,具有统计意义。然而,在实际摄像机成像时,由于成像传感器的特性、环境条件、运动目标本身等因素的影响,特征点处的测量噪声不符合高斯均匀分布,具有方向不确定性,通过循环迭代求解目标函数得到的位姿解只适应当前条件,而不是实际应用中的最优解[8-9]。部分研究人员采用归一化目标函数[10]来研究基于视觉的位姿估计问题,利用纯数学概念求解位姿,在一定程度上减小了成像误差的影响。文献[11]提出了一种基于尺度加权的优化方法。该方法假设目标像点处的噪声同向非独立同分布,且与目标点在摄像机坐标系下的垂直方向大小成正比。但所建立的目标函数考虑了目标像点噪声分布的方差不同,即尺度不确定性,没有考虑到像点误差的方向不确定性。空间目标点成像时的灰度模式在各个像点附近的分布差异很大,灰度分布的差异性直接传递到了像点提取误差上,使得像点误差各向异性且非独立同分布。采用传统方法构建的最小化目标函数已不适用于这种实际情况下的像点误差分布情况,所得到的优化位姿解也不再是全局最优解。因此,需要建立一种符合实际应用情况的最小化目标函数来估计位姿。

本文针对视觉测量过程中配合目标成像特征点处的测量噪声的各向异性且非独立同分布问题,参照文献[12-14]将成像特征点处的测量噪声误差称为特征点的不确定性。在建立目标函数时充分考虑成像特征的噪声分布情况,建立了成像特征点处的实际误差分布模型。该模型适应于不同程度的误差不确定性,并将该误差不确定性融入到新的目标函数中,其约束性强、冗余信息量大。最后,通过广义正交迭代算法对建立的新目标函数进行迭代优化求解,得到精度高、稳定性强,并适应于实际情况的最优位姿解。

2 问题分析 2.1 位姿估计的目标函数

位姿估计在视觉测量中也被看作目标特征点在相关坐标系下的坐标与摄像机坐标系下的坐标变换问题[15-16]。然而,目标经透视投影在摄像机成像平面上的投影坐标与理想坐标存在偏差,图 1所示为立体摄像机成像过程的共线性误差。

图 1 立体视觉测量共线性误差 Fig.1 Collinear error in stereo-vision measurement

图 1中,Ow-XwYwZw为世界坐标系,是整个系统的参考坐标系,最终计算的点的坐标都要转换到该坐标系下;Ocl-XlcYlcZlcOcr-XrcYrcZrc为摄像机坐标系; Piw的坐标是(Xiw, Yiw, Ziw),在相应的摄像机坐标系下的坐标为PilPir。目标特征点在摄像机坐标系与世界坐标系中的坐标有如下关系:

(1)

式中:

则在立体视觉测量系统中,世界坐标系下的空间点Piw在左、右相机坐标系下的关系为:

(2)

其中Rl, TlRr, Tr分别为外部参数。消去Piw可得两台相机间的相对位置关系:

(3)

给定n幅图像,就可以求出nRl(Rr)和Tl(Tr)。理论上,这nRl(Rr)和Tl(Tr)完全相同,且摄像机光心Ocl(Ocr)经成像平面点pil(pir)和相机坐标系下的空间点pic分别是对应共线的。但由于相机镜头畸变和特征点成像的灰度分布不确定性等因素的影响,pic不在理想成像视线OcPi上,存在偏差d。位姿估计就是要寻找旋转矩阵R和平移向量T使得:

(4)

其中γi=pi-(RPi+T), 为综合误差。最后通过迭代优化算法求解最小化目标函数,得到最优的位姿参数RT

2.2 特征点测量误差的不确定性分析

在立体视觉实际测量过程中,目标特征点的灰度模式在不同成像点处的分布是不同的。像点提取时会引入灰度分布的方向性,体现在像平面上的u方向和v方向。将像点提取误差的这种各向异性且非独立同分布情况通过成像特征点测量误差的不确定性进行描述,并使用像点测量误差的协方差逆矩阵[12-14]对像点测量误差的不确定性进行建模:

(5)

式中:D为像点测量误差的协方差矩阵,▽gu和▽gv分别是图像u方向和v方向上的梯度值,Ω是以成像特征点为中心的(圆形或椭圆形)区域,是区域Ω内的像素灰度之和。

像点测量误差协方差矩阵的逆矩阵D-1决定了一个圆心在xi=(ui, vi)T处的不确定性椭圆区域,椭圆的长、短轴ab表示该特征点xi处的不确定性,长、短轴ab相对uv方向的角度为不确定性方向。图 2中展示了3种不同类型的像点测量误差不确定性。

图 2 椭圆特征点的不确定性示意图 Fig.2 Schematic diagram of ellipsoid point uncertainty

当不确定性椭圆区域如图 2(a)a/b=1时,则表示特征点的不确定性各向同性,在u, v方向上不相关,两个方向上的不确定性大小相等且相对较小,其协方差阵为D=diag(σ, σ),σ表示误差的方差值。图 2(b)表示特征点的不确定性具有方向性,但在u, v方向上不相关。图 2(c)则说明特征点的不确定性具有方向性,且在uv方向上是相关的。

3 不确定性误差加权的新目标函数 3.1 误差加权目标函数

根据2.2节中的分析,若像点提取灰度分布如图 2(a)所示,按照传统方式构造目标函数时,迭代优化后能够得到全局最优的位姿解。而如果是图 2(b)2(c)两种情况,则不能采用传统方式构造目标函数,需要考虑实际情况下像点测量误差的不确定性。因此,通过仿射变换矩阵F把像点测量误差的不确定性融入到以重投影误差构造的目标函数中。由像点测量误差协方差矩阵D的结构可知,D是半正定对称的,可进行奇异值分解(Singular Value Decomposition, SVD),得到像点测量误差协方差矩阵的逆矩阵为:

(6)

利用协方差矩阵定义矩阵:

(7)
(8)

其中:F为3×3仿射变换矩阵,αβ是沿图像坐标两个方向的倾角,由σ1σ2决定。通过F矩阵能将成像特征点和重投影像点坐标变换到不确定性加权协方差数据空间中。设像平面上成像特征点的坐标为xi,重投影像点坐标分为,经过F变换后得到的点分别为xi′与,变换过程如下:

(9)

成像特征点经过F变换后,σ1σ2是成像特征点的不确定性。可以看出,成像特征点的不确定性与它在误差函数中所占的权重成反比例,即不确定性越大,权值越小,对目标函数的贡献越小;反之,不确定性越小,权值越大,对目标函数的贡献性越大。经过以上仿射变换后可建立具有统计意义的加权不确定性重投影误差:

(10)

式(10)表示,像平面原始数据的加权不确定性经F矩阵变换后在加权协方差数据空间中对重投影误差进行加权处理,以加权不确定性仿射重投影误差构造迭代优化目标函数,建立的目标函数形式如下:

(11)

这种形式的目标函数充分考虑了成像特征点处的灰度分布不确定性,经仿射变换后,原始数据空间中成像特征点的误差不确定性被相应的转换到加权协方差数据空间中,建立的目标函数包含了原始像点的加权不确定性。

把式(11)中图像空间的误差方程转换到欧氏空间中,以左相机坐标系作为立体视觉测量摄像机的测量坐标系。如图 1所示,空间中的点Pic(Piw)在左像机光心Ocl过空间点与成像平面上像点pil的视线上的投影点为Pilc,有Pilc=VilPic。设Pi投影到摄像机的归一化图像平面得到的像点坐标为pilpir,则沿着Oclpil视线方向的投影矩阵为:

(12)

同理,沿着Ocrpir视线方向的投影矩阵为:

(13)

设共有N个特征点Pi,并设,则相机ψ获取的特征点Pi的目标空间共线性误差可表示为:

(14)

进而建立如下所示立体视觉测量系统的新目标误差函数:

(15)

该目标函数是基于统一参考坐标系下的所有目标特征点空间共线性误差。相对于单个摄像机,立体视觉测量摄像机通过相机间的固定相对关系约束, 把所有摄像机获得的目标特征信息统一进行优化,既增强了约束性又增加了冗余信息,经优化得到的位姿解是特征点误差加权后的全局最优解。

3.2 目标函数迭代优化求解

基于测量误差不确定性加权的新目标函数求解步骤如表 1所示。

表 1 新目标函数求解过程 Tab. 1 Solving procedure of novel objective function
4 实验结果与分析 4.1 仿真实验与结果分析

为说明构造的新目标函数的有效性,在Matlab2013a开发环境下进行仿真实验,模拟参数相同的两个摄像机:图像分辨率为1 280×1 024 pixel,焦距f=35 mm,像元大小为du×dv=7.2 μm×7.2 μm,不考虑镜头畸变。以左摄像机为参考坐标系,按照均匀分布的方式,在[-1.15 1.15]×[-0.7 0.7]×[1 2.4] m3的空间内随机生成30个目标点。用椭圆率r=σ1/σ2表示加入图像中噪声不确定性的大小,σ1σ2表示椭圆噪声不确定区域的长轴和短轴,不确定性方向在0~180°内随机选取。实验中,姿态误差eR和位置相对误差eT分别为[17]

(16)
(17)

首先,考察图像椭圆噪声不同程度的方向不确定性对立体视觉位姿估计结果的影响。令图像椭圆噪声的不确定性r从1逐渐变化到15,其中σ2=0.1不变,σ1从0.1变化到1.5。当椭圆率r=1时,噪声为各向同性且独立同分布,此时像点误差对目标函数的权值相同。当椭圆率为r≠1时,噪声变各向异性且非独立同分布,此时像点误差对目标函数的权值根据σ1σ2确定。通过广义正交迭代算法对5种目标函数依次求解,结果如图 3所示。

图 3 位姿估计误差与椭圆噪声不确定性的关系 Fig.3 Estimation error related to elliptical noise

图 3能够看出,随着椭圆噪声不确定性的增大,基于不同目标函数的估计误差也逐渐增大。对比图 3(a)3(b)可以看出,新目标函数所得结果的精度最高,同时误差曲线斜率较小,相对稳定性较强,表明新目标函数可以适应于不同程度的图像噪声不确定性。

然后,考察目标特征点数量变化对立体视觉位姿估计结果的影响。目标特征点个数由4个增加到30个,固定像点椭圆噪声的不确定性r=2.2,分别用5种误差函数构造方法验证目标姿态估计误差与目标特征点数量的关系,实验结果如图 4所示。

图 4 位姿估计误差与特征点数量的关系 Fig.4 Estimation error related to point number

图 4可以看出,随着特征点数量的增加,基于各种目标函数的位姿估计精度不断提高。经对比发现,归一化目标函数对噪声具有一定的抑制作用;尺度加权目标函数相对于其它3种传统方法效果较好,而本文方法的精度最高。综上所述,基于特征点误差不确定性加权的目标函数在具有较高稳定性的同时,能够保持较高的位姿估计精度。

4.2 测量实验结果与分析

通过实际测量实验验证提出的基于特征测量误差加权的新目标函数在实际位姿估计应用中的效果。测量试验采用Mikrotron公司EoSensⓐ3CL系列的高速高灵敏度摄像机,型号为MC3010,分辨率为1 280×1 024 pixel,像元大小为0.008 mm/pixel,镜头型号为AF Zoom-Nikkor 24-85mm/1:2.8-4D。

4.2.1 虚拟长方锥体投影实验

在被测对象上不同位置处选定5个参考点进行检测,完成对目标的相对位姿测量。实验中, 为了评估新目标函数得出的位姿估计结果,在被测目标上设置了一个用于直观显示姿态的虚拟长方锥体,其端点分布如图 5所示。

图 5 被测目标空间位置及特征点分布示意图 Fig.5 Schematic diagram of measured target space position and feature point distribution

对虚拟长方锥体进行重投影变换到图像平面上,与被测对象的摆放位姿相比较,能够直观地看出位姿估计的效果。如果位姿估计结果比较精确,虚拟长方锥体的投影能够显示目标在真实场景的实际位姿,如果位姿估计结果不是全局最优解,虚拟长方锥体的投影与目标在真实场景中的位姿显示存在偏差和变形。分别采用物空间误差目标函数和新目标函数进行位姿估计,结果如图 6所示。

图 6 靶标位姿测试结果 Fig.6 Experimental results of target estimation

图 6中,对被测目标在不同位置和姿态下进行实验。由图 6 (c)可以看出,虚拟长方锥体可以很好地反应被测目标的姿态,虚拟控制点的位置直接体现了被测目标的运动方向,从而说明本文目标函数能够很好地应用到实际环境中。对比图 6(b)图 6(c)的中间图像可以看出,采用新目标函数估计位姿重投影得到的虚拟长方锥体顶点的位置方向,相对于物空间误差目标函数估计位姿重投影得到的虚拟长方锥体顶点的位置方向,更适合被测目标的实际场景位姿。

此外,通过进一步对进行位姿估计的参考点进行重投影得到重投影后的图像坐标,并与初始的参考点图像坐标进行对比,在不同位姿下重复进行实验,取40次实验结果进行分析,以最大误差和均方根来说明。结果如表 2所示,靶标重投影后图像坐标误差最大约为0.11 pixels,可以看出新目标函数在实际应用情况下可以得到较高精度的位姿估计结果。

表 2 重投影误差 Tab. 2 Re-projection errors
4.2.2 标准杆测试

为了进一步验证新目标函数方法在立体视觉测量系统中的可靠性,对标准杆长度进行测量验证。图 7为测试所用的两个标准杆,标准杆1的标准值D1=600.174 mm,标准杆2的标准值D2=1 001.595 mm。

图 7 标准杆 Fig.7 Staff gagues

实验中将标准长度杆放在测量视场内的任意位置,使用文献[18]中的方法对立体视觉测量系统进行标定。通过获取内外参数后的摄像机进行图像采集,对杆上的编码点进行识别,建立新目标函数并进行迭代优化求解,实现对该杆实际长度的精确测量,并与杆长的标准值进行对比,重复10组实验,结果如表 3表 4所示。

表 3 标准杆特征点的空间位置坐标测量结果 Tab. 3 Measuring results of feature point spatial coordinates

表 4 标准杆长度测量误差 Tab. 4 Errors of staff gague measurement

表 4显示,标准杆1的测量平均值为600.195 mm,绝对误差为0.021 mm,相对测量误差为0.003%;标准杆2的测量平均值为1 001.681 mm,绝对误差为0.086 mm,相对测量误差为0.009%。实验中两个标准杆的测量值与标准值非常接近,相对测量精度均优于0.01%,验证了本文构建的新目标函数能够准确估计位姿,可以应用于实际测量中。

5 结论

本文提出了基于特征点测量误差不确定性加权的目标函数建立方法。该方法充分考虑成像特征点处的噪声分布情况,将特征点不确定性融入到位姿估计中,建立基于测量误差不确定性加权的新目标函数,并通过广义正交迭代算法进行位姿估计求解。仿真和测试实验结果显示:当测量空间为2 300 mm×1 400 mm×1 400 mm,采用新目标函数得到靶标重投影后图像坐标的最大误差不超过0.11 pixel,立体视觉系统对标准杆的相对测量误差优于0.01%。实验结果表明,新目标函数通过冗余信息增强了系统对噪声的鲁棒性,扩展了视觉测量系统的实际应用范围。

参考文献
[1]
MU J, WANG X, JING F, et al. Analysis of the synchronization error measurement via non-collinear cross-correlation[J]. Laser and Particle Beams, 2015, 33(3): 355-359. DOI:10.1017/S0263034615000221
[2]
霍炬, 杨宁, 杨明. 飞行器仿真测试中合作目标投影光斑的跟踪识别[J]. 光学 精密工程, 2015, 23(8): 2134-2142.
HUO J, YANG N, YANG M. Tracking and recognition of projective spots for cooperation targets in vehicle simulation test[J]. Opt. Precision Eng., 2015, 23(8): 2134-2142. (in Chinese)
[3]
张跃强, 苏昂, 刘海波, 等. 基于多级直线表述和M-估计的三维目标位姿跟踪优化算法[J]. 光学学报, 2015, 35(1): 0115003.
ZHANG Y Q, SU A, LIU H B, et al. Three dimensional rigid objects pose tracking and optimization based on multilevel line representation and M-estimation[J]. Acta Optica Sinica, 2015, 35(1): 0115003. (in Chinese)
[4]
XU D, LI Y F, TAN M. A general recursive linear method and unique solution pattern design for the perspective-n-point problem[J]. Image and Vision Computing, 2008, 26(6): 740-750. DOI:10.1016/j.imavis.2007.08.008
[5]
张梁, 徐锦法, 夏青元, 等. 无人飞行器双目视觉位姿估计算法改进与验证[J]. 上海交通大学学报, 2015, 49(9): 1387-1393.
ZHANG L, XU J F, XIA Q Y, et al. An improvement and verification of position/attitude estimation algorithm based on binocular vision for unmanned aerial vehicle[J]. Journal of Shanghai Jiaotong University, 2015, 49(9): 1387-1393. (in Chinese)
[6]
唐巍, 叶东, 袁峰, 等. 灰色关联分析法在双目视觉测量系统误差分析中的应用[J]. 光学 精密工程, 2013, 21(2): 503-513.
TANG W, YE D, YUAN F, et al. Application of grey incidence method to error analysis of binocular vision measurement system[J]. Opt. Precision Eng., 2013, 21(2): 503-513. (in Chinese)
[7]
李鑫, 龙古灿, 刘进博, 等. 相机位姿估计的加速正交迭代算法[J]. 光学学报, 2015, 35(1): 0115004.
LI X, LONG G C, LIU J B, et al. Accelerative orthogonal iteration algorithm for camera pose estimation[J]. Acta Optica Sinica, 2015, 35(1): 0115004. (in Chinese)
[8]
HARTLEY R I, KAHL F. Global optimization through rotation space search[J]. International Journal of Computer Vision, 2009, 82(1): 64-79. DOI:10.1007/s11263-008-0186-9
[9]
SIGAL L, BLACK M J. Measure locally, reason globally:occlusion-sensitive articulated pose estimation[C]. Proceedings of 2006 IEEE Computer Society Conference on Computer Vision and Pattern Recognition, IEEE, 2006:2041-2048.
[10]
VENTURA J, ARTH C, REITMAYR G, et al.. A minimal solution to the generalized pose-and-scale problem[C]. Proceedings of 2014 IEEE Conference on Computer Vision and Pattern Recognition, IEEE, 2014:422-429.
[11]
LU C P, HAGER G D, MJOLSNESS E. Fast and globally convergent pose estimation from video images[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2000, 22(6): 610-622. DOI:10.1109/34.862199
[12]
ANANDAN P, IRANI M. Factorization with uncertainty[J]. International Journal of Computer Vision, 2002, 49(2-3): 101-116.
[13]
SHI J B, TOMASI C. Good features to track[C]. Proceedings of 1994 IEEE Computer Society Computer Vision and Pattern Recognition, IEEE, 1994:593-600.
[14]
STEELE R M, JAYNES C. Feature uncertainty arising from covariant image noise[C]. Proceedings of IEEE Computer Society Conference on Computer Vision and Pattern Recognition, IEEE, 2005:1063-1070.
[15]
王鹏, 周权通, 孙长库. 多特征点拓扑确定位姿测量算法研究[J]. 红外与激光工程, 2017, 46(5): 0517001.
WANG P, ZHOU Q T, SUN CH K. Study of pose estimation based on multiple feature points topological determination[J]. Infrared and Laser Engineering, 2017, 46(5): 0517001. (in Chinese)
[16]
张振杰, 郝向阳, 程传奇, 等. 基于共面直线迭代加权最小二乘的相机位姿估计[J]. 光学 精密工程, 2016, 24(5): 1168-1175.
ZHANG ZH J, HAO X Y, CHENG CH Q, et al. Iteratively reweighted least squares method for camera pose estimation based on coplanar line correspondences[J]. Opt. Precision Eng., 2016, 24(5): 1168-1175. (in Chinese)
[17]
WANG Y, YUAN F, JIANG H, et al. High precision and fast estimation of position and attitude measurement for space targets[J]. Optik-International Journal for Light and Electron Optics, 2017, 148: 76-84. DOI:10.1016/j.ijleo.2017.08.134
[18]
CUI J SH, HUO J, YANG M. Novel method of calibration with restrictive constraints for stereo-vision system[J]. Journal of Modern Optics, 2016, 63(9): 835-846. DOI:10.1080/09500340.2015.1106602