基于三維模型的單目車輛位姿估計

許凌志，符欽偉，陶 衛，趙 輝

（上海交通大學 儀器科學與工程系，上海200240）

1 引 言

實時車輛位姿估計旨在對車輛目標進行檢測的同時獲取位置與姿態信息，在智能汽車以及智慧交通管理等領域中有著重要的應用。例如在自動駕駛車輛上，系統需要獲取周圍車輛的位姿及運動狀態信息以進行穩健的決策。在現有的車輛位姿估計方法中，基于視覺的位姿估計方法有著無接觸、應用范圍廣的特點。特別是基于單目視覺傳感器的方法成本低且有著廣泛的使用基礎，使得基于單目視覺的位姿估計方法成為近年來的研究熱點。

但是，基于單目視覺的車輛目標位姿估計呈現出幾個特別之處：其一，車輛目標通常出現在城市道路或室內車庫中，當通過安裝于路口或者角落上的監控相機進行低角度拍攝時，車輛目標之間或車輛目標與其他目標間易出現遮擋嚴重或者在相機邊緣處被截斷的情況；其二，車輛目標的運動范圍大，造成目標在圖像上的尺度變化大，位姿估計的難度更大；其三，與在二維圖像上的任務不同，三維位姿估計需要建立二維圖像到三維空間的映射關系，然而單目視覺傳感器在圖像采集的過程中丟失了深度信息，因而無法獲得目標的絕對尺度信息。

目前，基于視覺的車輛位姿估計主要有以下三種方法：第一類是基于深度信息的方法，常見于自動駕駛場景。針對單目相機無法獲取深度信息的問題，該類方法的主要思路是依靠激光雷達等深度傳感器，彌補三維目標投影到二維圖像中所損失的深度信息，通過多源信息融合實現對車輛位姿的精確估計［1-3］。基于深度信息的位姿估計方法精度最高，但在實際使用中需要昂貴的硬件設備作為支撐，且龐大的數據量對計算機的性能配置提出更高的要求，不利于推廣和使用。第二類方法是基于深度學習的直接法，這一類方法依賴于深度神經網絡的擬合能力及大規模人工標注的數據集，將目標車輛的位姿直接作為神經網絡的輸出，單目圖像作為輸入進行端到端的訓 練。相 關 研 究 中，6D-VNet［4］在 目 標 檢 測 網 絡的基礎上，增加了兩個額外分支來進行目標的平移和旋轉四元數的回歸，以實現位姿估計；SSD-6D［5］將姿態空間劃分為相鄰子空間，從而將位姿估計由連續問題轉化為離散問題；MonoPair［6］中基于地平面假設對問題進行簡化，直接回歸了車輛在地平面上的坐標以及偏航角。然而，這類方法缺乏顯式的幾何約束關系，精度較低。第三類方法利用目標先驗模型，通過傳統特征算子或者神經網絡提取目標特征，并與三維模型進行模板匹配［7-8］，得到目標的位姿信息。Li［9］等人通過預測車輛目標的掩模并與不同姿態對應的模板進行匹配度計算得到結果。Barowski［10］是對目標進行部位級別的精細分割進而建立二維到三維的對應關系。Deep manta［11］，Yolo-6d［12］等方法［13］則是利用目標關鍵點特征，建立二維到三維的關鍵點對應關系，求解PnP問題得到目標位姿。這類方法利用三維模型恢復了尺度信息并獲得了較高的精度，然而復雜的實際場景導致的遮擋或者截斷等問題，會增加目標特征提取的難度并影響位姿估計的精度。

綜合考慮位姿估計應用場景與現有的遮擋和截斷以及車輛目標尺度變化較大等問題，本文提出一種單目相機結合車輛先驗模型進行位姿估計的改進方法，首先對檢測圖像中的目標進行尺度歸一化。隨后在姿態估計模塊中使用基于向量場的關鍵點檢測網絡得到所有關鍵點的亞像素級坐標，在此基礎上提出基于距離加權的損失函數以優化網絡的訓練目標，同時改進基于隨機 抽 樣 一 致（Random Sample Consensus，RANSAC）的關鍵點投票算法，最終通過關鍵點坐標以及三維目標模型建立二維到三維空間的對應關系，進而求解車輛的位姿信息。

2 基于車輛三維模型的位姿估計

2.1 車輛位姿估計原理

給定一幅圖像，姿態估計的目標是獲得圖像中目標在三維空間的方向及位置信息。具體來說，假設三維空間建立在相機坐標系中，定義車輛目標的位置為車輛的底面中心點，并以此為原點，車的正前方、左側方及正上方為坐標軸方向建立車輛坐標系。車輛目標的位姿估計問題轉換為計算車輛坐標系到相機坐標系的變換矩陣。該變換矩陣包括表示各個坐標軸旋轉角度的旋轉矩陣R和表示車輛坐標系原點在相機坐標系中的三維坐標的平移變換向量T。

本文提出的車輛位姿估計方法利用了已知的車輛三維模型，這些三維模型可以通過攝影測量或者激光掃描技術獲得，三維模型無其他特殊要求。原理如圖1所示，首先在輸入圖像中檢測出目標車輛，然后對所有目標的尺度進行歸一化后送入向量場回歸網絡，根據向量場的預測結果投票得到目標關鍵點的二維坐標，最終通過關鍵點的二維以及三維坐標建立二維圖像到三維空間的對應關系并求解車輛位姿。

圖1 位姿估計原理Fig.1 Principle diagram of pose estimation

2.2 尺度歸一化

為了預測目標的關鍵點坐標，首先需要對目標區域進行精確定位。常見的定位方法多采用語義分割的方式得到目標區域，但是對于車輛位姿估計任務而言，圖像中通常含有多個車輛目標，此時基于語義分割的方法無法區分屬于同一類別的目標。其次，與普通目標相比，車輛目標在相機視野中的運動范圍更大，導致車輛在圖像上的尺寸差異較大，造成后續位姿估計網絡的感受野波動，進而影響位姿估計精度。

為此本文設計了一個尺度估計模塊，如圖2所示。針對圖像中尺寸變化劇烈的車輛目標，在對它進行位姿估計前，將目標尺寸縮放到同一大小以消除尺度變化引起的位姿估計網絡訓練的不穩定。通過一個尺度估計模塊獲取目標在圖像中的尺度信息，并將提取的目標區域進行尺度歸一化。具體地，本文使用一個輕量的目標檢測［14］網絡來實現尺度估計的功能，獲得每個目標在圖像中的矩形框后在原圖中進行裁剪，并對目標區域進行尺度縮放，使得所有目標處于相同的尺度空間。這里使用雙線性插值對目標區域進行縮放，在保持長寬比的條件下將包含目標的矩形框的較短邊縮放到256個像素。

圖2 車輛位姿估計網絡結構圖Fig.2 Network architecture for vehicle pose estimation

2.3 基于向量場形式的關鍵點檢測網絡

本文采用回歸向量場的形式對關鍵點進行預測。向量場是一種關鍵點的中間表達形式，OpenPose［15］中用它來建模關鍵點之間的對應關系，PoseCNN［16］和PVNet［17］中將它作為中間變量來代替對關鍵點的回歸。使用向量場來回歸每個像素點與指定關鍵點之間的相對角度，并最終通過投票的方式得出關鍵點的坐標，采用向量場的形式可以更好地預測被截斷或者遮擋的關鍵點。具體地，對每個關鍵點，網絡為屬于目標的所有像素點預測一個指向該關鍵點的單位向量，在輸出層中使用兩個和原圖尺寸一樣的單通道圖分別表示該單位向量在x軸和y軸上的分量。

其中：p為當前像素點，(kx，ky)，(p x，p y)分別是對應關鍵點和當前像素點在圖像坐標系中的坐標。

向量場的預測通過一個全卷積網絡實現，網絡輸出包含目標的像素級分割掩模及向量場預測結果。本文使用U-net［18］結構，在網絡最終結果輸出前將來自4個尺度的特征圖分別上采樣到原圖尺寸后疊加在一起，最終通過一個卷積層輸出分割以及向量場預測的結果，如圖2所示。

2.4 基于距離加權的向量場損失函數

在關鍵點檢測部分，向量場回歸網絡有兩個預測對象，分別是目標區域的像素級掩模及關鍵點向量場，因此本文使用多任務損失函數進行監督。損失函數由兩部分組成：

其中λ1，λ2代表不同損失函數的權重。式（3）中的兩項分別對應目標分割以及關鍵點向量場回歸兩個任務。

由于在該網絡中僅需對目標區域和背景區域進行區分，在本文中使用二分類交叉熵損失函數。在PoseCNN［16］和PVNet［17］中，對 于 向 量 場的回歸采用Smooth l1損失函數分別對單位向量在x軸和y軸上的分量進行監督，如公式（4）所示：法表現訓練的最終目標，由于向量場回歸網絡的最終目標是預測關鍵點在圖像上的坐標，而該損失函數的優化目標卻是每個像素點指向向量的角度，沒有引入關鍵點定位誤差與每個像素點指向向量的關系。其二，通過對最終結果的損失值進行可視化發現（圖3），關鍵點周圍像素的損失值遠大于其余像素；在訓練的過程中，關鍵點周圍像素的梯度占據了主導進而導致在訓練過程中對該部分像素過于關注；在投票得到關鍵點的過程中，關鍵點周圍像素在整體中僅僅占據很小的比例，由于像素自身特征與關鍵點特征過于相似，鄰近像素對應的單位向量的學習難度大大提高，從而導致在訓練過程中對占據主體的像素的優化不夠充分。

圖3 像素權值損失值可視化Fig.3 Visualization of pixel weight loss

其中：K是所有關鍵點的集合，P是所有屬于目標實 例 的 像 素 集 合，v x|p，k，vy|p，k，gx|p，k，gy|p，k分 別 表 示向量在x軸和y軸分量的預測值與真值。

經過分析，將該損失函數用于向量場回歸會產生兩個問題。其一是該損失函數在形式上無針對于這個問題，本文提出基于距離加權的向量場損失函數，根據像素點與關鍵點的距離為每個像素點的損失值賦予一個與距離成正比的權重，在訓練過程中使得網絡更加關注距離較遠的像素點。損失函數的具體形式如下：

其中d p，k表示當前像素點p與對應關鍵點k的像素L2距離。

2.5 關鍵點誤差最小化的投票方法

在網絡推理階段，需要根據關鍵點向量場的預測結果投票得到關鍵點在圖像中的坐標。在原始的投票過程中，首先在目標區域中隨機選取兩個點，將這兩個點對應的向量的交點作為候選關鍵點，重復N次獲得多組候選關鍵點，最終，基于所有像素點為候選關鍵點打分并選擇分數最高的候選關鍵點作為結果。然而在生成候選關鍵點的過程中，隨機選取的兩個點對應的向量的夾角過小或過大，在向量角度誤差相同的情況下得到的關鍵點坐標誤差相應地會更大，如圖4所示。為了簡化問題，假設p2點對應的向量沒有誤差，p1點對應的向量的角度誤差為α，關鍵點為k，p1與關鍵點k的距離為r，p1和p2點對應向量的夾角為β，則關鍵點位置的誤差為：

圖4 候選關鍵點誤差示意圖Fig.4 Geometric diagram of estimation error of candi?date key point

當β=π/2時，e取得最小值，且誤差隨著|β?π/2|的增大而增大，因此用于生成候選關鍵點的兩個向量之間的夾角應盡可能接近π/2。

在改進的投票過程中，首先隨機選取一個點，在選取第二個點時要求該點與第一個點所在向量的夾角在一定范圍內，以獲得更精確的關鍵點坐標。實驗選擇的夾角為60°~120°。

在獲取關鍵點在目標區域中的相對坐標后，需將它們重新映射到原始圖像上。假設關鍵點在目標區域中的相對坐標為（x k，y k），對應目標檢測框在原始圖像的坐標為（xmin，ymin)，(xmax，ymax)，w，h為尺度歸一化后的目標區域尺寸，（x，y）為映射后關鍵點在原始圖像中的坐標，則有：

最后建立每個關鍵點在原始圖像中的二維坐標及它在車輛坐標系中的三維坐標的對應關系，并使用EPnP［19］算法求解目標位姿。

3 數據集與實驗結果

3.1 合成數據集介紹

現有的包含有車輛位姿標注的數據集例如Kitti［20］，主要面向自動駕駛場景。它在標注過程中考慮了道路地平面假設，僅僅對車輛在地平面上的位置以及其偏航角進行了標注，降低了車輛位姿估計問題的難度，此外這些數據集中存在由于多傳感器標定與同步問題而導致的位姿錯誤標注問題，如圖5所示。

圖5 傳感器同步（左）以及地平面假設（右）導致的錯誤位姿標注［21］Fig.5 Incorrect pose annotation caused by sensor syn?chronization（left）and ground plane assumption（right）［21］

為了驗證車輛位姿估計算法的有效性，本文提出了新的車輛位姿估計數據集，利用圖像渲染技術將真實的車輛三維模型根據指定位姿渲染到圖像中，生成訓練以及測試圖像，并且提供豐富的標注信息。標注信息包括目標二維以及三維的包圍矩形框、二維圖像上的目標實例分割標注、目標關鍵點在三維車輛坐標系和所有二維圖像上的坐標標注和以及相機內參。

該數據集使用的三維車輛模型來源于Shap?eNet［22］數據集。ShapeNet是一個大型的3D形狀數據集，包含橫跨270個類別的51 000個三維模型。由于ShapeNet中的車輛三維模型缺乏實際尺寸信息，本文在網絡上查找三維車輛模型的真實尺寸，并將三維模型縮放到實際尺寸。為了增加背景的復雜性，以衡量算法在復雜場景下的能力并且防止算法對相同背景的過擬合問題，本文選擇場景分類數據集MIT places365［23］作為合成數據集的背景圖片，該數據集中包含來自365個復雜場景的180萬張圖片。在數據合成時從數據集中隨機選取背景圖片。

在車輛關鍵點選擇方面，本文在車輛三維模型表面手動選擇關鍵點，分別是4個車輪中心以及頂部的4個頂點作為關鍵點，如圖6所示。

圖6 車輛關鍵點Fig.6 Vehicle key points

在對車輛位姿進行采樣時，考慮在實際場景中相機一般不會從車輛底部進行觀察，因此相機始終在車輛的上半部分，如圖7所示。同時增大相機與車輛目標的距離，以模擬車輛在實際場景下的多尺度變換特性。最終生成了10 000張圖片，包含有目標以及對應的標注。圖8為部分生成的圖片及其標注。

圖7 車輛位姿采樣示意圖Fig.7 Schematic diagram of vehicle pose sampling

圖8 合成數據集實例（第一行為原圖，第二到四行為對應的位姿、關鍵點以及掩模標注）Fig.8 Example of synthetic dataset（the first line is the original images and the second to fourth line are the corresponding pose，key point and mask annotation）

3.2 訓練細節

尺度估計模塊中，本文采用Yolov3［24］作為目標檢測網絡。對于向量場回歸網絡，采用ResNet18［25］作為特征提取網絡，在第4和第5部分不進行降采樣，并將后續的卷積更換為空洞卷積，保證網絡最小的特征圖為原圖的1/8。采用Adam優化器，初始學習率是0.001，使用poly學習率調整策略，總共訓練50輪。多任務損失函數之間的權重設置為1，整個訓練過程使用一塊NVIDIA 2080ti顯卡完成。

3.3 評價指標

對于3D車輛檢測任務而言，3D檢測框的IOU是一個常用的評價指標；但是3D IOU的計算同時考慮了目標的位置和姿態，不能全面地衡量網絡位姿估計的性能，因此在實驗過程中采用平均定位精度［26］和平均角度精度以及平均定位誤差T和累積角度誤差R來對網絡的位姿估計性能進行全面的衡量。平均定位誤差為相機坐標系下車輛目標中心的真實位置以及預測位置之間的歐式距離，X，Y，Z為它在x軸、y軸以及z軸上的誤差分量；α，β，λ分別為車輛自坐標系下目標以x軸、y軸、z軸為旋轉中心軸的角度誤差。

3.4 實驗結果

在上文提出的合成車輛位姿數據集上對本文提出的方法進行了測試，隨機選擇其中的80%作為訓練集，20%作為測試集。表1為實驗得到的平均定位誤差以及角度誤差。

表1 實驗得到的平均定位誤差和累積角度誤差Tab.1 Average position errors and cumulated angular errors in experiment

表1 中，第一行使用PVNet作為基礎對比網絡，由于PVNet要求圖像中只能存在單個目標，為了對比的公平性，實驗中生成的數據集中每幅圖像只合成一個目標。分別對本文提出的尺度估計模塊和改進的損失函數和投票過程進行測試，可以看出使用尺度估計模塊和改進的投票方法均能提升位姿估計的準確性，特別是尺度估計模塊的位置和角度誤差分別降低了37%和32%。同時，實驗發現在基礎網絡上僅僅使用基于距離加權的損失函數會降低算法性能。經理論分析，這是由于前序分割提取的目標區域尺寸波動較大，導致損失函數中基于像素距離的權值波動較大，進而導致損失函數不穩定影響網絡性能。將尺度估計模塊和基于距離加權的損失函數結合可以進一步提高精度。定位誤差主要分布于相機坐標系的z軸方向，說明使用單目相機進行目標位置估計的難點主要在于目標深度的估計。實驗結果表明，本文提出的方法相較于基礎方法在位置和角度誤差方面分別降低了43.3%和43.5%。

圖9 為算法在測試集中的平均定位精度以及平均角度精度。同樣可以發現，尺度估計模塊對于位姿估計精度的提升較大，基于距離加權的損失函數和投票方式可以進一步提升精度。測試集中90%的目標的定位誤差在0.4 m以內，累積角度誤差在7°以內。

圖9 平均定位精度和平均角度精度Fig.9 Average position accuracy and average angle accuracy

圖10 為位姿估計結果，可以看出基于向量場的形式不僅可以檢測可見的關鍵點，對于不可見的關鍵點同樣有著很高的檢測精度，可以有效地提升對于遮擋以及截斷目標的位姿估計精度。

圖10 位姿估計結果（第一行為測試圖片，第二行為關鍵點預測結果，第三行為位姿估計結果）Fig.10 Pose estimation result（the first behavior test picture，the second behavior key point prediction result and the third behavior pose estimation result）

4 結 論

本文提出了一種基于單目相機并結合車輛三維模型進行車輛位姿估計方法，結合尺度估計模塊和向量場的中間表達形式能夠很好地適應常見的多尺度和被截斷和遮擋的車輛目標。同時還提出了基于距離的向量場損失函數，以更加有針對性地優化向量場的訓練目標，并且改進了基于RANSAC的投票機制，進一步提高了位姿估計準確性。除此之外，本文貢獻了一個含有豐富標注信息的合成數據集，在其上的驗證結果表明，所提算法的平均定位誤差和角度誤差分別為0.162 m和4.692°，相比于目前方法有顯著提升，在實際場景中有著巨大的應用價值。