基于編碼標記點的高精度運動估計

遲書凱，葉 旋，高 翔，解則曉，陶冬冬

（中國海洋大學 工程學院，山東 青島 266100）

1 引 言

為實現(xiàn)前景運動的精確估計，本文提出了一種基于編碼標記點的高精度運動估計方法。前景運動估計指的是估計動態(tài)場景（前景）相對于靜態(tài)場景（背景）的運動參數(shù)，包括三自由度的旋轉參數(shù)以及三自由度的平移參數(shù)。其在智能監(jiān)控、目標跟蹤、姿態(tài)估計等領域有著廣泛的應用?，F(xiàn)有的前景運動估計方法主要包括兩類：基于RGB 圖像的前景運動估計以及基于RGB-D 圖像的前景運動估計。

基于RGB 圖像的前景運動估計方法主要有光流法［1-2］、目標跟蹤法［3-4］、直接估計法［5-6］等。光流法通過估計幀間光流的方式估計前景運動，當前景運動幅度較大時，會因不滿足光流假設導致光流估計的失敗；目標跟蹤法采用基于視覺的目標跟蹤方法對前景目標進行跟蹤以估計前景運動，這類方法通常只是給出前景目標的包圍盒且估計的是目標在圖像上的二維運動；直接估計法基于深度神經網絡，通過在二維圖像上直接估計前景目標的六自由度位姿參數(shù)實現(xiàn)前景運動估計，盡管這類方法可以估計六自由度的前景運動，但是這類基于圖像的姿態(tài)估計方法通常精度相對較低，通常難以滿足前景運動估計的精度要求。

另外，對于基于RGB-D 圖像的前景運動估計，Li 等人［7］提出了一種同時重建靜態(tài)背景與非剛體前景的方法，該方法采用實時實例分割算法“你只看系數(shù)”（You Only Look At CoefficienTs，YOLACT）［8］實現(xiàn)前背景分割，然后采用剛體與非剛體迭代最近點（Iterative Closest Point，ICP）算法，實現(xiàn)背景與前景的跟蹤。上述方法雖然能夠實現(xiàn)前景運動估計，但是由于RGB-D 圖像中深度圖噪聲較大且有效范圍較小，難以精確捕捉較大范圍的前景三維運動，在一些對精度要求較高的場景（如機器人末端運動精確估計）中應用受限。

由上述分析可知，現(xiàn)有前景運動估計方法存在維度缺失、范圍受限、精度不足等問題，在對精度要求較高的大范圍六自由度運動（如機械臂拾放、機器人搬運等）進行運動估計時，現(xiàn)有方法難以勝任。對此，本文提出了一種面向上述特定工業(yè)應用場景運動估計方法，該方法借助編碼標記點實現(xiàn)前景運動的高精度估計。相對于場景本身的紋理特征，編碼標記點這類人工標志可以提高測量系統(tǒng)的魯棒性，在場景結構復雜、紋理缺乏等外界條件下仍可提供較高精度的觀測量（圖像特征點位置），且?guī)缀醪粫嬖谡`匹配現(xiàn)象。該方法在測量環(huán)境內的前背景中均粘貼編碼標記點，所采用的標記點為環(huán)狀編碼標記點，不同的編碼標記點均可通過環(huán)狀編碼帶確定唯一的身份信息［9］。在此基礎上，本文提出了一套基于環(huán)狀編碼標記點的高精度運動估計流程，包括編碼標記點的提取與識別、與相機的分組、背景點與相機位姿初始化、前景點與前景運動初始化、全局優(yōu)化在內的五個步驟，可以同時估計相機位姿以及（前背景）標記點三維坐標，實現(xiàn)了前景運動的全自動、高精度估計。本文將圖聚類方法引入編碼標記點與相機的分組過程，實現(xiàn)了前景運動的全局最優(yōu)劃分；將廣義光束平差方法引入編碼標記點坐標與相機位姿的優(yōu)化過程，實現(xiàn)了前景運動的全局最優(yōu)估計。

2 方 法

2.1 方法概述

本文提出的基于標記點的高精度運動估計方法的輸入為自由拍攝的圖像，輸出為前景運動（旋轉、平移）的估計結果。為區(qū)分運動前景與靜止背景，在測量場景中粘貼編碼標記點，并將位于前景運動目標上與背景靜止環(huán)境下的編碼標記點分別稱為前景與背景標記點。該方法主要包括五個部分：1）進行編碼標記點的識別與提取，獲取圖像中各點的中心位置以及碼值信息；2）采用基于圖聚類的方法，實現(xiàn)編碼標記點與相機的分組，即自動區(qū)分前景與背景標記點以及不同次前景運動分別對應的相機集合；3）采用增量式從運動恢復結構的方式，對背景點三維坐標以及相機位姿進行初始化；4）根據(jù)估計的相機位姿，采用三角測量的方式初始化前景點的三維坐標，并通過圖優(yōu)化的方式初始化前景運動；5）對已估計的前背景點三維坐標、相機位姿、前景運動進行全局優(yōu)化，以獲取前景運動的最終估計結果。該方法的流程圖如圖1 所示，下文將對上述 五個部分進行具體介紹。

圖1 基于編碼標記點的前景運動估計方法的流程圖Fig.1 Flow chart of the proposed coded marker-based foreground motion estimation method

2.2 編碼標記點的提取與識別

在進行編碼點的提取時，采用Xie 等人［10］的方法，獲取標記點的近似成像中心以及像素級邊緣點，并利用Zernike 正交矩［11］實現(xiàn)邊緣點的亞像素化。在此基礎上，采用隨機抽樣一致性（Radom Sample Consensus，RANSAC）算 法［12］以 及非線性最小二乘優(yōu)化，對標記點成像橢圓參數(shù)進行初始化與優(yōu)化。上述優(yōu)化的目標函數(shù)定義如下：

基于編碼標記點的提取結果，采用Xie 等人提出的等分橢圓內切圓的方法［9］，獲取標記點的碼值信息，實現(xiàn)標記點的識別。

2.3 編碼標記點與相機的分組

為實現(xiàn)相機位姿以及前景運動的估計，需要先進行編碼標記點與相機的分組，即自動區(qū)分前景與背景標記點以及不同次前景運動分別對應的相機集合?；诒尘皹擞淈c數(shù)量遠小于位于前景標記點的假設，當對一對分屬不同次前景運動的圖像利用匹配（同一碼值）的標記點進行基于RANSAC 的基本矩陣估計［15］時，對應的標記點匹配對內點為背景點，外點為前景點?；谏鲜鲇^察，可以采用如下的樸素算法進行編碼標記點與相機的分組：對輸入圖像集合中的任意一對圖像進行基于RANSAC 的基本矩陣估計并記錄其中各匹配對的內外點情況；將各標記點在所有圖像對中屬于內點的比例作為標記點分組依據(jù)，背景點的該比例應高于前景點；將各圖像對中匹配標記點的內點比例作為相機分組依據(jù)，屬于同一次前景運動的圖像對該比例應高于分屬不同次前景運動的圖像對。

然而，上述樸素分組算法存在許多不足：（1）由于共外極線的情況，鏡頭畸變的存在以及距離閾值的不當，會導致部分內外點的錯誤歸類；（2）對于不同的實驗配置，上述算法中內點比例的取值分布會存在區(qū)別，因此標記點與相機分組的閾值難以確定；（3）上述算法僅利用了各標記點及各圖像對的內點比例這類局部信息，而未利用各標記點之間及各相機之間的全局信息，使得分組算法魯棒性較差。

針對上述問題，本文提出了一種基于圖聚類（Graph Clustering）的自動編碼標記點與相機的分組算法，從圖論的角度對該分組問題進行建模，將每幅圖像及每個標記點分別作為頂點，建立相機圖結構及標記點圖結構，再通過Dhillon 等人的圖聚類算法［16］，實現(xiàn)編碼標記點與相機的自動分組，該方法可以有效利用全局信息，有著更強的魯棒性。在此基礎上，對上述加權標記點圖進行圖聚類，即可實現(xiàn)編碼標記點的自動（前背景）分組。

2.4 背景點與相機位姿初始化

經過上一節(jié)的編碼標記點分組后，可以利用背景點在各幅圖像上的投影橢圓中心測量值，借助從運動恢復結構技術［17-18］，估計各背景點的三維坐標以及各相機的位姿（旋轉、平移）。由于本文涉及的相機與標記點數(shù)量相對較少且標記點的匹配關系精確已知，因此采用一種簡化的增量式從運動恢復結構的方法進行背景點與相機位姿的初始化，具體過程簡述如下：

Step 1：找到公共可見的編碼標記點匹配對數(shù)最多的一對圖像并根據(jù)其匹配標記點成像橢圓中心估計圖像對之間的本質矩陣并對其分解獲取相對旋轉、平移［19］；

Step 2：通過三角測量算法［20］，估計匹配標記點的三維坐標，實現(xiàn)初始圖像對重建；

Step 3：通過迭代的形式添加新的圖像并進行場景擴展，直至完成所有相機的位姿估計。

在迭代添加相機時，每次選取與當前已獲取三維坐標的標記點公共可見對數(shù)最多的相機，給定的二維-三維點對應關系，采用基于RANSAC的透視三點（Perspective-3-Point，P3P）算法［21］估計新添加相機的位姿，并采用三角測量算法估計之前未獲取的公共可見標記點的三維坐標，實現(xiàn)場景擴展。另外，在初始圖像對重建及每次添加圖像與擴展場景后，均采用光束平差（Bundle Adjustment，BA）技術［22］對標記點三維坐標，相機內參數(shù)及其位姿進行優(yōu)化。

由于本文中各圖像均由同一設備采集，在背景點與相機位姿初始化的過程中各幅圖像之間共享內參數(shù)。本文采用的相機模型為含有徑向及切向畸變的相機模型，相機內參數(shù)包括焦距{fx，fy}，主 點p0=(u0，v0)，徑 向 畸 變 系 數(shù){k1，k2，k3}，切向畸變系數(shù){p1，p2}。其中，焦距初始值從相機的可交換圖像文件格式（Exchangeable Image file Format，EXIF）讀取，主點與畸變系數(shù)的初始值分別置為圖像中心與0，在對相機內參進行優(yōu)化時固定主點，只優(yōu)化焦距與畸變參數(shù)。根據(jù)采用的上述相機模型，若某橢圓實際成像中心為pc=(uc，vc)T，則其理想成像中心p?c=(u?c，v?c)T可通過下式進行計算：

其中，r2=x2＋y2，(x，y)T為橢圓實際成像中心歸一化坐標，計算如下：

經過上述流程，可以估計各背景點的三維坐標以及各相機的旋轉、平移，實現(xiàn)背景點與相機位姿初始化。

2.5 前景點與前景運動初始化

在估計各相機位姿以后，可以通過三角測量算法計算各前景點在不同次前景運動中分別的三維坐標，進而通過各前景點在不同次前景運動中的坐標變換關系表征各次前景運動。需要注意，在對前景點進行三角測量時需要按照相機分組結果，對每次前景運動分別進行。在此基礎上，可以通過Arun 等人［23］提出的方法估計各次前景運動之間的相對旋轉、平移，并將其記為{Ri，j，ti，j}，其 中Ri，j與ti，j分 別 表 示 第i次 與 第j次前景運動之間的相對旋轉與相對平移。然后，根據(jù)獲取的前景運動的相對旋轉與平移構建位姿圖，圖中頂點表示各次前景運動的絕對位姿，邊表示兩次前景運動之間的相對位姿。給定位姿圖中的相對位姿，采用位姿圖優(yōu)化［24］的方式，估計各次前景運動的絕對位姿初始值，實現(xiàn)前景運動的初始化。上述位姿圖優(yōu)化的目標函數(shù)定義如下：

2.6 全局優(yōu)化

經上述步驟，本文對相機位姿、（前背景）標記點三維坐標以及前景運動進行了初始化，此小節(jié)需要利用上述初值，通過非線性最小二乘優(yōu)化的方式，對上述變量進行全局優(yōu)化，以實現(xiàn)前景運動的高精度估計。

在此基礎上，即可通過求解下式中的最優(yōu)化問題實現(xiàn)上述參數(shù)的全局優(yōu)化：別介紹上述四類誤差項的定義：

圖2 本文前景運動估計系統(tǒng)及符號標記示意圖Fig.2 Schematic diagram of our proposed foreground motion estimation system and symbolic representation

基于上述介紹，采用Gao 等人提出的廣義光束平差方法［25］，對式（5）進行優(yōu)化求解，同時最小化其中的反投影誤差以及空間誤差，以獲取前景運動的最終估計結果{RF*i，tF*i}。

3 實驗結果

為驗證本文提出的基于編碼標記點的前景運動估計方法的有效性，現(xiàn)對其中的一個關鍵步驟編碼標記點與相機分組，以及前景運動估計的最終結果進行實驗驗證。本文采用的圖像采集設備為Apple iPhone 7，鏡頭焦距為4 mm，采集的圖像分辨率為4 032×3 024。

3.1 編碼標記點與相機分組實驗

為驗證本文提出的編碼標記點與相機分組方法的有效性，將本文方法與2.3 節(jié)介紹的樸素分組方法進行對比。在進行對比時，為評價分組效果，需要先確定評價指標。在此，本文采用F1-Measure 進行分組效果評價。具體來說，假設將人工標記的分組結果（真值）與某待評測方法的分 組 結 果 分 別 記 為{Pj|j=1，2，…，N} 與{Ci|i=1，2，…，M}，其中，Pj與Ci分別表示人工標記的分組結果的第j組與待評測方法的分組結果的第i組元素集合。首先，先根據(jù){Pj}與{Ci}計算F1-Measure 矩陣：

對于本文分組方法與樸素分組方法來說，給定不同的基本矩陣估計外點閾值，可獲得不同的分組結果。根據(jù)上述分組評價指標，即可對兩種方法進行分組效果對比，結果如圖3 所示。

圖3 編碼標記點與相機分組對比實驗結果Fig.3 Comparative results on coded marker and camera partitioning

由圖可知，對于標記點分組，兩種方法效果接近其都較為準確，而對于相機分組而言，本文提出的基于圖聚類的分組方法效果明顯優(yōu)于樸素分組方法。另外，隨著基本矩陣估計的外點閾值的變化，本文提出的相機分組方法效果變化不明顯而樸素分組方法效果變化劇烈，說明本文方法對外點閾值變化更加不敏感，魯棒性更強。在實際測量過程中，用于基于RANSAC 的基本矩陣估計的外點閾值取為4 像素。

3.2 前景運動估計精度對比實驗

圖4 前景運動估計精度評價實驗示例圖像Fig.4 Example images of foreground motion estimation accuracy evaluation experiment

基于上述評價指標與實驗數(shù)據(jù)，我們首先驗證了本文提出的基于RANSAC 與Polar-N-Direction 距離的標記點成像橢圓擬合方法的有效性。具體來說，我們將采用此方法擬合橢圓與基于最小二乘擬合橢圓［9-10］分別得到的橢圓成像中心坐標用于進行前景運動估計，通過比較最終前景運動估計的精度來衡量橢圓擬合的效果，結果如表1 所示。由表1 可知，采用本文方法進行橢圓擬合獲取的橢圓中心精度高于基于最小二乘的橢圓擬合方式，能夠實現(xiàn)更為精確的前景運動估計。

表1 標記點提取精度對比實驗結果Tab.1 Comparative experiment results on coded maker extraction accuracy （mm）

另外，為驗證本文采用的相機模型（見2.4節(jié)）的有效性，基于上述評價指標與實驗數(shù)據(jù)，進行了優(yōu)化不同內參數(shù)時前景運動估計精度對比實驗，結果如表2 所示。由表2 可知，本文采用的相機模型（固定主點，優(yōu)化焦距、畸變）精度最高，對切向畸變系數(shù)以及主點坐標進行優(yōu)化可以分別提升及降低前景運動估計精度。因此，對于本文選用的圖像采集設備及其采集的具體測試數(shù)據(jù)，我們最終選擇優(yōu)化的相機內參為包括焦距，徑向、切向畸變在內的七參數(shù)模型：{fx，fy，k1，k2，k3，p1，p2}。

表2 內參優(yōu)化對比實驗結果Tab.2 Comparative experiment results on optimized intrinsic parameters （mm）

為驗證本文提出的前景運動估計方法的有效性，基于上述評價指標與實驗數(shù)據(jù)，在此進行了前景運動估計精度對比實現(xiàn)，將本文方法（proposed method）與（1）無全局優(yōu)化的前景運動估計方法（W/O Global Optimization），（2）樸素的前景運動估計方法（Naive Method），（3）以及基于場景本身紋理特征的前景運動估計方法（Texture-Based Method）進行對比。對于無全局優(yōu)化方法，直接將2.5 節(jié)通過圖優(yōu)化獲取的前景運動初值作為前景運動的最終估計結果。對于該樸素方法，在采用本文2.3 節(jié)提出的方法對標記點與相機進行自動分組之后，對于同屬一組的相機而言，所有標記點均可看作背景點，因此可以采用本文2.4 節(jié)的方法計算各組內部的相機位姿與標記點坐標，然后根據(jù)各組計算得到的實際背景點的坐標將各組測量結果統(tǒng)一至同一基準之下，在此基礎上實現(xiàn)前景運動估計。對于基于紋理的方法，我們將采集的圖像導入至當前最具代表性的基于圖像紋理特征的三維建模系統(tǒng)COLMAP 中，獲取測試場景的稠密點云，結果如圖5 所示。由圖5 可知，背景固定場景以及4 次前景運動場景均可成功重建。在此基礎上先根據(jù)標尺將進行場景尺度恢復，然后人為劃分出不同次前景運動對應的三維點云并采用帶顏色的ICP 算法［26］估計各次前景運動。

圖5 測試場景COLMAP 稠密重建結果Fig.5 Dense reconstruction result on the test scene by COLMAP

本文方法與兩種對比方法的對比實驗結果如表3 所示。由表3 可知，相對于基于圖優(yōu)化的前景運動初始化結果，基于廣義光束平差的全局優(yōu)化能夠獲得更為精確的前景運動估計，且本文提出的前景運動估計方法在精度與穩(wěn)定性方面均高于樸素方法與基于紋理的方法，估計誤差的均值小于0.3 mm，可以滿足大多數(shù)場景的測量精度要求。

表3 前景運動估計精度對比實驗結果Tab.3 Comparative experiment results on foreground motion estimation accuracy （mm）

4 結 論

本文提出了一種基于編碼標記點的高精度運動估計方法。該方法通過在測量環(huán)境中前背景上粘貼環(huán)狀編碼標記點，并在每次前景運動后采集若干圖像，之后同時估計所有相機位姿以及背景固定標記點、前景運動標記點空間坐標，實現(xiàn)前景運動的高精度估計。針對上述目標，提出了一套高精度運動估計流程，包括編碼標記點的提取與識別、編碼標記點與相機的分組、背景點與相機位姿初始化、前景點與前景運動初始化、全局優(yōu)化五個步驟。實驗結果表明，本文提出的運動估計方法精度在0.3 mm 左右，可以滿足高精度前景運動估計的需求。另外，本文通過實驗驗證了提出的編碼標記點與相機的分組方法以及運動估計整體流程的有效性。