基于lmi的物空間誤差視覺測量全局最優(yōu)位姿估計

基于lmi的物空間誤差視覺測量全局最優(yōu)位姿估計

1局部誤差評估視覺測量是通過分析和計算相機(jī)成像系統(tǒng)中記錄的圖像，測量三維圖像中幾何參數(shù)和運(yùn)動參數(shù)的非閉合三維測量方法。其核心技術(shù)包括三角、交會、自標(biāo)定和光束法平差,其中交會主要是確定像機(jī)相對于標(biāo)靶的位姿。在計算機(jī)視覺中,交會過程可以歸結(jié)為廣義的PnP問題,即在攝像機(jī)標(biāo)定的情況下利用已知的空間參考點(diǎn)和其同名像點(diǎn)確定像機(jī)的位姿。位姿估計在折線光路測量、三維拼接和相位標(biāo)靶測量等技術(shù)中廣泛應(yīng)用。在視覺測量中,為了提高位姿評估的精度,一般以線性解法作為初始值,然后采用光束法平差進(jìn)行優(yōu)化,其優(yōu)化過程對于高斯噪聲具有統(tǒng)計意義,因而對噪聲的魯棒性好。但是,光束法平差應(yīng)用的是典型的非線性最小二乘法,屬于局部優(yōu)化算法,對初始值要求較高,只有良好的初始值才能使其在正確解處收斂。因此,如果噪聲過大,目標(biāo)函數(shù)有多個極值,根據(jù)線性解法解得的初始值可能會使光束法平差在局部極小處收斂。為此,Lu提出以物空間誤差為目標(biāo)函數(shù)的正交迭代(OI)算法,得到位姿估計全局最優(yōu)解。但此算法是基于弱透視假設(shè),假設(shè)不滿足時會導(dǎo)致不穩(wěn)定。Hartley等人利用分支定界全局優(yōu)化算法搜索旋轉(zhuǎn)空間求解出位姿的全局最優(yōu)解,Schweighofer等人提出魯棒的共面特征位姿評估算法。本文采用以四元數(shù)為參數(shù)的物空間誤差作為目標(biāo)函數(shù),在考慮旋轉(zhuǎn)矩陣正交化約束的條件下對目標(biāo)非凸多項(xiàng)式函數(shù)進(jìn)行凸松弛(LMI),可以全局求解最優(yōu)旋轉(zhuǎn)矩陣和平移矢量。本文算法屬于全局優(yōu)化算法,不需要提供初始值,便可以求得全局最優(yōu)值,避免了光束法平差陷入局部極小的危險;而且,不需要像OI算法的弱透視假設(shè),對于空間共面和非共面參考點(diǎn)進(jìn)行位姿評估均適用,具有廣義性。2常用的歸一化圖像坐標(biāo)系在視覺測量中,一般采用中心透視模型描述成像關(guān)系。世界坐標(biāo)系下的3-D參考點(diǎn)成像為2-D圖像坐標(biāo)可以簡單描述為:給定一列非共線的3-D參考點(diǎn)pi=(Xi,Yi,Zi)T,i=1…n,n≥3,相應(yīng)的攝像機(jī)坐標(biāo)系下的同名像點(diǎn)為qi=(X′i,Y′i,Z′i)T,兩坐標(biāo)間的變換關(guān)系可以表示為qi=Rpi+T(1)式中:R為旋轉(zhuǎn)矩陣;具有正交性T;為平移向量。在攝像機(jī)標(biāo)定的情況下,像點(diǎn)在攝像機(jī)坐標(biāo)系下的坐標(biāo)可以歸一化,設(shè)攝像機(jī)標(biāo)定的內(nèi)參數(shù)矩陣為K,qi在歸一化圖像坐標(biāo)系下的坐標(biāo)為vi=(x′i,y′i,1)T,則vi=K-1qi(2)光束法平差一般以重投影誤差作為目標(biāo)函數(shù),Lu提出以物空間誤差作為目標(biāo)函數(shù),更方便求解旋轉(zhuǎn)矩陣和平移向量的全局最優(yōu)解。物空間誤差的物理意義和數(shù)學(xué)表達(dá)式可以描述為:如圖1所示,對于某一給定3-D參考點(diǎn)B,其在歸一化圖像坐標(biāo)系的理想成像點(diǎn)為A。實(shí)際提取A點(diǎn)的圖像坐標(biāo)時,不可避免的會有一定的偏差,假設(shè)其對應(yīng)的實(shí)際提取像點(diǎn)為A′,攝像機(jī)光心C和實(shí)際提取像點(diǎn)A′確定的光線方向矢量為l′,光線同物點(diǎn)B和光心C確定的光線方向矢量l不重合。將3-D參考點(diǎn)B向l′方向正交投影,可以得到參考點(diǎn)B到光線l′的距離,稱其為物空間誤差。設(shè)在由C為原點(diǎn)的攝像機(jī)坐標(biāo)系中,根據(jù)矩陣投影分析理論,由l′決定的投影算子Vi可以表示為Vi=vivTi/vTivi,對應(yīng)的正交投影算子為I-Vi。那么物空間誤差ei可以表示為ei=[I-Vi][Rpi+T](3)對所有同名參考點(diǎn)和像點(diǎn),物空間誤差為E(R?Τ)=n∑i=1∥[Ι-Vi][Rpi+Τ]∥2(4)E(R?T)=∑i=1n∥[I?Vi][Rpi+T]∥2(4)方程(4)是關(guān)于T的二次函數(shù),固定旋轉(zhuǎn)矩陣R,可以得到平移矢量T的最優(yōu)閉合解Τopt=-Λ-1(n∑i=1ΛiRpi)(5)其中,Λ=n∑i=1Λi?Λi=Ι-Vi。聯(lián)立式(4)和式(5),并定義算子Qi=R-(∑ΛiR/∑Λi)物空間誤差表示為旋轉(zhuǎn)矩陣R的函數(shù),即E(R)=n∑i=1∥ΛiQipi∥2=pΤi(n∑i=1QΤiΛΤiΛiQi)pi(6)R為矩陣,需要將其向量化以求解極值,經(jīng)過仿射變換,可以將矩陣矢量化。R向量化后為9個參數(shù),為了使參數(shù)空間變小,采用四元數(shù)[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經(jīng)仿射變換可以表示為Rpi=Gie(7)Gi=(Xi0-2Ζi2YiXi2Yi2Ζi-Xi0-XiYi2Ζi0-2Xi-Yi2Xi0Yi2Ζi-YiΖi-2Yi-2Xi0-Ζi02Xi-Ζi2YiΖi)根據(jù)方程(6)和(7)式可得E(e)=eΤ(n∑i=1ΜΤiΛΤiΛiΜi)e(8)其中,Mi=Gi-(∑ΛiGi/∑Λi)。又由于旋轉(zhuǎn)矩陣正交,因此需要迫使方程(8)中的四元數(shù)[q1,q2,q3,q4]滿足正交約束q21+q22+q23+q24=1。并且,由于參考點(diǎn)位于像機(jī)前面,令q1>0。因此,求解旋轉(zhuǎn)矩陣R的問題轉(zhuǎn)化為minE(e),使q21+q22+q23+q24=1q1>0(9)3分布式全局優(yōu)化算法凸函數(shù)在數(shù)學(xué)上的定義為:在凸集S上的函數(shù)f(x),如果對于任意x1,x2∈S和0<λ<1,總有f[λx1+(1-λ)x2]≤λf(x1)+(1-λ)f(x2),則稱f(x)為在凸集S上的凸函數(shù),凸函數(shù)的極值點(diǎn)唯一。非凸函數(shù)的極值點(diǎn)不唯一,若采用局部優(yōu)化算法,會使其陷入局部最小,而不能確保找到全局最優(yōu)解。Lasserre提出利用線性矩陣不等式對非凸函數(shù)在可性區(qū)域進(jìn)行LMI,逼近多項(xiàng)式全局最優(yōu)解。因此,將非凸多項(xiàng)式全局優(yōu)化松弛為凸函數(shù),進(jìn)行線性規(guī)劃近似求解的極值,就是非凸多項(xiàng)式全局解的近似值。經(jīng)過多次松弛,可以完全逼近非凸多項(xiàng)式的全局最優(yōu)解。求解旋轉(zhuǎn)矩陣R的全局最優(yōu)解,即求解在一定可行區(qū)域下四階非凸多項(xiàng)式的全局最優(yōu)解。廣義非凸多項(xiàng)式優(yōu)化問題的一般形式可以描述為ming0(x),使gi(x)≥0,i=1…m(10)線性矩陣不等式LMI優(yōu)化方法是通過將目標(biāo)函數(shù)轉(zhuǎn)化為下屆更低的凸函數(shù),將可行區(qū)域轉(zhuǎn)化為凸包絡(luò),使得多極值非凸優(yōu)化問題可以轉(zhuǎn)化為凸優(yōu)化問題求解。定義k階多項(xiàng)式的基空間υk(x)=[1,x1…xn,x21…x1xn,x22…x2xn,…xkn],多項(xiàng)式g(x)可以表示為g(x)=∑α∈Νgαxα,其中xα=xα11,xα22…xαnn,gα是多項(xiàng)式的系數(shù),即可以通過矩陣將多項(xiàng)式在基空間線性化。對方程(10)進(jìn)行δ次LMI的一般過程可以歸結(jié)為:1)采用k階多項(xiàng)式的基空間線性化目標(biāo)函數(shù)g0(x);2)構(gòu)造δ-1階多項(xiàng)式基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廣義非凸多項(xiàng)式的LMI表示為minx∑[g0]αxα,使gi(x)vδ-1(x)vδ-1(x)T≥0vδ-1(x)vδ-1(x)T≥0經(jīng)過上述線性矩陣不等式的LMI,非凸多項(xiàng)式函數(shù)的全局優(yōu)化問題可以轉(zhuǎn)化為凸函數(shù)全局優(yōu)化問題,可以作為半定規(guī)劃問題進(jìn)行求解。具體的計算過程可以采用Henrion發(fā)布的GloptiPoly3軟件包,利用四元數(shù)為參數(shù)構(gòu)建多項(xiàng)式物空間誤差目標(biāo)函數(shù)和約束,采用帶約束的多項(xiàng)式LMI全局優(yōu)化算法求解。為了得到可靠的全局最優(yōu)解,本文實(shí)驗(yàn)中均是對多項(xiàng)式進(jìn)行6次LMI。4實(shí)驗(yàn)結(jié)果與誤差分析攝像機(jī)位姿R和T的確定與參考點(diǎn)分布情況和數(shù)量有關(guān)。利用數(shù)值仿真分析LMI算法時,考慮立體參考點(diǎn)與共面參考點(diǎn)為均勻隨機(jī)分布、LMI算法對噪聲的魯棒性以及參考點(diǎn)的數(shù)量對攝像機(jī)位姿評估的影響。整個仿真實(shí)驗(yàn)采用的圖像大小為640×480pixel,攝像機(jī)標(biāo)定的等效距為Fx=Fy=800pixel,主點(diǎn)坐標(biāo)[x0,y0]=pixel,平移矢量為T=[200,200,1000]pixel。為了描述求解精度,分別定義旋轉(zhuǎn)矩陣和平移矢量的相對誤差為Erot和Etrans,Erot(%)=‖Angletrue-Angle‖/‖Angletrue‖Etrans(%)=‖Ttrue-T‖/‖Ttrue‖在仿真實(shí)驗(yàn)中,旋轉(zhuǎn)角按均勻分布隨機(jī)產(chǎn)生,每個結(jié)果都是在獨(dú)立重復(fù)200次實(shí)驗(yàn)后求平均得到。因此,仿真曲線中的每一個點(diǎn)是在隨機(jī)產(chǎn)生200次旋轉(zhuǎn)角,對相對誤差求平均的結(jié)果。一般情況下,用于評估攝像機(jī)位姿的參考點(diǎn)隨機(jī)分布在立體場景。仿真中,參考點(diǎn)在[-10,10]×[-10,10]×[-10,10]均勻隨機(jī)產(chǎn)生6個點(diǎn),采用均值為零的高斯噪聲模型模擬實(shí)際噪聲。為了驗(yàn)證算法的有效性,將LMI算法與光束法平差(EPnP+GN)和正交迭代算法(EPnP+OI)進(jìn)行對比,這兩種算法均是以Lepetit提出的線性解法作為初始值驅(qū)動。圖2描述了圖像噪聲的方差為0~6時旋轉(zhuǎn)角和平移矢量的相對誤差隨噪聲方差的變化。相對誤差與噪聲的方差幾乎成線性關(guān)系,而且LMI算法和EPnP+OI的結(jié)果幾乎重合,其相對誤差都小于EPnP+GN的相對誤差。當(dāng)圖像噪聲方差為6時,LMI的旋轉(zhuǎn)角和平移矢量相對誤差分別約為1.96%和2.38%,EPnP+GN的旋轉(zhuǎn)角和平移相對誤差分別為2.93%和3.28%。存在噪聲的情況下,考查LMI算法的求解精度隨參考點(diǎn)數(shù)目的變化。取圖像噪聲的方差為2,圖3描述了相對誤差隨參考點(diǎn)數(shù)的變化情況。參考點(diǎn)數(shù)目的增加,對相對誤差的減小有一定作用,但是到達(dá)一定數(shù)目后,這種作用會變得不明顯。在參考點(diǎn)數(shù)目增加的過程中,LMI算法和EPnP+OI的相對誤差仍然是保持相當(dāng)?shù)慕咏?并且略小于EPnP+GN的結(jié)果,說明了LMI算法求解旋轉(zhuǎn)和平移的穩(wěn)定性和高精度?？紤]參考點(diǎn)為共面時的情況,即Zi=0。LMI算法對噪聲的魯棒性和隨參考點(diǎn)數(shù)目的變化情況同參考點(diǎn)為立體分布時的情況類似,如圖4和圖5所示。參考點(diǎn)無論是立體分布還是共面分布,LMI的結(jié)果都稍優(yōu)于EpnP+GN的結(jié)果。EPnP+GN屬于局部優(yōu)化算法,迭代過程中,目標(biāo)函數(shù)的下降方向只是向距初始值最近的極小點(diǎn)靠近,如果函數(shù)具有多個極值,那么最后的收斂點(diǎn)有可能只是局部極小點(diǎn),因而其求解目標(biāo)函數(shù)結(jié)果的全局最優(yōu)性在數(shù)學(xué)上不能得到保證,其精度取決于初始值的精度。而LMI算法和EPnP+OI求解的結(jié)果是目標(biāo)函數(shù)的全局最優(yōu)解。與EPnP+OI算法相比,LMI算法的計算過程沒有弱透視模型的假設(shè),完全不需要初始值的情況下達(dá)到全局收斂。在視覺測量中,標(biāo)定并校正后的圖像坐標(biāo)誤差一般為弱噪聲的情況,EPnP+GN的結(jié)果可能會非常接近LMI算法的結(jié)果,但是其收斂性在數(shù)學(xué)上得不到有效的保證?？傊?LMI算法不需要初始評估,利用LMI直接求解全局最優(yōu),且數(shù)學(xué)上得到保證。而光束法平差的收斂性在數(shù)學(xué)上不可證,正交迭代算法是基于弱透視假設(shè)前提。而且,LMI算法對于共面和非共面參考點(diǎn)均適用,具有廣義性。5攝像機(jī)位姿測量算法對平面參考點(diǎn)利用LMI算法確定攝像機(jī)位姿進(jìn)行實(shí)驗(yàn)驗(yàn)證。將已知相位分布的平面二維正弦灰度調(diào)制條紋圖顯示在液晶顯示器上,對拍攝的條紋通過傅里葉分析方法計算出兩個截斷的正交相位分布,利用截斷正交相位分布提取相應(yīng)的圖像特征點(diǎn),特征點(diǎn)的像素坐標(biāo)可以達(dá)到亞pixel級。實(shí)驗(yàn)中,采用JAICV-A50黑白攝像機(jī)拍攝傅里葉條紋,攝像機(jī)的分辨率為480×640pixel,采用Philips170S87液晶顯示器作為平面標(biāo)靶,分辨率為1024×1280pixel,顯示器的點(diǎn)間距為0.264mm。在進(jìn)行攝像機(jī)位姿評估前需要對攝像機(jī)進(jìn)行標(biāo)定,采用基于傅里葉條紋分析的攝像機(jī)標(biāo)定方法,標(biāo)定的攝像機(jī)參數(shù)見表1。在利用LMI算法確定攝像機(jī)位姿實(shí)驗(yàn)中,將實(shí)驗(yàn)分為兩組:第1組,固定參考點(diǎn)數(shù),對液晶顯示器上的條紋進(jìn)行不同方位的多次拍攝,確定算法的穩(wěn)定性;第2組,固定攝像機(jī)的拍攝角度,采用不同數(shù)目的參考點(diǎn)評估位姿。實(shí)驗(yàn)中,采用重投影誤差的方差作為位姿精度的評價標(biāo)準(zhǔn),且均與光束法平差進(jìn)行對比。第1組:攝像機(jī)在50個不同位姿拍攝條紋,其中的一幅條紋如圖6所示,提取400個特征點(diǎn)作為世界坐標(biāo)系的參考點(diǎn),利用LMI算法計算旋轉(zhuǎn)矩陣和平移矢量,然后利用成像模型計算重投影誤差。50次不同實(shí)驗(yàn)重投影誤差的方差如圖7所示,與光束法平差相比,LMI算法的重投影方差波動性稍小,并且數(shù)值很小,說明了LMI算法的精確度很高。第2組:攝像機(jī)與顯示器近似平行的情況下拍攝一幅圖片,以同一個點(diǎn)為世界坐標(biāo)系的原點(diǎn),然后提取不同數(shù)目的參考點(diǎn)。圖8顯示了點(diǎn)數(shù)分別為6和100時參考點(diǎn)的分布圖。利用光束法平差和LMI算法進(jìn)行位姿評估,其對比結(jié)果見表2。在使用傅里葉條紋提取特征點(diǎn)進(jìn)行位姿評估中,參考點(diǎn)數(shù)目過少,兩種算法的結(jié)果不穩(wěn)定,隨著點(diǎn)數(shù)的增加,算法趨于穩(wěn)定。但是當(dāng)點(diǎn)數(shù)增加到一定數(shù)目后,繼續(xù)增加點(diǎn)數(shù)對精度的提高效果不再明顯。由表2可見,兩者重投影的方差相當(dāng)接近。這是因?yàn)閰⒖键c(diǎn)都是在圖像消除畸變的基礎(chǔ)上提取的,因而其特征點(diǎn)的精度至少在亞pixel級別。當(dāng)參考點(diǎn)噪聲水平不高時,光束法平差結(jié)果的精度是很高的。但是,由于第2組實(shí)驗(yàn)中選取的圖片是在攝像機(jī)與顯示器近似平行的情況下拍攝的,其亞pixel級提取精度較高,而且選取的原點(diǎn)接近光心,畸變小。因此,當(dāng)其點(diǎn)數(shù)為100時,重投影的方差比第1組實(shí)驗(yàn)中點(diǎn)數(shù)為400時反而更小。因此,在利用傅里葉條紋評估位姿時,沒有