[發(fā)明專利]一種基于目標(biāo)三維模型的實時位姿跟蹤方法

有效

• 申請?zhí)枺?/td>: 201910148832.0
• 申請日: 2019-02-28
• 公開（公告）號: CN109903313B
• 公開（公告）日: 2022-04-26
• 發(fā)明（設(shè)計）人: 孫曉亮;王梓;王剛;李璋;于起峰
• 申請（專利權(quán)）人: 中國人民解放軍國防科技大學(xué)
• 主分類號: G06T7/246
• 分類號: G06T7/246;G06T7/13
• 代理公司: 湖南省國防科技工業(yè)局專利中心 43102
• 代理人: 馮青

1.一種基于目標(biāo)三維模型的實時位姿跟蹤方法，其特征在于：基于目標(biāo)的三維模型及初始位姿估計投影得到二值圖像，目標(biāo)區(qū)域為1，背景區(qū)域為0，提取目標(biāo)輪廓，利用輪廓特征，建立輪廓部件模型，依據(jù)局部輪廓辨識度將輪廓分割成若干局部輪廓段；針對每一輪廓段，在梯度域基于相位匹配準(zhǔn)則建立二維-三維對應(yīng)；利用剛體目標(biāo)各部件之間的幾何關(guān)系，消除錯誤匹配對；基于建立的二維-三維匹配點對，通過RPNP魯棒的N點透視算法求解位姿參數(shù)，迭代整個過程直至收斂，實現(xiàn)序列圖像中目標(biāo)位姿跟蹤，

其步驟為：

(1)輪廓部件模型生成

利用目標(biāo)的輪廓特征建立投影圖像與輸入圖像之間匹配對的建立，利用目標(biāo)三維模型采用的是頂點和三角面片的數(shù)據(jù)格式，利用目標(biāo)的三維模型及目標(biāo)的初始位姿估計，采用公式(1)將目標(biāo)三維模型中的每個頂點投影到圖像上，

式中R及T分別為描述剛體運動的旋轉(zhuǎn)矩陣及平移向量，[X_W Y_W Z_W]^T、[X_C Y_C Z_C]^T及[uv]^T分別為頂點在世界坐標(biāo)系、相機坐標(biāo)系及圖像坐標(biāo)系中的坐標(biāo)，F(xiàn)_x、F_y、C_x及C_y為相機焦距及主點參數(shù)，通過標(biāo)定獲得，依據(jù)頂點投影結(jié)果，計算面片對應(yīng)的投影區(qū)域，最終確定在投影圖像中整個目標(biāo)對應(yīng)的區(qū)域，將目標(biāo)對應(yīng)的區(qū)域置1，背景區(qū)域置0，得到二值投影圖像，記為T；

采用腐蝕數(shù)學(xué)形態(tài)學(xué)操作算子對二值投影圖像進行處理，其中核元素為3*3的十字模板，再與原始二值圖像做差，得到目標(biāo)投影區(qū)域的最外圍輪廓，對應(yīng)的順序點集記為N為輪廓長度；

在原始二值圖像中，對于輪廓中每一個輪廓點對應(yīng)的位置s_i，計算其對應(yīng)的Shi-Tomasi角點響應(yīng)值，記為v_i，角點響應(yīng)值越大，說明對應(yīng)位置的輪廓走向存在較大改變，更具辨識度，依據(jù){v_i}_N從大到小對所有輪廓點進行排序，并采用非最大值抑制方法選出Q個辨識度高的輪廓點，記為Q為選取的輪廓點個數(shù)，

針對以其為中心選擇長度L的輪廓段S_j，則{S_j}_Q即為得到的目標(biāo)在當(dāng)前位姿參數(shù)下的輪廓部件模型，對應(yīng)S_j，在投影計算的過程中保存深度信息，依據(jù)相機參數(shù)可反求出對應(yīng)的物體上的點的三維坐標(biāo)，記為輪廓部件模型的點對集合為

(2)梯度域相位匹配

在梯度域進行匹配操作，旨在建立可靠的匹配對，采用3*3大小的Sobel算子求取投影到得到的二值圖像T及輸入圖像I對應(yīng)的梯度圖；

針對每一個輪廓段S_j，記T中輪廓段S_j對應(yīng)的圖像子塊為T'，在輸入圖像中待匹配圖像子塊記為I'，(dx_pT,dy_pT)及(dx_pI,dy_pI)分別為T'及I'中的像素點梯度向量，定義梯度相位角為梯度向量與圖像坐標(biāo)系軸正方向的夾角，對應(yīng)的梯度相位角度記為θ_pT及θ_pI，梯度相位角的范圍為[0,2π]，則基于余弦和準(zhǔn)則的梯度相位匹配準(zhǔn)則如公式(2)所示

式中X和Y為圖像子塊的尺寸，Score為相似度值，(θ_pT-θ_pI)即為梯度向量之間的夾角，將梯度向量歸一化

式中norm(g)求取向量的模值，則

cos(θ_pT-θ_pI)＝dx'_pT*dx'_pI+dy'_pT*dy'_pI (4)

本發(fā)明認(rèn)為向量方向相同或相反完全等價，即(θ_pT-θ_pI)＝0與(θ_pT-θ_pI)＝π等價，改寫公式(2)中相似度準(zhǔn)則為

有二倍角公式知cos(2(θ_pT-θ_pI))＝2cos(θ_pT-θ_pI)²-1，則相似度準(zhǔn)則進一步轉(zhuǎn)化為

將公式(4)帶入公式(6)得

在投影二值圖像中，僅提取最外圍目標(biāo)輪廓，為消除實時輸入圖像中目標(biāo)外圍輪廓附近的梯度造成干擾，采用原始二值圖像子塊T'對應(yīng)的梯度幅值對匹配準(zhǔn)則進行加權(quán)，即

其中w_pT＝norm((dx_pT,dy_pT))，Score的取值范圍為[0,1]；

針對每一個輪廓段S_j，在其初始位置附近一定尺寸鄰域內(nèi)采用滑動窗口策略搜索最佳匹配位置，若鄰域內(nèi)最大相似度值大于預(yù)先設(shè)定的閾值，則認(rèn)為成功匹配，否則此輪廓段匹配失敗，以輪廓段中心表示匹配對，則最終得到的成功匹配對集合記為K為成功匹配對數(shù)；

(3)誤匹配剔除

針對的剛體目標(biāo)的位姿跟蹤，剛體目標(biāo)不存在形變，自身各部件間相對幾何位置關(guān)系固定，采用局部結(jié)構(gòu)保持的匹配策略，剔除誤匹配點；

針對依據(jù)結(jié)構(gòu)保持策略，建立如下目標(biāo)函數(shù)

式中P＝{p_k}_K,p_k∈{0,1}為標(biāo)識向量，p_k＝1表示匹配點正確，反之為誤匹配點，N_p為點p的鄰域，以歐氏距離度量，λ為控制參數(shù)，K為成功匹配對數(shù)，為正則項，為判別是否在鄰域內(nèi)，為集合中第k個輪廓部件中心點，為集合中第j個輪廓部件中心點，

通過最小化公式(9)中所示目標(biāo)函數(shù)，即求得標(biāo)識向量P，進而剔除誤匹配點，得到正確的匹配點對集合，記為結(jié)合目標(biāo)三維模型投影中得到的點對集合記為建立輸入圖像與目標(biāo)三維模型之間的二維-三維對應(yīng)關(guān)系，對應(yīng)的二維-三維匹配點對集合為

(4)位姿參數(shù)求解

基于得到的二維-三維匹配點對集合采用RPNP魯棒的N點透視算法求解目標(biāo)位姿參數(shù)，迭代整個過程，并依據(jù)迭代中位姿參數(shù)更新量判斷迭代是否終止，輸出最終的位姿更新結(jié)果。