基于lmi的物空间误差视觉测量全局最优位姿估计

基于lmi的物空间误差视觉测量全局最优位姿估计

1局部误差评估视觉测量是通过分析和计算相机成像系统中记录的图像，测量三维图像中几何参数和运动参数的非闭合三维测量方法。其核心技术包括三角、交会、自标定和光束法平差,其中交会主要是确定像机相对于标靶的位姿。在计算机视觉中,交会过程可以归结为广义的PnP问题,即在摄像机标定的情况下利用已知的空间参考点和其同名像点确定像机的位姿。位姿估计在折线光路测量、三维拼接和相位标靶测量等技术中广泛应用。在视觉测量中,为了提高位姿评估的精度,一般以线性解法作为初始值,然后采用光束法平差进行优化,其优化过程对于高斯噪声具有统计意义,因而对噪声的鲁棒性好。但是,光束法平差应用的是典型的非线性最小二乘法,属于局部优化算法,对初始值要求较高,只有良好的初始值才能使其在正确解处收敛。因此,如果噪声过大,目标函数有多个极值,根据线性解法解得的初始值可能会使光束法平差在局部极小处收敛。为此,Lu提出以物空间误差为目标函数的正交迭代(OI)算法,得到位姿估计全局最优解。但此算法是基于弱透视假设,假设不满足时会导致不稳定。Hartley等人利用分支定界全局优化算法搜索旋转空间求解出位姿的全局最优解,Schweighofer等人提出鲁棒的共面特征位姿评估算法。本文采用以四元数为参数的物空间误差作为目标函数,在考虑旋转矩阵正交化约束的条件下对目标非凸多项式函数进行凸松弛(LMI),可以全局求解最优旋转矩阵和平移矢量。本文算法属于全局优化算法,不需要提供初始值,便可以求得全局最优值,避免了光束法平差陷入局部极小的危险;而且,不需要像OI算法的弱透视假设,对于空间共面和非共面参考点进行位姿评估均适用,具有广义性。2常用的归一化图像坐标系在视觉测量中,一般采用中心透视模型描述成像关系。世界坐标系下的3-D参考点成像为2-D图像坐标可以简单描述为:给定一列非共线的3-D参考点pi=(Xi,Yi,Zi)T,i=1…n,n≥3,相应的摄像机坐标系下的同名像点为qi=(X′i,Y′i,Z′i)T,两坐标间的变换关系可以表示为qi=Rpi+T(1)式中:R为旋转矩阵;具有正交性T;为平移向量。在摄像机标定的情况下,像点在摄像机坐标系下的坐标可以归一化,设摄像机标定的内参数矩阵为K,qi在归一化图像坐标系下的坐标为vi=(x′i,y′i,1)T,则vi=K-1qi(2)光束法平差一般以重投影误差作为目标函数,Lu提出以物空间误差作为目标函数,更方便求解旋转矩阵和平移向量的全局最优解。物空间误差的物理意义和数学表达式可以描述为:如图1所示,对于某一给定3-D参考点B,其在归一化图像坐标系的理想成像点为A。实际提取A点的图像坐标时,不可避免的会有一定的偏差,假设其对应的实际提取像点为A′,摄像机光心C和实际提取像点A′确定的光线方向矢量为l′,光线同物点B和光心C确定的光线方向矢量l不重合。将3-D参考点B向l′方向正交投影,可以得到参考点B到光线l′的距离,称其为物空间误差。设在由C为原点的摄像机坐标系中,根据矩阵投影分析理论,由l′决定的投影算子Vi可以表示为Vi=vivTi/vTivi,对应的正交投影算子为I-Vi。那么物空间误差ei可以表示为ei=[I-Vi][Rpi+T](3)对所有同名参考点和像点,物空间误差为E(R‚Τ)=n∑i=1∥[Ι-Vi][Rpi+Τ]∥2(4)E(R‚T)=∑i=1n∥[I−Vi][Rpi+T]∥2(4)方程(4)是关于T的二次函数,固定旋转矩阵R,可以得到平移矢量T的最优闭合解Τopt=-Λ-1(n∑i=1ΛiRpi)(5)其中,Λ=n∑i=1Λi‚Λi=Ι-Vi。联立式(4)和式(5),并定义算子Qi=R-(∑ΛiR/∑Λi)物空间误差表示为旋转矩阵R的函数,即E(R)=n∑i=1∥ΛiQipi∥2=pΤi(n∑i=1QΤiΛΤiΛiQi)pi(6)R为矩阵,需要将其向量化以求解极值,经过仿射变换,可以将矩阵矢量化。R向量化后为9个参数,为了使参数空间变小,采用四元数[q1,q2,q3,q4]表示法描述R。定义e(q)=[q21,q1q2,q1q3,q1q4,q22,q2q3,q2q4,q23,q3q4,q24],有R=(q21+q22-q23-q242(q2q3+q1q4)2(q2q4-q1q3)2(q2q3-q1q4)q21-q22+q23-q242(q3q4+q1q2)2(q2q4+q1q3)2(q3q4-q1q2)q21-q22-q23-q24)方程(4)中的Rpi经仿射变换可以表示为Rpi=Gie(7)Gi=(Xi0-2Ζi2YiXi2Yi2Ζi-Xi0-XiYi2Ζi0-2Xi-Yi2Xi0Yi2Ζi-YiΖi-2Yi-2Xi0-Ζi02Xi-Ζi2YiΖi)根据方程(6)和(7)式可得E(e)=eΤ(n∑i=1ΜΤiΛΤiΛiΜi)e(8)其中,Mi=Gi-(∑ΛiGi/∑Λi)。又由于旋转矩阵正交,因此需要迫使方程(8)中的四元数[q1,q2,q3,q4]满足正交约束q21+q22+q23+q24=1。并且,由于参考点位于像机前面,令q1>0。因此,求解旋转矩阵R的问题转化为minE(e),使q21+q22+q23+q24=1q1>0(9)3分布式全局优化算法凸函数在数学上的定义为:在凸集S上的函数f(x),如果对于任意x1,x2∈S和0<λ<1,总有f[λx1+(1-λ)x2]≤λf(x1)+(1-λ)f(x2),则称f(x)为在凸集S上的凸函数,凸函数的极值点唯一。非凸函数的极值点不唯一,若采用局部优化算法,会使其陷入局部最小,而不能确保找到全局最优解。Lasserre提出利用线性矩阵不等式对非凸函数在可性区域进行LMI,逼近多项式全局最优解。因此,将非凸多项式全局优化松弛为凸函数,进行线性规划近似求解的极值,就是非凸多项式全局解的近似值。经过多次松弛,可以完全逼近非凸多项式的全局最优解。求解旋转矩阵R的全局最优解,即求解在一定可行区域下四阶非凸多项式的全局最优解。广义非凸多项式优化问题的一般形式可以描述为ming0(x),使gi(x)≥0,i=1…m(10)线性矩阵不等式LMI优化方法是通过将目标函数转化为下届更低的凸函数,将可行区域转化为凸包络,使得多极值非凸优化问题可以转化为凸优化问题求解。定义k阶多项式的基空间υk(x)=[1,x1…xn,x21…x1xn,x22…x2xn,…xkn],多项式g(x)可以表示为g(x)=∑α∈Νgαxα,其中xα=xα11,xα22…xαnn,gα是多项式的系数,即可以通过矩阵将多项式在基空间线性化。对方程(10)进行δ次LMI的一般过程可以归结为:1)采用k阶多项式的基空间线性化目标函数g0(x);2)构造δ-1阶多项式基vδ-1(x),将约束条件gi(x)≥0提升为线性不等式约束gi(x)vδ-1(x)vδ-1(x)T≥0;3)加入线性矩阵不等式约束vδ-1(x)vδ-1(x)T≥0广义非凸多项式的LMI表示为minx∑[g0]αxα,使gi(x)vδ-1(x)vδ-1(x)T≥0vδ-1(x)vδ-1(x)T≥0经过上述线性矩阵不等式的LMI,非凸多项式函数的全局优化问题可以转化为凸函数全局优化问题,可以作为半定规划问题进行求解。具体的计算过程可以采用Henrion发布的GloptiPoly3软件包,利用四元数为参数构建多项式物空间误差目标函数和约束,采用带约束的多项式LMI全局优化算法求解。为了得到可靠的全局最优解,本文实验中均是对多项式进行6次LMI。4实验结果与误差分析摄像机位姿R和T的确定与参考点分布情况和数量有关。利用数值仿真分析LMI算法时,考虑立体参考点与共面参考点为均匀随机分布、LMI算法对噪声的鲁棒性以及参考点的数量对摄像机位姿评估的影响。整个仿真实验采用的图像大小为640×480pixel,摄像机标定的等效距为Fx=Fy=800pixel,主点坐标[x0,y0]=pixel,平移矢量为T=[200,200,1000]pixel。为了描述求解精度,分别定义旋转矩阵和平移矢量的相对误差为Erot和Etrans,Erot(%)=‖Angletrue-Angle‖/‖Angletrue‖Etrans(%)=‖Ttrue-T‖/‖Ttrue‖在仿真实验中,旋转角按均匀分布随机产生,每个结果都是在独立重复200次实验后求平均得到。因此,仿真曲线中的每一个点是在随机产生200次旋转角,对相对误差求平均的结果。一般情况下,用于评估摄像机位姿的参考点随机分布在立体场景。仿真中,参考点在[-10,10]×[-10,10]×[-10,10]均匀随机产生6个点,采用均值为零的高斯噪声模型模拟实际噪声。为了验证算法的有效性,将LMI算法与光束法平差(EPnP+GN)和正交迭代算法(EPnP+OI)进行对比,这两种算法均是以Lepetit提出的线性解法作为初始值驱动。图2描述了图像噪声的方差为0~6时旋转角和平移矢量的相对误差随噪声方差的变化。相对误差与噪声的方差几乎成线性关系,而且LMI算法和EPnP+OI的结果几乎重合,其相对误差都小于EPnP+GN的相对误差。当图像噪声方差为6时,LMI的旋转角和平移矢量相对误差分别约为1.96%和2.38%,EPnP+GN的旋转角和平移相对误差分别为2.93%和3.28%。存在噪声的情况下,考查LMI算法的求解精度随参考点数目的变化。取图像噪声的方差为2,图3描述了相对误差随参考点数的变化情况。参考点数目的增加,对相对误差的减小有一定作用,但是到达一定数目后,这种作用会变得不明显。在参考点数目增加的过程中,LMI算法和EPnP+OI的相对误差仍然是保持相当的接近,并且略小于EPnP+GN的结果,说明了LMI算法求解旋转和平移的稳定性和高精度。考虑参考点为共面时的情况,即Zi=0。LMI算法对噪声的鲁棒性和随参考点数目的变化情况同参考点为立体分布时的情况类似,如图4和图5所示。参考点无论是立体分布还是共面分布,LMI的结果都稍优于EpnP+GN的结果。EPnP+GN属于局部优化算法,迭代过程中,目标函数的下降方向只是向距初始值最近的极小点靠近,如果函数具有多个极值,那么最后的收敛点有可能只是局部极小点,因而其求解目标函数结果的全局最优性在数学上不能得到保证,其精度取决于初始值的精度。而LMI算法和EPnP+OI求解的结果是目标函数的全局最优解。与EPnP+OI算法相比,LMI算法的计算过程没有弱透视模型的假设,完全不需要初始值的情况下达到全局收敛。在视觉测量中,标定并校正后的图像坐标误差一般为弱噪声的情况,EPnP+GN的结果可能会非常接近LMI算法的结果,但是其收敛性在数学上得不到有效的保证。总之,LMI算法不需要初始评估,利用LMI直接求解全局最优,且数学上得到保证。而光束法平差的收敛性在数学上不可证,正交迭代算法是基于弱透视假设前提。而且,LMI算法对于共面和非共面参考点均适用,具有广义性。5摄像机位姿测量算法对平面参考点利用LMI算法确定摄像机位姿进行实验验证。将已知相位分布的平面二维正弦灰度调制条纹图显示在液晶显示器上,对拍摄的条纹通过傅里叶分析方法计算出两个截断的正交相位分布,利用截断正交相位分布提取相应的图像特征点,特征点的像素坐标可以达到亚pixel级。实验中,采用JAICV-A50黑白摄像机拍摄傅里叶条纹,摄像机的分辨率为480×640pixel,采用Philips170S87液晶显示器作为平面标靶,分辨率为1024×1280pixel,显示器的点间距为0.264mm。在进行摄像机位姿评估前需要对摄像机进行标定,采用基于傅里叶条纹分析的摄像机标定方法,标定的摄像机参数见表1。在利用LMI算法确定摄像机位姿实验中,将实验分为两组:第1组,固定参考点数,对液晶显示器上的条纹进行不同方位的多次拍摄,确定算法的稳定性;第2组,固定摄像机的拍摄角度,采用不同数目的参考点评估位姿。实验中,采用重投影误差的方差作为位姿精度的评价标准,且均与光束法平差进行对比。第1组:摄像机在50个不同位姿拍摄条纹,其中的一幅条纹如图6所示,提取400个特征点作为世界坐标系的参考点,利用LMI算法计算旋转矩阵和平移矢量,然后利用成像模型计算重投影误差。50次不同实验重投影误差的方差如图7所示,与光束法平差相比,LMI算法的重投影方差波动性稍小,并且数值很小,说明了LMI算法的精确度很高。第2组:摄像机与显示器近似平行的情况下拍摄一幅图片,以同一个点为世界坐标系的原点,然后提取不同数目的参考点。图8显示了点数分别为6和100时参考点的分布图。利用光束法平差和LMI算法进行位姿评估,其对比结果见表2。在使用傅里叶条纹提取特征点进行位姿评估中,参考点数目过少,两种算法的结果不稳定,随着点数的增加,算法趋于稳定。但是当点数增加到一定数目后,继续增加点数对精度的提高效果不再明显。由表2可见,两者重投影的方差相当接近。这是因为参考点都是在图像消除畸变的基础上提取的,因而其特征点的精度至少在亚pixel级别。当参考点噪声水平不高时,光束法平差结果的精度是很高的。但是,由于第2组实验中选取的图片是在摄像机与显示器近似平行的情况下拍摄的,其亚pixel级提取精度较高,而且选取的原点接近光心,畸变小。因此,当其点数为100时,重投影的方差比第1组实验中点数为400时反而更小。因此,在利用傅里叶条纹评估位姿时,没有