重投影優(yōu)化的自由雙目相機(jī)位姿估計(jì)方法

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

（1.天津大學(xué) 精密儀器與光電子工程學(xué)院，天津 300072；2.光電信息技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，天津 300072；3.中國(guó)民航大學(xué) 電子信息與自動(dòng)化學(xué)院，天津 300300）

1 引言

自由雙目立體視覺系統(tǒng)是通過相機(jī)旋轉(zhuǎn)拍攝部分場(chǎng)景、分塊重建并拼接三維點(diǎn)云的方式，實(shí)現(xiàn)大場(chǎng)景三維重建。相機(jī)位姿是指相機(jī)坐標(biāo)系變換至世界坐標(biāo)系的旋轉(zhuǎn)矩陣和平移向量。由于雙目相機(jī)自由旋轉(zhuǎn)，導(dǎo)致左右相機(jī)的位姿不斷變化，想要實(shí)現(xiàn)自由雙目立體視覺系統(tǒng)的分塊重建和點(diǎn)云拼接，就需要準(zhǔn)確估計(jì)每個(gè)旋轉(zhuǎn)位置的左右相機(jī)位姿。

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

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

2 自由雙目相機(jī)位姿估計(jì)原理

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

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

在已知左相機(jī)運(yùn)動(dòng)參數(shù) (Rmotionl,Tmotionl)和右相機(jī)運(yùn)動(dòng)參數(shù) (Rmotionr,Tmotionr)的條件下，根據(jù)式(1)可以計(jì)算得到每次測(cè)量時(shí)左右相機(jī)的位姿。

3 基于重投影優(yōu)化的自由雙目相機(jī)位姿估計(jì)算法

3.1 旋轉(zhuǎn)相機(jī)運(yùn)動(dòng)參數(shù)估計(jì)

由圖1可以看出，相鄰兩次測(cè)量中單個(gè)相機(jī)主要做旋轉(zhuǎn)運(yùn)動(dòng)，平移距離遠(yuǎn)小于成像距離，因此可以忽略相機(jī)平移帶來(lái)的影響，將相機(jī)的運(yùn)動(dòng)近似為純旋轉(zhuǎn)模型，求解純旋轉(zhuǎn)相機(jī)的旋轉(zhuǎn)矩陣，作為相機(jī)運(yùn)動(dòng)參數(shù)的初值，再利用非線性優(yōu)化的方式得到運(yùn)動(dòng)參數(shù)的最優(yōu)解。

假設(shè)P為空間中一個(gè)三維點(diǎn)，P投影到旋轉(zhuǎn)前相機(jī)中的像點(diǎn)p1和 旋轉(zhuǎn)后相機(jī)中的像點(diǎn)p2如式(2)所示，p1和p2均為齊次坐標(biāo)，有

其中K表示相機(jī)內(nèi)參數(shù)矩陣，(R,t)表示相機(jī)旋轉(zhuǎn)前的位姿，由于相機(jī)做純旋轉(zhuǎn)運(yùn)動(dòng)，因此t在旋轉(zhuǎn)后不發(fā)生變化，Rmotionl表示相機(jī)的旋轉(zhuǎn)矩陣。

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

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

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

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

3.2 重投影誤差函數(shù)的構(gòu)建

初始位置雙目相機(jī)的相對(duì)外參數(shù)通過平面標(biāo)定靶立體標(biāo)定獲得，將初始位置的左相機(jī)坐標(biāo)系作為世界坐標(biāo)系，得到左右相機(jī)在初始位置的位姿。雙目相機(jī)第一次旋轉(zhuǎn)后，根據(jù)相機(jī)的運(yùn)動(dòng)參數(shù)和在初始位置的位姿，利用重投影優(yōu)化的位姿估計(jì)算法得到旋轉(zhuǎn)后的相機(jī)位姿。之后每次旋轉(zhuǎn)，都以上一個(gè)旋轉(zhuǎn)位置的相機(jī)位姿為基礎(chǔ)，計(jì)算當(dāng)前位置的位姿。下面將介紹重投影誤差函數(shù)的構(gòu)建。

相鄰兩次拍攝間有一定的重疊區(qū)域，區(qū)域內(nèi)的特征點(diǎn)點(diǎn)云坐標(biāo)根據(jù)上一個(gè)旋轉(zhuǎn)位置的雙目相機(jī)位姿利用三角測(cè)量法[22]計(jì)算得到。Pi=為空間中位于相鄰兩次測(cè)量重疊區(qū)域的一點(diǎn)，根據(jù)相機(jī)在上個(gè)旋轉(zhuǎn)位置的位姿和運(yùn)動(dòng)參數(shù)(Rmotionl,Tmotionl) ，點(diǎn)Pi重 投影回圖像中的坐標(biāo)為

其中，K表示相機(jī)的內(nèi)參數(shù)矩陣，si為將轉(zhuǎn)化為齊次坐標(biāo)的系數(shù)。

Pi在 相機(jī)旋轉(zhuǎn)后采集圖像中的實(shí)際成像位置為特征點(diǎn)zi，像素坐標(biāo)為。點(diǎn)Pi的 重投影誤差為實(shí)際成像坐標(biāo)zi與 重投影位置坐標(biāo)的差，即

利用重疊區(qū)域內(nèi)所有三維點(diǎn)的重投影誤差距離平方之和構(gòu)建目標(biāo)函數(shù)，有

利用LM算法對(duì)目標(biāo)函數(shù)進(jìn)行優(yōu)化，使重投影誤差之和達(dá)到最小，即可得到左相機(jī)運(yùn)動(dòng)參數(shù)的最優(yōu)解。同理可以得到右相機(jī)運(yùn)動(dòng)參數(shù)的最優(yōu)解。

3.3 重投影優(yōu)化的旋轉(zhuǎn)相機(jī)位姿估計(jì)算法流程

將純旋轉(zhuǎn)運(yùn)動(dòng)下的相機(jī)運(yùn)動(dòng)參數(shù)作為初值，計(jì)算重疊區(qū)域內(nèi)特征點(diǎn)云的重投影誤差，以重投影誤差距離之和構(gòu)建待優(yōu)化參數(shù)，作為相機(jī)運(yùn)動(dòng)參數(shù)的目標(biāo)函數(shù)，利用LM算法優(yōu)化目標(biāo)函數(shù)至最小，即可得到相機(jī)運(yùn)動(dòng)參數(shù)的最優(yōu)解，代入式(1)計(jì)算得到相機(jī)旋轉(zhuǎn)后的位姿。基于重投影優(yōu)化的自由雙目相機(jī)位姿估計(jì)算法流程如下：

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

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

（3）依據(jù)上一個(gè)旋轉(zhuǎn)位置的左右相機(jī)位姿，利用三角測(cè)量法重建兩次拍攝間重疊區(qū)域內(nèi)的特征點(diǎn)點(diǎn)云坐標(biāo)，由式(5)計(jì)算特征點(diǎn)點(diǎn)云的重投影位置坐標(biāo)，根據(jù)式(8)構(gòu)建待優(yōu)化參數(shù)為(Rmotionl,Tmotionl)的目標(biāo)函數(shù)。利用LM方法對(duì)目標(biāo)函數(shù)進(jìn)行迭代優(yōu)化，得到重投影誤差最小時(shí)左相機(jī)運(yùn)動(dòng)參數(shù)的最優(yōu)解。

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

4 實(shí)驗(yàn)結(jié)果及分析

4.1 位姿估計(jì)精度驗(yàn)證

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

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

圖2給出了旋轉(zhuǎn)估計(jì)誤差和平移估計(jì)誤差隨重投影誤差的變化。為模擬實(shí)際拍攝中噪聲的影響，在參考點(diǎn)成像時(shí)加入均值為0標(biāo)準(zhǔn)差為1的高斯噪聲，實(shí)驗(yàn)中目標(biāo)函數(shù)經(jīng)過7次迭代達(dá)到最小。由圖2可以看出，隨著重投影誤差的減小，旋轉(zhuǎn)估計(jì)誤差和平移估計(jì)誤差逐漸減小，重投影誤差最小達(dá)到69.87 pixel，此時(shí)旋轉(zhuǎn)估計(jì)誤差為0.03%，平移估計(jì)誤差為0.07%。

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

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

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

圖4給出在不同參考點(diǎn)數(shù)目條件下，位姿估計(jì)誤差的變化曲線。設(shè)置參考點(diǎn)數(shù)目為5～100，高斯噪聲的標(biāo)準(zhǔn)差為3，計(jì)算平均估計(jì)誤差。結(jié)果表明，隨著參考點(diǎn)數(shù)目的增加，所提算法的位姿估計(jì)誤差減小，在參考點(diǎn)數(shù)目大于55時(shí)，位姿估計(jì)誤差趨于平緩，旋轉(zhuǎn)估計(jì)誤差約為0.095%，平移估計(jì)誤差約為0.5%。

圖4 不同參考點(diǎn)數(shù)目對(duì)算法的影響Fig.4 Changes in post estimation error to the algorithm with different numbers of reference points

圖5給出在不同噪聲條件下，位姿估計(jì)誤差的變化曲線。設(shè)置參考點(diǎn)個(gè)數(shù)為100，高斯噪聲標(biāo)準(zhǔn)差變化由0.5～5，計(jì)算平均誤差。結(jié)果表明，隨著噪聲的增加，所提算法的位姿估計(jì)誤差增大。在高斯噪聲標(biāo)準(zhǔn)差不超過5時(shí)，旋轉(zhuǎn)估計(jì)誤差不超過0.14%，平移估計(jì)誤差不超過0.8%。

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

4.2 基于位姿估計(jì)的水泥模型重建實(shí)驗(yàn)

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

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

實(shí)驗(yàn)中計(jì)算機(jī)CPU為Intel Core i7-8700處理器，操作系統(tǒng)為Windows 10家庭版，采用Opencv實(shí)現(xiàn)圖像處理相關(guān)功能，采用ceres庫(kù)實(shí)現(xiàn)重投影優(yōu)化功能。考慮到計(jì)算機(jī)的處理能力，實(shí)驗(yàn)中對(duì)拍攝的圖像進(jìn)行降4倍采樣處理，圖像分辨率變?yōu)?440 pixel×960 pixel。

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

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

保持相機(jī)不動(dòng)，采集水泥模型左半?yún)^(qū)域的圖像。左右相機(jī)分別旋轉(zhuǎn)一定角度對(duì)水泥模型中間區(qū)域進(jìn)行拍攝，最后再旋轉(zhuǎn)一定角度對(duì)水泥模型右半?yún)^(qū)域進(jìn)行拍攝，3次拍攝得到圖像如圖7所示，其中第一行均為左圖，第二行均為右圖。

以左相機(jī)為例，針對(duì)第一次旋轉(zhuǎn)，提取圖7(a)和7(b)中的SIFT特征點(diǎn)，利用FLANN實(shí)現(xiàn)特征匹配，根據(jù)特征點(diǎn)匹配關(guān)系通過DLT方法求解單應(yīng)矩陣Hl：

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

根據(jù)式(4)分解Hl，并將結(jié)果轉(zhuǎn)化為正交矩陣形式，可以得到左相機(jī)旋轉(zhuǎn)運(yùn)動(dòng)矩陣Rmotionl。

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

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

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

利用每次拍攝時(shí)左右相機(jī)位姿對(duì)模型每個(gè)區(qū)域進(jìn)行三維重建，三維重建采用Opencv中的SGBM算法實(shí)現(xiàn)，得到結(jié)果如圖8(a)～8(c)(彩圖見期刊電子版)所示。根據(jù)每個(gè)拍攝位置的左相機(jī)位姿，將點(diǎn)云轉(zhuǎn)換至世界坐標(biāo)系下，實(shí)現(xiàn)三維點(diǎn)云的拼接，得到結(jié)果如圖8(d)(彩圖見期刊電子版)所示。利用Geomagic Studio 12在三維點(diǎn)云上對(duì)6個(gè)藍(lán)色標(biāo)志點(diǎn)的圓心進(jìn)行提取，計(jì)算AB、CD和EF間距離。重復(fù)進(jìn)行位姿估計(jì)和三維重建實(shí)驗(yàn)4次，得到結(jié)果如表1所示。

圖8 三維重建結(jié)果Fig.8 3D reconstruction results

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

表1 標(biāo)志點(diǎn)間距Tab.1 Distance between mark points （mm）

5 結(jié)論

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