PnP算法在室內定位中的應用

席志紅，李 爽，甘興利

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

PnP算法在室內定位中的應用

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

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

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

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

0 引言

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

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

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

1 PnP問題

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

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

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

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

圖1 空間n點成像關系

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

2 改進PnP問題求解算法

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

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

λivi=RPi+t。

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

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

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

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

式中，協方差矩陣由下述公式得出：

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

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

不確定性由向量v到向量vr的轉換表示為：

通過下式求解相機位姿：

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

Bu=0,

式中，B為齊次方程組的系數;

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

N=UDVT,

通過奇異值分解得到旋轉矩陣：

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

3 實驗及結果分析

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

3.1 模擬數據實驗及結果分析

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

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

圖2 對應點數目與旋轉角度和平移向量誤差之間的關系

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

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

圖3 高斯噪聲標準差與R、t誤差之間的關系

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

3.2 真實場景實驗及結果分析

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

圖4 實驗環境

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

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

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

圖6 各位置估計誤差

圖7 誤差累積分布函數

分析圖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 結束語

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

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

[2] 趙霞,袁家政,劉宏哲.基于視覺的目標定位技術的研究進展[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] 楊森,吳福朝.攝像機位姿的加權線性算法[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] 胡占義,雷成,吳福朝.關于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算法在室內定位中的應用[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

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

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