PnP算法在室內(nèi)定位中的應用

席志紅，李 爽，甘興利

(1.哈爾濱工程大學 信息與通信工程學院，黑龍江 哈爾濱150001； 2.衛(wèi)星導航系統(tǒng)與裝備技術(shù)國家重點實驗室，河北 石家莊050081)

PnP算法在室內(nèi)定位中的應用

席志紅1，李 爽1，甘興利2

(1.哈爾濱工程大學 信息與通信工程學院，黑龍江 哈爾濱150001； 2.衛(wèi)星導航系統(tǒng)與裝備技術(shù)國家重點實驗室，河北 石家莊050081)

基于視覺的定位技術(shù)是近年來室內(nèi)定位研究領(lǐng)域的熱點之一，有著廣泛的應用前景。針對基于視覺的定位算法中存在的定位結(jié)果穩(wěn)定性不高、易受噪聲干擾等問題，對國內(nèi)外現(xiàn)有方法進行總結(jié)分析，提出一種結(jié)合空間參考點不確定性的相機位姿估計算法。采用模擬數(shù)據(jù)和真實數(shù)據(jù)分別進行測試，實驗結(jié)果表明，該算法具有精度高、穩(wěn)定性好等優(yōu)點，平均誤差在0.08 m，70%的點定位誤差小于0.1 m，90%的點定位誤差小于0.2 m，最大定位誤差為0.3 m，達到室內(nèi)定位精度的標準。

室內(nèi)定位；視覺定位；透視n點投影問題；相機位置估計

0 引言

隨著城市的高速發(fā)展，高樓大廈鱗次櫛比，商場、機場和車站等場所建筑規(guī)模和人流量不斷增大，據(jù)統(tǒng)計人們80%～90%的生活時間處于室內(nèi)環(huán)境中，因此精確、便捷的室內(nèi)位置服務(wù)存在廣泛的需求[1]。目前室內(nèi)定位技術(shù)主要包括：藍牙、WiFi、超寬帶、地磁和視覺等方式，其中基于視覺的定位技術(shù)具有非接觸、測量范圍廣等優(yōu)點[2]。

基于視覺的定位系統(tǒng)有：EasyLiving室內(nèi)視覺定位系統(tǒng)和Trip定位系統(tǒng)等。EasyLiving系統(tǒng)采用2個彩色三維相機，能夠同時實現(xiàn)室內(nèi)環(huán)境中的多目標的識別與定位。但由于三維點云處理計算量巨大，對處理器的計算能力要求非常高，因此該系統(tǒng)成本昂貴，不適宜大范圍推廣[3]。Trip系統(tǒng)采用普通的CCD相機和二維圓形條形碼標簽，利用二維碼識別技術(shù)進行目標的識別和定位。該系統(tǒng)對處理器運算能力的要求較低，因此系統(tǒng)的成本較低。缺點是要求待定位目標必須攜帶二維條形碼標簽且能夠被相機有效采集[4]。

針對上述方法中存在的成本高、需要攜帶特定標識物等不足，本文采用基于PnP問題求解的室內(nèi)定位方法，具有成本低且攜帶方便等優(yōu)勢[5]。針對PnP問題求解算法中存在的穩(wěn)定性差、易受噪聲干擾等問題進行研究，提出一種結(jié)合空間參考點不確定性的位姿估計算法并進行實驗驗證。

1 PnP問題

利用3D/2D對應點關(guān)系求解相機位姿的問題稱為透視n點投影(Perspective-n-Points,PnP)問題。PnP問題起源于1841年的攝影測量領(lǐng)域，由Fishler和Bolles于1981年正式提出，是計算機視覺領(lǐng)域的一個經(jīng)典問題[6]。PnP問題又稱為給定控制點的位姿估計問題，即在給定相機內(nèi)部參數(shù)條件下，拍攝一幅包含n個空間參考點的圖像，已知這n個參考點在世界坐標系下和其對應投影點在圖像坐標系下的坐標，通過它們之間的對應關(guān)系，計算相機在拍攝圖像時所處的位置和姿態(tài)[7]。

2009年Lepetit和Moreno提出的高精度快速位姿估計算法EPnP(Efficient Perspective-n-Point)，其核心思想是將空間參考點都通過4個虛擬控制點來表示，再通過空間參考點和對應投影點之間的關(guān)系求解虛擬控制點的攝像機坐標，然后求得所有空間點在相機坐標系下的坐標，最后用Horn絕對定位算法求得旋轉(zhuǎn)矩陣和平移向量[8]。楊森和吳福朝在文獻[9]提出了加權(quán) DLT 算法和加權(quán)EPnP算法，雖然算法的時間復雜度略有增加，但位姿估計的精度進一步提高；Chen借鑒EPnP算法的思想，提出一種基于虛擬控制點的迭代算法，其位姿估計的精度和魯棒性較以往迭代算法均有所提高[10]。RPnP算法由華中科技大學徐遲博士于2011年提出，其核心思想是將參考點劃分為3點子集，以三角約束為基礎(chǔ)構(gòu)造代價函數(shù)，并對代價函數(shù)求導確定相機位姿的最優(yōu)解，該方法能夠在較少冗余點的情況下得到相對精確的結(jié)果[11]。

PnP問題根據(jù)空間參考點數(shù)目n的大小，可以分成2類：一類是3≤n≤5的情況，另一類是n≥6的情況[12]。第一類情況又可以具體分為P3P問題、P4P問題和P5P問題。對于P3P問題，當3個空間參考點所決定的平面不通過光心時，最多有4個解；對于P4P問題，當 4個空間參考點共面時，有唯一解，不共面時，最多有5個解[13]；對于P5P問題，任3個參考點不共線時，最多可以有2個解[14]。這一類PnP問題由于用于計算的參考點數(shù)較少，所以通常存在多解問題且對圖像噪聲極為敏感，抗干擾能力較弱[14]。第二類PnP問題，用于計算位姿的參考點較多，所以此類情況下PnP問題研究的焦點為求解位姿的精度、時間復雜度和算法的魯棒性與抗噪性等[15]。本文在求解相機位姿的過程中使用的2D/3D參考點個數(shù)大于6，屬于PnP問題研究領(lǐng)域中的第二類情況。

空間三維參考點Pi,i=1,…,n和對應投影點在圖像坐標系下的位置pi,i=1,…,n之間的映射關(guān)系如圖1所示。

圖1 空間n點成像關(guān)系

圖1中，XwYwZwOw為世界坐標系；XcYcZcOc為圖像坐標系。

2 改進PnP問題求解算法

傳統(tǒng)的PnP問題求解算法中沒有考慮空間參考點的不確定性，因此相機位姿的求解結(jié)果穩(wěn)定性較差，且容易受到噪聲的干擾[16]。本文提出的算法在原有算法中結(jié)合了空間參考點的不確定性，具體流程如下：

假設(shè)給定一個已經(jīng)過標定的攝像頭(即相機的焦距、光心位置和畸變參數(shù)已知)、世界坐標系下的空間參考點Pi,i=1,…,n和對應參考點在相機坐標系下的點vi,i=1,…,n，通過3D/2D參考點之間的對應關(guān)系可以得到2個坐標系之間的轉(zhuǎn)換方程為：

λivi=RPi+t。

式中，λi為vi點的深度，vi滿足‖vi‖=1；R為旋轉(zhuǎn)矩陣；t為平移向量。

通過坐標系變換矩陣A將圖像坐標系下的點p′轉(zhuǎn)換到相機坐標系下的點p，則

式中，JA為坐標系變換A的雅克比矩陣，因此相機坐標系下的點p的不確定性可以表示為：

式中，協(xié)方差矩陣的秩為2，為奇異矩陣且不可逆。將點p的坐標歸一化得向量v：

式中，協(xié)方差矩陣由下述公式得出：

同樣，協(xié)方差矩陣是奇異矩陣，各分量之間相關(guān)，不滿足最大似然求解法各元素獨立的要求，因此引入零空間來表示向量v：

式中，函數(shù)f(·)為奇異值分解函數(shù)；Jvr(v)為向量v變換到向量vr的雅克比矩陣,

不確定性由向量v到向量vr的轉(zhuǎn)換表示為：

通過下式求解相機位姿：

將上式用齊次線性方程組表示為：

Bu=0,

式中，B為齊次方程組的系數(shù);

式中，u滿足約束條件‖u‖=1。對系數(shù)矩陣N進行奇異值分解,

N=UDVT,

通過奇異值分解得到旋轉(zhuǎn)矩陣：

通過上述計算過程得到相機的旋轉(zhuǎn)矩陣和平移向量初始值，構(gòu)建誤差函數(shù)采用高斯-牛頓法或L-M法迭代尋找最優(yōu)解[17]。

3 實驗及結(jié)果分析

本文實驗分為2個部分：模擬數(shù)據(jù)實驗和真實場景實驗。實驗平臺為：Inter(R) Core(TM) i7-6700 CPU，主頻：3.4 GHz，內(nèi)存大小：16.00 GB；軟件平臺為Win1064位操作系統(tǒng)，Matlab R2014a。對比實驗方法包括：本文算法、RPnP算法、EPnP算法和CEPPnP算法。

3.1 模擬數(shù)據(jù)實驗及結(jié)果分析

式中，ER,Et分別為旋轉(zhuǎn)角度平均誤差、平移向量誤差占真實平移距離的百分比；函數(shù)f(·)為旋轉(zhuǎn)矩陣到旋轉(zhuǎn)角度的轉(zhuǎn)換。

(1) 對應點數(shù)目n不同，目的比較在不同對應點數(shù)目下各種算法的計算精度。對應點數(shù)目的變化范圍為[10,150]，變化步長為10，對每個對應點數(shù)目進行200次獨立實驗，再對誤差求取平均值。實驗結(jié)果如圖2所示。

圖2 對應點數(shù)目與旋轉(zhuǎn)角度和平移向量誤差之間的關(guān)系

實驗結(jié)果分析：① 在2D/3D對應點數(shù)目n不同的情況下，通過本文算法計算得到的旋轉(zhuǎn)矩陣和平移向量的平均誤差均小于其他3種算法；② 當對應點數(shù)n<100時，4種算法獲得的旋轉(zhuǎn)矩陣和平移向量的平均誤差均隨著對應點數(shù)目n的增加而減小，當對應點數(shù)n>100時，平均誤差基本穩(wěn)定，不再隨著n的增多而繼續(xù)減小。

(2) 噪聲強度不同，目的比較各種算法在不同噪聲強度下的計算精度。實驗中設(shè)置對應點數(shù)目為100，圖像中對應點添加均值為0，標準差為σ的高斯噪聲，其中σ在[0,20]內(nèi)變化，變化步長為1。對每級噪聲做200次獨立實驗，再對誤差求平均值。實驗結(jié)果如圖3所示。

圖3 高斯噪聲標準差與R、t誤差之間的關(guān)系

實驗結(jié)果分析：① 在噪聲標準差不同的情況下，通過本文算法計算得到的旋轉(zhuǎn)矩陣和平移向量的平均誤差均小于其他3種算法；② RPnP和EPnP算法獲得的旋轉(zhuǎn)矩陣和平移向量的平均誤差隨著噪聲標準差增加而增加，本文和CEPPnP算法受噪聲標準差變化影響較小，計算結(jié)果穩(wěn)定性較好，抗噪能力較強。

3.2 真實場景實驗及結(jié)果分析

實驗場合：常規(guī)實驗室，房間長寬為6.6 m×5.9 m，在房間中選擇一點作為坐標系原點，建立世界坐標系OXwYwZw，實驗房間平面地圖如圖4所示。

圖4 實驗環(huán)境

實驗相機：羅技C270網(wǎng)絡(luò)攝像頭。測量工具：卷尺。手持攝像頭在實驗室內(nèi)沿圖4中箭頭所指的方向移動，在共記25個位置采集周圍環(huán)境圖像，圖像像素為320*240，其中圖像包含的空間參考點的世界坐標已知。真實移動路徑和各個算法求解路徑如圖5所示。

圖5 真實移動路徑和各個算法求解路徑

圖5(a)為手持攝像頭在室內(nèi)的真實移動軌跡，圖5(b)、圖5(c)、圖5(d)和圖5(e)分別為RPnP、EPnP、CEPPnP和本文算法估計的移動軌跡，從圖5(e)可得本文算法估計的移動軌跡最接近真實移動軌跡。各位置估計誤差圖如圖6所示，誤差累積分布函婁和圖如圖7所示。

圖6 各位置估計誤差

圖7 誤差累積分布函數(shù)

分析圖6和圖7可得，RPnP平均定位誤差為0.33 m，EPnP平均定位誤差為0.19 m，CEPnP平均定位誤差為0.09 m，本文算法平均定位誤差為0.08 m，70%的點定位誤差小于0.1 m，90%的點定位誤差小于0.2 m，最大誤差小于0.3 m，且定位誤差變化幅度小。

4 結(jié)束語

本文研究表明，根據(jù)對應特征點計算相機位姿的技術(shù)可以應用于室內(nèi)定位領(lǐng)域，且具有定位精度高、成本低和攜帶便捷等優(yōu)勢。重點針對定位結(jié)果穩(wěn)定性不高，提出一種結(jié)合參考不確定性的位置計算方法。實驗結(jié)果表明，本文提出的算法平均定位精度達到cm級，且抗噪聲能力強，魯棒性好，基本滿足室內(nèi)定位需求。本文的不足之處是在真實環(huán)境實驗中采用的場景規(guī)模較小，內(nèi)容較簡單，下一步應針對規(guī)模更大、內(nèi)容更加復雜的場景進行實驗。

[1] 梁韻基,周興社,於志文.普適環(huán)境室內(nèi)定位系統(tǒng)研究[J].計算機科學,2010,37(3):112-116.

[2] 趙霞,袁家政,劉宏哲.基于視覺的目標定位技術(shù)的研究進展[J].計算機科學,2016,43(6):10-16.

[3] 王娟,趙艷.基于單幅圖像目標空間定位的算法研究[J].河北科技大學學報,2011,32(4):347-350.

[4] CHOIC,CHRISTENSENHI.Robust 3D Visual Tracking Using Particle Filtering on the Special Euclidean Group:A Combined Approach of KeyPoint and Edge Features[J].International Journal of Robotics Research,2012,31(4):498-519.

[5] MULLER M,RASSWEILER M C,KLEIN J,et al.Mobile Augmented Reality for Computer Assisted Percutaneous Nephrolithotomy[J].International Journal of Computer Assisted Radiology and Surgery,2013,8(4):663-675.

[6] WU Y,HU Z Y.PnP Problem Revisited[J].Journal of Mathematical Imaging and Vision,2006,24(1):131-141.

[7] SUN F M.A Note on the Difference Between the Camera Resection and the PnP Problem[J].Acta Automatica Sinica,2005,31(3):402-405.

[8] LEPETIT V,MORENO N,FUA F.EPnP:An Accurate O(n) Solution to the PnP Problem[J].International Journalof Computer Vision,2009,81(2):155-166.

[9] 楊森,吳福朝.攝像機位姿的加權(quán)線性算法[J].軟件學報,2011,22(10):2476-2487.

[10] CHEN P,HU G D.A Simple Algorithm for Camera Pose Estimation[C]∥Proceedings of International Conference on Mechatronics and Automation.Chengdu,2012:2181-2186.

[11] LI S Q,XU C,XIE M.A Robust o(n) Solution to the Perspective-n-point Problem[J].IEEE Trans.on Pattern Anal.Mach.Intell,2012,34(7):1444-1450.

[12] KNEIP L,SCARAMUZZA D,SIEGWART R.A Novel Parametrization of the Perspective-three-point Problem for a Direct Computation of Absolute Camera Position and Orientation[C]∥In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition,2011:2969-2976.

[13] 胡占義,雷成,吳福朝.關(guān)于P4P問題的一點討論[J].自動化學報,2001,27(6):770-775.

[14] TRIGGS B.Camera Pose and Calibration from 4 or 5 Known 3d Points[C]∥International Conference on Computer on Computer Vision,1999:278-285.

[15] ZHENG Y,SUGIMOTO S,OKUTOMI M.Revisiting the PnP Problem:a Fast,General and Optimal Solution[C]∥In ICCV,2013:4321-4328.

[16] FERRAZ L,BINEFA X,MORENO N.Very Fast Solution to the PnP Problem with Algebraic Outlier Rejection[C]∥In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition,2014:501-508.

[17] FORSTNER W.Minimal Representations for Uncertainty and Estimation in Projective Spaces[C]∥In Proceedings of the Asian Conference on Computer Vision,2010:619-632.

[18] 李書杰,劉曉平.攝像機位姿的高精度快速求解[J].中國圖象圖形學報,2014,19(1):20-27.

PnPSolutionAppliedinIndoorLocation

XI Zhi-hong1,LI Shuang1,GAN Xing-li2

(1.InformationandCommunicationEngineeringInstitute,HarbinEngineeringUniversity,HarbinHeilongjiang150001,China; 2.TheStateKeyLaboratoryofSatelliteNavigationSystemandEquipmentTechnology,ShijiazhuangHebei050081,China)

Indoor position technology based on visual is a hot spot which has a wide range of applications and broad prospects.In order to solve the problem that the robustness of the existing localization algorithm is not high,and sensitive to noise,after summing of the most recent years on indoor localization based on visual,the paper put forward a method to calculate the camera position which combines with the uncertainty of spatial reference points.Through the simulation experiment on simulated data and real scenarios,the proposed algorithm has been proven to be of high accuracy and robustness.The results show that average error in 0.08 m,about 70% point positioning error is less than 0.1 m,and about 90% of the point error is less than 0.2 m,maximum error is 0.3 m,reaching the standard accuracy of indoor position.

indoor position;visual location;Perspective-n-Point;camera pose estimation

10.3969/j.issn.1003-3106.2017.010.09

席志紅，李爽，甘興利.PnP算法在室內(nèi)定位中的應用[J].無線電工程,2017,47(10):39-44.[XI Zhihong,LI Shuang,GAN Xingli.PnP Solution Applied in Indoor Location[J].Radio Engineering,2017,47(10):39-44.]

TP391.4

A

1003-3106(2017)10-0039-06

2017-01-11

國家“十三五”重點研發(fā)計劃基金資助項目(SQ2016YFGX040104)。

席志紅女，(1965—)，博士， 教授。主要研究方向：數(shù)字圖像處理。李爽男，(1990—)，碩士研究生。主要研究方向：室內(nèi)定位與導航。