重投影優化的自由雙目相機位姿估計方法

陳天擇，葛寶臻，羅其俊,3

（1.天津大學 精密儀器與光電子工程學院，天津 300072；2.光電信息技術教育部重點實驗室，天津 300072；3.中國民航大學 電子信息與自動化學院，天津 300300）

1 引言

自由雙目立體視覺系統是通過相機旋轉拍攝部分場景、分塊重建并拼接三維點云的方式，實現大場景三維重建。相機位姿是指相機坐標系變換至世界坐標系的旋轉矩陣和平移向量。由于雙目相機自由旋轉，導致左右相機的位姿不斷變化，想要實現自由雙目立體視覺系統的分塊重建和點云拼接，就需要準確估計每個旋轉位置的左右相機位姿。

旋轉相機位姿估計算法主要包括旋轉軸標定法和特征點自標定法。旋轉軸標定法是對相機的旋轉軸參數進行預先標定，結合旋轉角度，計算相機旋轉后的位姿。鄭圣子等[1]提出一種基于多個平面標靶的旋轉軸標定法，將相機光心的運動軌跡擬合為空間圓形，得到圓心即為旋轉軸位置；李肖等[2]通過一個棋盤格平面標定靶，立體標定4個位置的雙目相機旋轉平移矩陣，利用最小二乘法求解旋轉軸參數，獲取雙目相機的動態外參數；于海等[3]利用雙線陣圖像傳感器對旋轉角度誤差進行補償，實現高精度的角位移測量。特征點自標定法主要包括：利用圖像間的多對匹配特征點和利用已知三維點與圖像特征點間匹配關系兩種。Sweeney、Moulon、Zhu和Zhuang等[4-7]利用兩視圖間的匹配特征點求解本征矩陣，通過分解本征矩陣[8]得到不同位置相機間的位姿關系，依據全局一致性同時優化所有相機在世界坐標系的位姿；杜瑞建等[9]利用單應矩陣實現高分辨率圖像和雙目圖像的特征匹配，進而得到紋理圖像與三維點云間的匹配關系；Snavely Noah、Wu C、Schonberger J L等[10-12]在已知三維點與圖像特征點匹配關系的條件下，構建3D和2D點對應問題(Perspective-n-Point, PnP)，利用直接線性變換法(Direct Linear Transform, DLT)、EPnP[13]、UPnP[14]等方法求解相機位姿。還有一類方法在已知三維點的條件下，構建并優化誤差函數，進而得到相機位姿的最優解。李正煒等[15]提出一種采用仿真圖像進行相關度局部最優搜索的姿態估計方法；Engel J等[16]提出將相機運動前的位姿作為初值，通過最小化光度誤差函數得到相機當前位姿，在相機運動連續且平滑時取得良好的效果，張可等[17]指出光流對噪聲、陰影、遮擋等環境干擾因素抵抗性較差；Li等[18]提出將當前位置之前的多幅圖像中的特征點云重投影回當前圖像，保證有足夠的特征點用來優化相機位姿；Cui等[19]提出適用于運動恢復結構(Structure from Motion,SfM)的位姿獲取方法，將利用P3P方法得到的相機位姿作為初值，通過最小化特征點的重投影誤差函數，得到優化后的相機位姿；周單等[20]提出基于自適應重投影誤差單目位姿優化算法，以位姿初值為中心設置約束區間，通過建立重投影誤差和約束區間的關系，自動調節約束區間，進而對位姿參數進行約束非線性優化；趙亞鳳等[21]提出通過非線性優化確定兩對正交消隱點的最優位置，進而求得雙目相機的外參數。

針對相機位姿動態變化的自由雙目立體視覺系統，本文提出了一種重投影優化的自由雙目相機位姿估計算法。通過相機的位姿將空間三維點投影回圖像坐標系，計算得到重投影位置坐標，重投影位置和該三維點在圖像中實際成像位置間的距離，稱為重投影誤差。首先，利用平面標定靶立體標定初始位置雙目相機的位姿；然后，在每次旋轉后，根據上一個旋轉位置的左右相機位姿，利用三角測量得到相鄰兩次測量間重疊區域內特征點的三維點云坐標；針對每個相機，利用旋轉前后拍攝的兩幅圖像中的特征點匹配關系求解單應矩陣，將分解單應矩陣得到的相機運動參數作為初值，結合上一個旋轉位置的相機位姿，將兩幅圖像重疊區域內的特征點點云重投影回圖像坐標系，計算得到重投影誤差之和，將其作為代價函數，利用列文伯格-馬爾夸特(Levenberg-Marquardt, LM)算法優化代價函數，得到相機運動參數的最優解，進而獲取相機旋轉后的位姿。

2 自由雙目相機位姿估計原理

自由雙目立體視覺系統由兩個可自由旋轉的相機構成，它通過旋轉掃描獲取目標表面的分片點云，然后合成大場景的三維模型。自由雙目立體視覺系統工作過程中，每個相機可自由旋轉。如圖1所示，第k次測量時，雙目相機對Ωk區域成像，估計每個相機在世界坐標系的位姿信息并進行三維重建，得到Ωk區域的三維點云面片Ck。然后左右每個相機繞各自的旋轉軸旋轉，對Ωk+1區域成像，相鄰兩次測量之間有一定比例的重疊區域，利用重疊區域的特征點點云估計第k+1次測量時雙目相機的位姿并進行三維重建，得到三維點云面片Ck+1。最后，將所有點云面片轉換至世界坐標系中，實現點云拼接，得到完整場景的三維點云結構。左右相機位姿估計原理相同，以左相機為例，第k次測量時，左相機在世界坐標系下的位姿為 (Rlk,tlk)。相機繞旋轉軸旋轉一定角度到達第k+1次測量的位置，相機的旋轉和平移運動用參數Rmotionl和Tmotionl表示，此時左相機的位姿定義為

圖1 自由雙目立體視覺系統中相鄰兩次測量Fig.1 Two adjacent measurements of the free binocular vision system

在已知左相機運動參數 (Rmotionl,Tmotionl)和右相機運動參數 (Rmotionr,Tmotionr)的條件下，根據式(1)可以計算得到每次測量時左右相機的位姿。

3 基于重投影優化的自由雙目相機位姿估計算法

3.1 旋轉相機運動參數估計

由圖1可以看出，相鄰兩次測量中單個相機主要做旋轉運動，平移距離遠小于成像距離，因此可以忽略相機平移帶來的影響，將相機的運動近似為純旋轉模型，求解純旋轉相機的旋轉矩陣，作為相機運動參數的初值，再利用非線性優化的方式得到運動參數的最優解。

假設P為空間中一個三維點，P投影到旋轉前相機中的像點p1和 旋轉后相機中的像點p2如式(2)所示，p1和p2均為齊次坐標，有

其中K表示相機內參數矩陣，(R,t)表示相機旋轉前的位姿，由于相機做純旋轉運動，因此t在旋轉后不發生變化，Rmotionl表示相機的旋轉矩陣。

令相機旋轉前的相機坐標系作為世界坐標系，則R為單位矩陣，t為零向量。將式(2)中的兩式合并，可以得到p1和p2間的關系為

將KRmotionlK?1合并表示為矩陣H，即可得：

合并式(4)和式(3)可以得到兩幅圖像中匹配特征點間的對應關系為p2=Hp1。單應矩陣描述的是一組三維點在兩幅圖像間的變換關系，單應矩陣Hm和一對匹配特征點p1和p2的表達式為p2=Hmp1。 可以看出H和單應矩陣Hm的形式相同，因此可以通過求解單應矩陣的方式求解矩陣H。提取并匹配相機旋轉前后拍攝的兩幅圖像中的特征點，根據匹配關系利用DLT算法求得單應矩陣H。在已知內參數矩陣的條件下，通過式(4)計算相機的旋轉運動參數Rmotionl。

由于旋轉矩陣是正交矩陣，而由式(4)得到的Rmotionl可能不滿足該性質，因此可以利用SVD分解將Rmotionl轉 化為正交矩陣的形式。對Rmotionl進行SVD分解，用單位矩陣代替Rmotionl的奇異值構成的對角矩陣，即可得到正交矩陣形式的Rmotionl。

3.2 重投影誤差函數的構建

初始位置雙目相機的相對外參數通過平面標定靶立體標定獲得，將初始位置的左相機坐標系作為世界坐標系，得到左右相機在初始位置的位姿。雙目相機第一次旋轉后，根據相機的運動參數和在初始位置的位姿，利用重投影優化的位姿估計算法得到旋轉后的相機位姿。之后每次旋轉，都以上一個旋轉位置的相機位姿為基礎，計算當前位置的位姿。下面將介紹重投影誤差函數的構建。

相鄰兩次拍攝間有一定的重疊區域，區域內的特征點點云坐標根據上一個旋轉位置的雙目相機位姿利用三角測量法[22]計算得到。Pi=為空間中位于相鄰兩次測量重疊區域的一點，根據相機在上個旋轉位置的位姿和運動參數(Rmotionl,Tmotionl) ，點Pi重 投影回圖像中的坐標為

其中，K表示相機的內參數矩陣，si為將轉化為齊次坐標的系數。

Pi在 相機旋轉后采集圖像中的實際成像位置為特征點zi，像素坐標為。點Pi的 重投影誤差為實際成像坐標zi與 重投影位置坐標的差，即

利用重疊區域內所有三維點的重投影誤差距離平方之和構建目標函數，有

利用LM算法對目標函數進行優化，使重投影誤差之和達到最小，即可得到左相機運動參數的最優解。同理可以得到右相機運動參數的最優解。

3.3 重投影優化的旋轉相機位姿估計算法流程

將純旋轉運動下的相機運動參數作為初值，計算重疊區域內特征點云的重投影誤差，以重投影誤差距離之和構建待優化參數，作為相機運動參數的目標函數，利用LM算法優化目標函數至最小，即可得到相機運動參數的最優解，代入式(1)計算得到相機旋轉后的位姿。基于重投影優化的自由雙目相機位姿估計算法流程如下：

（1）標定初始位置雙目相機的相對外參數，將左相機坐標系作為世界坐標系，確定雙目相機的位姿(Rl0,tl0)和 (Rr0,tr0)，令k=1。

（2）由于左右相機位姿的估計方法相同，以左相機為例，根據相機第k次旋轉前后拍攝的圖像中特征點的匹配關系，利用直接線性變換法求解單應矩陣H。相機在純旋轉運動時，不發生平移，依據式(4)分解H得到相機旋轉運動矩陣Rmotionl，作為相機運動參數的初值。

（3）依據上一個旋轉位置的左右相機位姿，利用三角測量法重建兩次拍攝間重疊區域內的特征點點云坐標，由式(5)計算特征點點云的重投影位置坐標，根據式(8)構建待優化參數為(Rmotionl,Tmotionl)的目標函數。利用LM方法對目標函數進行迭代優化，得到重投影誤差最小時左相機運動參數的最優解。

（4）同理對右相機進行步驟（2）和步驟（3），得到右相機運動參數的最優解。將和分別代入式(1)，得到第k次旋轉后的左右相機位姿 (Rlk,tlk)、(Rrk,trk) 。將(Rlk,tlk)和 (Rrk,trk)作為條件，令k=k+1，重復步驟（2）至步驟（4），即可得到每次旋轉后左右相機的位姿。

4 實驗結果及分析

4.1 位姿估計精度驗證

利用opencv和ceres庫進行仿真實驗模擬。仿真實驗中相機的等效焦距為96000 pixel，分辨率為5760 pixel×3840 pixel，參考點為在世界坐標系 區 域[?650, 650]×[?400, 400]×[24500, 25500]內隨機分布的若干個點，單位為mm。仿真實驗流程為：左相機光心位于世界坐標系原點，以朝向z軸正方向的位姿拍攝參考點圖像，右相機光心位于(5000, 0, 0)位置，朝向(?1, 0, 5)方向拍攝參考點圖像；模擬旋轉軸偏離光心的情況，雙目相機的旋轉軸均位于沿著相機朝向的負方向偏離100 mm的位置，旋轉軸方向與世界坐標系y軸平行；為保證1次旋轉后雙目相機的視場有50%的重疊比例，左相機繞旋轉軸旋轉2.1°，右相機繞旋轉軸旋轉1.7°，同時拍攝參考點圖像。每種條件下重復運行50次仿真實驗，消除參考點不同分布形式對位姿估計的影響，相機位姿的旋轉和平移估計誤差為[20]。

其中Rture和tture代表仿真實驗中位姿的設定值，tr(·)代表矩陣的跡。

圖2給出了旋轉估計誤差和平移估計誤差隨重投影誤差的變化。為模擬實際拍攝中噪聲的影響，在參考點成像時加入均值為0標準差為1的高斯噪聲，實驗中目標函數經過7次迭代達到最小。由圖2可以看出，隨著重投影誤差的減小，旋轉估計誤差和平移估計誤差逐漸減小，重投影誤差最小達到69.87 pixel，此時旋轉估計誤差為0.03%，平移估計誤差為0.07%。

圖2 位姿估計誤差隨重投影誤差的變化Fig.2 Changes in pose estimation error with reprojection error

研究不同參考點個數和不同噪聲條件下，利用所提算法和P3P+LM[19]方法估計相機位姿時，重投影誤差代價的變化過程。實驗中參考點個數分別為15、45、75，高斯噪聲標準差σ分別為1、2、3，LM算法的迭代次數設置為50，得到結果如圖3（彩圖見期刊電子版）所示。結果表明，針對自由雙目立體視覺中的相機位姿估計問題，所提算法相比P3P+LM能更快速、更穩定收斂于全局最優解。在實驗條件下，所提算法收斂至全局最優解所需的迭代步數相比P3P+LM算法更少。原因是所提算法中傳遞給重投影優化的運動參數初值和全局最優值相近，更容易收斂；而P3P得到的位姿初始值相比真值誤差較大，導致優化算法難以收斂。

圖3 不同條件下算法的收斂情況Fig.3 Convergence of the algorithm under various conditions

圖4給出在不同參考點數目條件下，位姿估計誤差的變化曲線。設置參考點數目為5～100，高斯噪聲的標準差為3，計算平均估計誤差。結果表明，隨著參考點數目的增加，所提算法的位姿估計誤差減小，在參考點數目大于55時，位姿估計誤差趨于平緩，旋轉估計誤差約為0.095%，平移估計誤差約為0.5%。

圖4 不同參考點數目對算法的影響Fig.4 Changes in post estimation error to the algorithm with different numbers of reference points

圖5給出在不同噪聲條件下，位姿估計誤差的變化曲線。設置參考點個數為100，高斯噪聲標準差變化由0.5～5，計算平均誤差。結果表明，隨著噪聲的增加，所提算法的位姿估計誤差增大。在高斯噪聲標準差不超過5時，旋轉估計誤差不超過0.14%，平移估計誤差不超過0.8%。

圖5 不同噪聲對算法的影響Fig.5 Changes in post estimation error to the algorithm under different noise

4.2 基于位姿估計的水泥模型重建實驗

搭建自由雙目立體視覺系統如圖6所示，用兩部5DMarkⅢ單反相機，分辨率為5760 pixel×3840 pixel，像元尺寸為0.00625 mm，鏡頭焦距為1200 mm（600 mm定焦鏡頭+2倍增倍鏡）。綜合考慮相機視場范圍、系統的空間分辨率，系統基線設置為8.4 m，測量距離設置為48 m。分區域拍攝水泥模型的圖像，估計每個位置的相機位姿，利用位姿進行三維重建并拼接點云模型，通過測量模型上特征點間距離，驗證所提算法估計相機位姿的有效性。

圖6 自由雙目立體視覺裝置圖Fig.6 Diagram of free binocular stereo vision device

實驗中計算機CPU為Intel Core i7-8700處理器，操作系統為Windows 10家庭版，采用Opencv實現圖像處理相關功能，采用ceres庫實現重投影優化功能。考慮到計算機的處理能力，實驗中對拍攝的圖像進行降4倍采樣處理，圖像分辨率變為1440 pixel×960 pixel。

實驗過程為：首先，確定相機的內參數。根據鏡頭的物理焦距和像元尺寸計算得到等效焦距為48000 pixel，光心位于像面中心，由于長焦鏡頭的畸變較小，且成像距離較遠，忽略畸變對成像的影響。利用每格寬度為60 mm的棋盤格平面標定靶對初始位置雙目相機的相對外參數進行立體標定，棋盤格上有角點10行7列共70個，通過棋盤格標定確定平移向量的單位為mm，標定得到結果為(R0,T0)：

將初始位置的左相機坐標系作為世界坐標系，得到左相機位姿(Rl0,tl0)和 右相機位姿(Rr0,tr0)：

保持相機不動，采集水泥模型左半區域的圖像。左右相機分別旋轉一定角度對水泥模型中間區域進行拍攝，最后再旋轉一定角度對水泥模型右半區域進行拍攝，3次拍攝得到圖像如圖7所示，其中第一行均為左圖，第二行均為右圖。

以左相機為例，針對第一次旋轉，提取圖7(a)和7(b)中的SIFT特征點，利用FLANN實現特征匹配，根據特征點匹配關系通過DLT方法求解單應矩陣Hl：

圖7 雙目相機采集圖像Fig.7 Images obtained by binocular cameras

根據式(4)分解Hl，并將結果轉化為正交矩陣形式，可以得到左相機旋轉運動矩陣Rmotionl。

根據旋轉前相機位姿 (Rl0,tl0)和 (Rr0,tr0)利用Opencv中triangulation函數重建圖7(a)和7(b)重疊區域的特征點點云。相機在純旋轉運動時，不發生平移。將 (Rl0,tl0)和Rmotionl代入式(5)計算每個點云的重投影坐標，根據式(8)構建重投影誤差目標函數。圖7(c)和圖7(e)分別為2次旋轉后的左圖和右圖，利用LM算法優化目標函數至最小，得到左相機運動參數的最優解。同理，利用圖7(d)和7(e)中的特征點匹配關系求解單應矩陣，分解單應矩陣得到右相機旋轉運動參數Rmotionr，作為初值構建重投影誤差目標函數，利用LM算法優化目標函數至最小，得到右相機運動參數的最優解

將 (Rl0,tl0)和代入式(1)，得到第1次旋轉后的左相機位姿 (Rl1,tl1)。同理可得第1次旋轉后的右相機位姿(Rr1,tr1)：

以第1次旋轉后的相機位姿(Rl1,tl1)，(Rr1,tr1)作為已知，重復上述過程，同理得到第2次旋轉后左相機位姿(Rl2,tl2)和 右相機位姿(Rr2,tr2)：

利用每次拍攝時左右相機位姿對模型每個區域進行三維重建，三維重建采用Opencv中的SGBM算法實現，得到結果如圖8(a)～8(c)(彩圖見期刊電子版)所示。根據每個拍攝位置的左相機位姿，將點云轉換至世界坐標系下，實現三維點云的拼接，得到結果如圖8(d)(彩圖見期刊電子版)所示。利用Geomagic Studio 12在三維點云上對6個藍色標志點的圓心進行提取，計算AB、CD和EF間距離。重復進行位姿估計和三維重建實驗4次，得到結果如表1所示。

圖8 三維重建結果Fig.8 3D reconstruction results

在實際模型上，用鋼尺測量AB、CD和EF間的距離3次，得到實際測量距離的平均值分別為506.3 mm、204.5 mm和507.8 mm。由 表1知，計算得到AB、CD和EF相比實際測量距離的平均誤差分別為1.12%、2.77%和1.14%，對3個誤差取平均值，得到模型表面兩點間的平均測量誤差為1.68%。

表1 標志點間距Tab.1 Distance between mark points （mm）

5 結論

本文針對自由雙目立體視覺中遇到的相機自由旋轉，導致位姿動態變化的問題，提出一種重投影優化的自由雙目相機位姿估計算法。將相機運動近似為純旋轉模型，通過分解單應矩陣求解相機運動參數，將其作為初值構建重投影誤差目標函數并進行優化，得到相機運動參數的最優解，結合相機旋轉前的位姿，計算相機旋轉后的位姿。該方法無需標定相機的旋轉軸，不受限于對獲取旋轉臺旋轉角度的精度要求。通過仿真實驗結果可知，利用所提算法得到相機位姿的估計誤差隨重投影誤差減小而減小，所提算法相比P3P+LM算法能快速穩定收斂至全局最優；通過水泥模型的三維重建實驗，拼接得到的三維模型上的兩點距離平均測量誤差為1.68%。本文所提算法可以用于解決自由雙目立體視覺系統中的旋轉相機位姿估計問題，進而實現大場景的三維重建。