基于邊緣匹配的物體位姿追蹤

陳 策，岳繼光，董延超，沈潤杰

(同濟大學(xué)電子與信息工程學(xué)院，上海 201804)

0 引言

目標(biāo)位姿追蹤作為工業(yè)零部件無約束抓取等應(yīng)用的關(guān)鍵技術(shù)，最初通過確定物體表面至少4 個特征點的圖像坐標(biāo)以解算目標(biāo)位姿[1-3]。2008 年Choi 等人使用SIFT 特征點匹配定位方法估計物體位姿[4]，平移誤差在5 cm 以內(nèi)，姿態(tài)角度誤差在6°以內(nèi)。之后Youngmin 等人使用關(guān)鍵幀的特征點提取與匹配方法實現(xiàn)了多目標(biāo)的位姿追蹤[5]，該算法運行效率高。2014 年Karl 等人使用SIFT 特征點檢測目標(biāo)并實現(xiàn)分割，之后利用深度相機提取的深度圖信息匹配物體模型[6]，實現(xiàn)了多目標(biāo)的檢測與位姿追蹤，實驗中對機械臂抓取場景進行了實際測試，算法每幀耗時30 ms，平移誤差在3 cm 以內(nèi)，姿態(tài)角度誤差在4°以內(nèi)。至此，基于特征點的追蹤方法已在實時性以及精度上滿足一定要求。

基于圖像邊緣或輪廓的匹配方法，最早于1995 年就被提出，該方法通過尋找模型邊緣與圖像邊緣的匹配關(guān)系，將物體模型對齊于場景的邊緣圖像，以解算目標(biāo)相對相機的位置與姿態(tài)，實現(xiàn)位姿追蹤[7]。2015 年Wang 等人研究邊緣匹配的全局優(yōu)化方法[8]，使用圖模型進行全局尋優(yōu)，該方法在復(fù)雜背景情況中也取得了不錯的效果，位置誤差在2 cm 以內(nèi)，姿態(tài)誤差在4°以內(nèi)。基于隨機森林的空間點匹配方法[9-11]，能夠檢測物體并實現(xiàn)追蹤，但算法耗時較長且誤差較大；基于深度學(xué)習(xí)的RGB-D 位姿判別算法使用深度神經(jīng)網(wǎng)絡(luò)提取姿態(tài)描述子，之后回歸出物體位置和姿態(tài)[12-14]，該類方法仍然存在算法耗時過長，前期成本較高，精度不理想等缺陷。

所提算法主要解決復(fù)雜背景[15,16]下的貧紋理物體位姿追蹤問題。基于現(xiàn)有算法，改進場景灰度圖像[17]與物體三維模型的尋優(yōu)過程，提高配準(zhǔn)的精度以及魯棒性。通過模型邊緣方向與場景梯度方向構(gòu)造自適應(yīng)遮擋權(quán)重，有效降低誤匹配點對整體尋優(yōu)的影響，提高追蹤算法在復(fù)雜情況下的追蹤精度。

1 目標(biāo)追蹤算法

本文利用物體三維CAD 模型、目標(biāo)初始位姿，將模型邊緣點投影于圖像平面，利用方向倒角匹配構(gòu)建優(yōu)化函數(shù)，尋找模型邊緣點與場景邊緣圖像的最優(yōu)匹配，獲得當(dāng)前幀中目標(biāo)物體的精確位姿。該方法具有追蹤的連續(xù)性，相比使用檢測的追蹤方法實時性和連續(xù)性更高，不易產(chǎn)生突變。所提方案的基本流程如圖1 所示，主要分為三個部分：邊緣圖像方向倒角距離（DCM）計算；模型邊緣點投影并構(gòu)建目標(biāo)函數(shù)；以及最后的解析尋優(yōu)過程。

圖1 追蹤算法流程圖 Fig.1 Tracking algorithm flowchart

1.1 方向倒角距離匹配

為實現(xiàn)物體CAD 模型邊緣與圖像邊緣的對齊，常用的方法是倒角距離匹配（Chamfer Matching，簡稱CM）。假設(shè)M={mi}表示物體模型中的邊緣經(jīng)過離散采樣光柵化后映射于圖像坐標(biāo)系中的點，其中 i=1,2,… , | M |；N ={nj}代表場景邊緣圖像中的邊緣點，其中j=1,2,…,| N|。為實現(xiàn)物體模型與邊緣圖像的匹配，構(gòu)造倒角距離誤差函數(shù)如式（1）所示：

其中w 代表物體模型中的邊緣點數(shù)量，w=| M|。倒角匹配距離為二維圖像距離，使用該方法進行匹配魯棒性較差，易受背景影響。

Tuzel 等人[18]基于倒角距離匹配方案提出了方向倒角匹配（Directional Chamfer Matching，簡稱DCM）方法。在原有圖像距離基礎(chǔ)上，增加方向匹配信息，使匹配過程不僅考慮二維圖像距離，而且加入方向誤差。該方法提高了模型與圖像邊緣的匹配精度。假設(shè) φ(?)代表二維圖像中的方向運算符，即 φ(mi)代表模型點 mi在相機成像平面中對應(yīng)的模型邊緣方向，φ (nj)代表邊緣圖中的點 nj對應(yīng)的邊緣方向，構(gòu)造方向倒角距離誤差函數(shù)如下：

其中λ 代表方向誤差權(quán)重。基于DCM 的匹配方法正確率更高，能夠有效降低匹配誤差，并且對于遮擋以及復(fù)雜背景情況魯棒性更強，本文將使用DCM 構(gòu)造目標(biāo)函數(shù)，尋找模型邊緣與圖像邊緣的最優(yōu)匹配關(guān)系，實現(xiàn)物體的位姿追蹤。

1.2 目標(biāo)函數(shù)的構(gòu)建

基于DCM 構(gòu)建目標(biāo)函數(shù)需要計算物體邊緣點在相機成像平面的圖像坐標(biāo)；并通過場景邊緣圖像計算對應(yīng)DCM 張量。進而得到所有模型邊緣光柵點對應(yīng)的優(yōu)化殘差，以得到整體尋優(yōu)的目標(biāo)函數(shù)。首先通過物體三維CAD 模型以及目標(biāo)初始位姿，利用計算機提取物體深度邊緣。并將邊緣直線光柵化為三維離散點集P ={o1, … , om}∈?3，離散邊緣點位于物體模型邊緣之上，離散邊緣點的數(shù)量直接影響匹配的精度以及算法的效率。通過試驗，對于本文所用的目標(biāo)模型，光柵點間距選擇在1-2 mm。繼而提取點={, …,}∈?3，使得：

其次是場景邊緣圖像提取。本文使用LSD 算法提取場景邊緣，該算法的目標(biāo)在于檢測圖像中局部直線輪廓，并將輪廓構(gòu)成直線段；相比其他邊緣提取算法，能夠有效抑制長度較短的邊緣提取誤差，如圖2 所示，LSD 邊緣提取算法更加適合于DCM 三維張量的計算。

圖2 邊緣提取圖 Fig.2 Edge extraction

為加速DCM 張量的方向誤差計算，在場景邊緣圖像中，按一定角度間隔ε 對邊緣方向進行離散化，使得各角度范圍內(nèi)的邊緣單獨成圖，如圖3(a)所示。ε 的取值將對邊緣匹配精度以及算法運行時間造成影響。通過測試，權(quán)衡精度與耗時的關(guān)系，選定ε 為3°，即將邊緣圖像分為60 個離散的方向。為計算DCM 張量，首先根據(jù)各方向?qū)?yīng)的邊緣圖像計算倒角匹配距離，尋找所有光柵點與當(dāng)前離散角度邊緣中的最近邊緣點距離。該距離即是倒角匹配距離DTV{φ?i}，其中φ?i, i = {1,…,60}代表離散的邊緣方向，V代表當(dāng)前場景圖像。計算結(jié)果如圖3(b)所示，其中像素點的灰度代表當(dāng)前像素點距離邊緣的距離，灰度值越高，距離越遠(yuǎn)。通過各角度對應(yīng)的倒角匹配距離計算方向倒角距離DT3V{φ?i}，結(jié)果如圖3(c)所示。方向倒角匹配張量為三維距離張量，代表不同離散角度中，任一像素點的圖像距離與角度距離之和最小的邊緣距離。根據(jù)雙向動態(tài)規(guī)劃算法，首先將所有角度的方向倒角距離初始化為圖像二維距離，根據(jù)前向遞推以及后向遞推以計算各點對應(yīng)的距離最小值：

其中λ 代表方向誤差權(quán)重，該方法計算速度快，能夠在最多1.5 次前向與后向循環(huán)之后得到各角度邊緣圖像中的所有像素點對應(yīng)的DT3V{φ?i}，時間復(fù)雜度控制在 Ο ( q)以內(nèi)，其中q 代表圖像的像素個數(shù)。

完成模型邊緣光柵點的提取、映射以及場景圖像DCM 距離計算之后，通過光柵點的圖像坐標(biāo)ix 以及該模型邊緣點的方向構(gòu)造方向倒角匹配殘差函數(shù)：

圖3 離散方向殘差示意圖 Fig.3 Discrete direction residuals

其中n 代表模型邊緣點的數(shù)量， φ?(xi)代表對應(yīng)邊緣點在二維圖像中的邊緣切向方向，該方向即點集P 與P 映射到圖像坐標(biāo)系中的位置連線方向。通過構(gòu)造后的殘差函數(shù)作為優(yōu)化的目標(biāo)函數(shù)，將匹配問題轉(zhuǎn)化為尋優(yōu)問題，尋找使得所有邊緣光柵點匹配殘差之和最小的相對位姿。設(shè)立物體相對于相機的旋轉(zhuǎn)向量R =[ rxryrz]T以及平移向量T =[ txtytz]T，由此根據(jù)殘差函數(shù)得到優(yōu)化的目標(biāo)函數(shù)：

其中， oi代表三維模型邊緣光柵點， π(?)代表相機投影模型，g (T, R )∈?6代表物體相對于相機的三維空間位姿變換關(guān)系。通過優(yōu)化目標(biāo)函數(shù) E (T, R )以獲得當(dāng)前幀中物體相對于相機的精確位姿變換關(guān)系，并將該幀位姿作為下一幀圖像的初始位姿，以實現(xiàn)姿態(tài)追蹤。

1.3 解析尋優(yōu)過程

待優(yōu)化目標(biāo)函數(shù)由模型邊緣光柵點的方向倒角距離(DT3V)構(gòu)成，而優(yōu)化變量為目標(biāo)相對于相機的位姿變換關(guān)系，即g (T, R )∈?6。優(yōu)化目標(biāo)函數(shù)，關(guān)鍵在于尋找DT3V相對于待優(yōu)化變量的導(dǎo)數(shù)，Imperoli等人[19]通過直接計算數(shù)值導(dǎo)數(shù)的方法進行尋優(yōu)，該方法在優(yōu)化變量上進行微小攝動以計算 ΔD T3V，以獲取對應(yīng)優(yōu)化變量相對于目標(biāo)函數(shù)的數(shù)值導(dǎo)數(shù)，該方法在設(shè)定合適步長的情況下能夠使得目標(biāo)函數(shù)降低到最小值，但是在初始位姿與實際位姿相差較大或者背景較為復(fù)雜的情況下容易匹配失敗，陷入局部最小值。為提高匹配精度以及系統(tǒng)魯棒性，本文將利用解析方法，計算導(dǎo)數(shù)以提高匹配的魯棒性與精度。

目標(biāo)函數(shù) E (T, R )是由場景DT3V張量以及模型映射到圖像中的二維圖像坐標(biāo)點 xi(ui, vi)及該點邊緣方向φ?(xi)確定，因此尋找位姿變換關(guān)系 g (T, R )相對于二維圖像坐標(biāo)(ui, vi)的雅克比矩陣能夠解析地表示物體相對相機的位姿與物體在圖像坐標(biāo)系中映射點的坐標(biāo)變化關(guān)系。當(dāng)場景DT3V張量確定之后，模型映射點的坐標(biāo)變換將直接影響目標(biāo)函數(shù)，因此通過解耦位姿變換與模型映射點的圖像坐標(biāo)關(guān)系，能夠優(yōu)化尋優(yōu)過程，提高配準(zhǔn)精度。

假設(shè)目標(biāo)坐標(biāo)系為 Cobj，相機坐標(biāo)系 Ccam， Cobj相對于 Ccam的一次變換為T（對應(yīng)李代數(shù)為ξ ，對應(yīng)旋轉(zhuǎn)矩陣R 以及平移向量t），現(xiàn)存在目標(biāo)坐標(biāo)系 Cobj上一點p，其位于 Ccam中的位置為Tp。給定T 左乘一個擾動Δ T=exp(δξ∧)，∧算符代表求取向量的反對稱矩陣，并設(shè)定擾動項的李代數(shù)為δξ =[δ ρ ,δ φ ]T，可得：

根據(jù)相機投影模型，對于點p′：

使用第三行消去點p′距離相機坐標(biāo)系的距離s，得：

由此計算偏導(dǎo)數(shù)可得：

根據(jù)式（11）以及（14）可得到物體上一點投影到圖像平面內(nèi)的坐標(biāo)相對于坐標(biāo)系變換關(guān)系的雅克比矩陣：

使用該雅克比矩陣替代數(shù)值導(dǎo)數(shù)優(yōu)化方法，經(jīng)過測試，在復(fù)雜背景情況下，算法的匹配精度以及魯棒性均有提升。

2 自適應(yīng)權(quán)重優(yōu)化方法

為提高追蹤算法對復(fù)雜背景的魯棒性，本文提出自適應(yīng)權(quán)重優(yōu)化算法，動態(tài)改變光柵點優(yōu)化權(quán)重，降低擾動點的優(yōu)化占比，提高追蹤的精確度。

在完成當(dāng)前幀邊緣圖像與物體三維模型的匹配尋優(yōu)后，獲得了當(dāng)前時刻物體相對于相機的位姿變換關(guān)系 g (T, R )，通過該位姿變換關(guān)系能夠?qū)⒛Ｐ瓦吘壒鈻劈c集P ={o1, … , om}∈?3及用于求取模型邊緣方向的臨近點集={,… ,}∈?3映射為圖像坐標(biāo)系中的二維模型邊緣點集X={ x1,…, xm} ={π ( oi, g), i=0,…, m}∈?2以及={, …,} ={π (, g ), i=0,…, m}∈?2。計算邊緣光柵映射點以及對應(yīng)臨近點的連線方向，即為相應(yīng)模型邊緣在圖像中的方向Dmodel(xi)。在完成模型邊緣點方向計算后，對場景圖像的梯度方向進行提取。對于圖像函數(shù) f ( x,y )，其在點( x , y )處的梯度是一個具有大小和方向的矢量，設(shè)Gx和 Gy分別代表x 和y 方向的梯度，該梯度的矢量可以表示為：

本文使用Sobel 算子對梯度進行提取，該算子使用梯度矩陣與圖像矩陣作平面卷積，如式（17,18）所示，以計算圖像灰度函數(shù)的梯度近似值，所需運算資源相對較少，并且能有效降低僅對單個像素點進行梯度計算的誤差。

其中A 表示原始圖像矩陣中以計算點為中心的3*3 圖像矩陣，由此即可獲得圖像中所有像素的梯度大小，以及方向

根據(jù)模型光柵點的圖像坐標(biāo) xi( ui, vi)獲得場景圖像中對應(yīng)該點的梯度大小Mg( xi)以及方向 Dg( xi)，進而結(jié)合該點對應(yīng)的模型邊緣方向Dm(xi)，計算該點的優(yōu)化權(quán)重。首先對梯度大小進行篩選，若對應(yīng)點所在圖像梯度過小，證明該點所處圖像位置邊緣信息不明顯，導(dǎo)致該問題的原因可能是：（1）該處邊緣由于光照等外部原因，在圖像中顯示不明顯，邊緣提取算法未能成功提取；（2）該處邊緣被遮擋。在這兩種情況下，使用該光柵點的殘差作位姿優(yōu)化都會引入誤差，因盡量降低該點對下一幀位姿優(yōu)化的權(quán)值，防止誤匹配點對物體位姿追蹤的影響。其次，當(dāng)梯度大小大于設(shè)定閾值后，證明該匹配點已匹配到場景圖像的邊緣，但模型邊緣點與場景邊緣的匹配正確與否以及匹配的精度需要使用模型邊緣的方向以及場景梯度方向進一步確定。通過計算邊緣方向與梯度方向之差的余弦值作為該光柵點的優(yōu)化權(quán)值，使得兩者方向垂直情況下的權(quán)重更高，由此可得：

將優(yōu)化權(quán)重加入目標(biāo)優(yōu)化函數(shù)，得：

將圖像坐標(biāo)點 xi轉(zhuǎn)換為三維空間點 oi：

使用該方法不僅能夠降低圖像噪音以及邊緣提取誤差對系統(tǒng)帶來的擾動，而且在復(fù)雜背景中也能夠及時降低干擾點對于整體尋優(yōu)的影響，保證系統(tǒng)的魯棒性。

3 實驗驗證

本文所提算法系統(tǒng)使用C++構(gòu)建，上位機配置為Intel i7 CPU、8G RAM，算法平均每幀優(yōu)化配準(zhǔn)過程耗時57 ms。由于位姿數(shù)據(jù)庫的建立成本過高，并且精確度難以保證，因此本文使用計算機圖形學(xué)（簡稱：CG）渲染視頻作追蹤精度測試，并由渲染軟件得到目標(biāo)相對相機的位姿真值[20]，CG 渲染視頻追蹤效果如圖4 所示。

根據(jù)軟件生成真值用于精度測試，本文將相機放置據(jù)物體不同的距離，以測試物體在圖像平面內(nèi)不同尺度的追蹤精度，如表1 所示，其中S 表示物體在圖像中的平均尺度大小，單位為pix，R 表示平均角度偏差，T 表示平均平移向量偏差：

圖4 CG 渲染視頻追蹤 Fig.4 CG render video tracking

表1 CG 渲染視頻追蹤精度表 Tab.1 CG rendering video tracking accuracy table

由實驗可知，當(dāng)物體平均尺度在200～300 像素時的追蹤精度明顯優(yōu)于100 像素左右的情況，主要原因為物體尺度過小后邊緣匹配點減少，并且邊緣提取精度也較低，容易受到擾動的影響。

真實場景測試視頻由單目灰度相機提取，分辨率為1280*1024，真實場景測試主要考察算法對背景復(fù)雜、環(huán)境光照變化以及噪音較大時的表現(xiàn)，追蹤效果如圖6 所示。進而使用公開數(shù)據(jù)庫Rigid Pose[21]對算法精度進行測試，追蹤效果如圖5 所示。將追蹤結(jié)果與數(shù)據(jù)庫真值進行對比，如圖7 所示為每一幀圖像中目標(biāo)相對于相機的位姿關(guān)系，子圖a-f 分別對應(yīng)相機坐標(biāo)系中x，y，z 軸的旋轉(zhuǎn)以及平移關(guān)系，其中黃色線代表數(shù)據(jù)庫真值，藍(lán)色線代表追蹤算法實際追蹤結(jié)果。本文對比了Pauwels[22]等人提出的追蹤算法，并測試了相同數(shù)據(jù)集上的追蹤精度。如表2 所示，本文所提方法的角度追蹤平均誤差為1.835°，平移向量平均誤差為0.989 mm，相比Pauwels 所提的稀疏點追蹤方法，優(yōu)勢明顯；而與深度稠密追蹤方法相比，僅在平移向量誤差上稍大，但本文算法僅使用灰度圖像，在算法復(fù)雜度與計算耗時上有較大優(yōu)勢。根據(jù)測試，本文所提方法在CG 渲染視頻、真實灰度相機拍攝視頻以及公開數(shù)據(jù)集上的追蹤效果良好，平均角度誤差控制在2°以內(nèi)，平均平移向量誤差在1 mm 以內(nèi)，并且隨視頻質(zhì)量的提高，精度仍有一定程度提高。

圖5 Rigid Pose 追蹤結(jié)果圖 Fig.5 Rigid Pose tracking results

圖6 真實視頻追蹤 Fig.6 Real video tracking

圖7 Rigid Pose 數(shù)據(jù)集追蹤精度 Fig.7 Rigid Pose dataset tracking accuracy

表2 追蹤算法平均誤差 Tab.2 Average error of tracking algorithm

4 結(jié)論

本文給出了基于物體三維CAD 模型的追蹤方法，構(gòu)建方向倒角殘差函數(shù)，將追蹤問題轉(zhuǎn)換為尋優(yōu)問題，進而尋找模型與場景邊緣圖像的最優(yōu)匹配，以解算目標(biāo)物體相對相機的位姿關(guān)系。算法精度高、魯棒性強，標(biāo)準(zhǔn)測試數(shù)據(jù)庫實驗表明角度誤差在2°以內(nèi)，平移向量誤差在1 mm 以內(nèi)，均已達(dá)到現(xiàn)有算法較高水平；提出基于自適應(yīng)權(quán)重優(yōu)化的解決方案，該方法不僅能夠提高物體在復(fù)雜背景情況下的追蹤精度，還有效降低了系統(tǒng)噪音對追蹤效果的影響，進一步提高算法的適用性。并且該算法計算耗時較低，便于移植到移動設(shè)備平臺，對于工業(yè)視覺的應(yīng)用具有重要應(yīng)用價值。