本發(fā)明涉及一種可應(yīng)用于增強(qiáng)現(xiàn)實(shí)、虛擬現(xiàn)實(shí)、機(jī)器人導(dǎo)航等領(lǐng)域的同時(shí)定位與稠密三維重建方法，尤其涉及一種同時(shí)定位與稠密三維重建方法。

背景技術(shù)：

同時(shí)定位與地圖構(gòu)建(SimultaneouslyLocatingandMapping，SLAM)是機(jī)器人和計(jì)算機(jī)視覺(jué)領(lǐng)域的一個(gè)基本問(wèn)題。在過(guò)去的幾時(shí)年來(lái)，實(shí)時(shí)運(yùn)動(dòng)結(jié)構(gòu)學(xué)(StructurefromMotion)和視覺(jué)SLAM有很多成功的應(yīng)用。然而，視覺(jué)SLAM難以處理無(wú)紋理的場(chǎng)景，而且要做到實(shí)時(shí)穩(wěn)定地恢復(fù)稠密三維結(jié)構(gòu)也比較困難。使用深度傳感器可以幫助解決這兩個(gè)問(wèn)題。隨著深度傳感器(如微軟Kinect和英特爾RealSense 3D相機(jī))的流行，大量基于RGB-D的SLAM方案得到提出。

大多數(shù)稠密的SLAM方法使用幀到幀或幀到模型的對(duì)準(zhǔn)方法，這類(lèi)方法很容易累計(jì)誤差，在具有挑戰(zhàn)性的場(chǎng)景下容易失敗。一些方法提出使用非剛性網(wǎng)格變形優(yōu)化模型，減小累積誤差。然而由于不精確的對(duì)準(zhǔn)導(dǎo)致的模型誤差無(wú)法得到消除，并且會(huì)進(jìn)一步增大對(duì)準(zhǔn)誤差。

最近，BundleFusion[A.Dai,M.Nieβner,M.S.Izadi,and C.Theobalt.BundleFusion:Real-time globally consistent 3d reconstruction using online surface re-integration.arXiv preprint arXiv:1604.01093,2016.]提出了端到端的實(shí)時(shí)重建系統(tǒng)。BundleFusion使用所有的RGB-D輸入數(shù)據(jù)，層次化的優(yōu)化所有的相機(jī)姿態(tài)和三維模型。不同于前面的幀到模型的對(duì)準(zhǔn)方法，BundleFusion采用每一幀和所有幀的暴力匹配求解相機(jī)姿態(tài)。為此，BundleFusion使用兩顆強(qiáng)GPU(NVIDIA GeForce GTX Titan X和GTX Titan Black)以達(dá)到實(shí)時(shí)運(yùn)行。同時(shí)，它保存所有的輸入數(shù)據(jù)，有極大的內(nèi)存消耗從而限制了其運(yùn)行場(chǎng)景大小和運(yùn)行時(shí)間。

近年來(lái)，許多研究者提出了不同的基于RGB-D相機(jī)進(jìn)行稠密三維重建和實(shí)時(shí)同事定位與地圖構(gòu)建的方法。Huang[A.S.Huang,A.Bachrach,P.Henry,M.Krainin,D.Maturana,D.Fox,and N.Roy.Visual odometry and mapping for autonomous flight using an RGB-D camera.In Robotics Research-The 15th International Symposium ISRR,9-12 December 2011,Flagstaff,Arizona,USA,pages 235–252,2011.]等人提出使用RGB-D數(shù)據(jù)進(jìn)行實(shí)時(shí)的運(yùn)動(dòng)測(cè)量，使用稀疏的BA進(jìn)行離線(xiàn)稠密三維重建。Endres[F.Endres,J.Hess,J.Sturm,D.Cremers,and W.Burgard.3-d mapping with an rgb-d camera.IEEE Transactions on Robotics,30(1):177–187,2014.]等提出使用視覺(jué)特征結(jié)合深度信息估計(jì)相機(jī)姿態(tài)，用三維網(wǎng)格占據(jù)率表示場(chǎng)景。Kerl[C.Kerl,J.Sturm,and D.Cremers.Dense visual SLAM for RGB-D cameras.In 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems,Tokyo,Japan,November 3-7,2013,pages 2100–2106,2013]等提出使用稠密的RGB-D數(shù)據(jù)，直接最小化光學(xué)誤差和深度誤差來(lái)實(shí)現(xiàn)相機(jī)姿態(tài)估計(jì)，這種方法比基于稀疏視覺(jué)特征的方法具有更高的精度。Newcombe[.R.A.Newcombe,S.Izadi,O.Hilliges,D.Molyneaux,D.Kim,A.J.Davison,P.Kohli,J.Shotton,S.Hodges,and A.W.Fitzgibbon.Kinectfusion:Real-time dense surface mapping and tracking.In 10th IEEE International Symposium on Mixed and Augmented Reality,ISMAR 2011,Basel,Switzerland,October 26-29,2011,pages 127–136,2011]等提出了KinectFusion，使用基于體素的表示重建三維模型，通過(guò)ICP算法將每一幀對(duì)準(zhǔn)到模型上，實(shí)驗(yàn)表明其在小尺度下有很好的重建質(zhì)量。然而，受制于GPU內(nèi)存的限制，KinectFusion不適用于大尺度的場(chǎng)景重建。同時(shí)，KinectFusion也不能處理漂移問(wèn)題和回路閉合。許多工作對(duì)KinectFusion進(jìn)行了改進(jìn)，這些工作主要集中在使用更有效的數(shù)據(jù)結(jié)構(gòu)實(shí)現(xiàn)大尺度場(chǎng)景的重建。例如，Kintinuous[T.Whelan,J.McDonald,M.Kaess,M.Fallon,H.Johannsson,and J.Leonard.Kintinuous:Spatially extended Kinect-Fusion.In RSS Workshop on RGB-D:Advanced Reasoning with Depth Cameras,Sydney,Australia,Jul 2012.]使用移動(dòng)體積的方法改進(jìn)KinectFusion。Nieβner[M.Nieβner,M.S.Izadi,and M.Stamminger.Real-time 3d reconstruction at scale using voxel hashing.ACM Trans.Graph.,32(6):169:1–169:11,2013.]等提出用空間哈希的方法存儲(chǔ)稀疏的體素信息。然而，這些方法都沒(méi)有解決相機(jī)姿態(tài)估計(jì)的漂移問(wèn)題和回路閉合所要求的模型調(diào)整。

無(wú)漂移的相機(jī)姿態(tài)估計(jì)和稀疏重建在視覺(jué)SLAM中得到了大量的研究。許多單目SLAM方法可以在小場(chǎng)景甚至街道大小的場(chǎng)景進(jìn)行實(shí)時(shí)相機(jī)跟蹤與稀疏三維重建。一些方法也可以處理重定位與回路閉合。然而，這些方法都不能產(chǎn)生稠密的三維模型。一些可以實(shí)時(shí)重建稠密三維模型的方法大都受制于小的場(chǎng)景或沒(méi)有考慮無(wú)漂移的三維稠密重建。

一些離線(xiàn)方法可以通過(guò)回路閉合實(shí)現(xiàn)無(wú)漂移的稠密三維重建，然而其速度遠(yuǎn)達(dá)不到實(shí)時(shí)。最近，Whelan[T.Whelan,M.Kaess,H.Johannsson,M.F.Fallon,J.J.Leonard,and J.McDonald.Real-time large-scale dense RGB-D SLAM with volumetric fusion.I.J.Robotics Res.,34(4-5):598–626,2015.]等提出基于體素融合的RGB-D SLAM系統(tǒng)可以在大場(chǎng)景下檢測(cè)到回路閉合，通過(guò)盡量剛性的模型表面變形糾正相機(jī)漂移帶來(lái)的模型誤差。ElasticFusion[T.Whelan,S.Leutenegger,R.F.Salas-Moreno,B.Glocker,and A.J.Davison.ElasticFusion:Dense SLAM without A pose graph.In Robotics:Science and Systems XI,Sapienza University of Rome,Rome,Italy,July 13-17,2015,2015.]使用面片來(lái)表示三維模型，在回路閉合的時(shí)同樣采用盡量剛性的模型變換矯正三維模型。這些方法使用幀到模型的對(duì)準(zhǔn)，對(duì)準(zhǔn)時(shí)的誤差會(huì)影響模型精度，而不精準(zhǔn)的模型又會(huì)降低相機(jī)追蹤的精度。在回路閉合時(shí)使用表面變形無(wú)法糾正模型錯(cuò)誤，最終會(huì)導(dǎo)致在復(fù)雜場(chǎng)景下追蹤失敗。BundleFusion[A.Dai,M.Nieβner,M.S.Izadi,and C.Theobalt.BundleFusion:Real-time globally consistent 3d reconstruction using online surface reintegration.arXiv preprint arXiv:1604.01093,2016.]使用暴力匹配進(jìn)行每一幀的對(duì)準(zhǔn)，在調(diào)整姿態(tài)時(shí)重新集成，從而獲得全局一致的模型。然而，BundleFusion保存所有的輸入數(shù)據(jù)，限制了其在長(zhǎng)序列數(shù)據(jù)上的運(yùn)行。同時(shí)，即使用很強(qiáng)的GPU，在長(zhǎng)序列輸入時(shí)使用暴力匹配也是非常耗時(shí)的。本發(fā)明使用基于關(guān)鍵幀的表示和追蹤框架，可以不斷調(diào)整相機(jī)姿態(tài)并重新融合深度圖。本發(fā)明的內(nèi)存消耗是和關(guān)鍵幀的數(shù)據(jù)成線(xiàn)性的，因此在相當(dāng)規(guī)模的場(chǎng)景下，由于關(guān)鍵幀的數(shù)目受限，可以無(wú)限長(zhǎng)時(shí)間的運(yùn)行。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明針對(duì)上述技術(shù)的不足，提出了一種同時(shí)定位與稠密三維重建方法，能夠可靠的處理強(qiáng)烈旋轉(zhuǎn)和快速運(yùn)動(dòng)，支持重定位并能實(shí)時(shí)地構(gòu)建與調(diào)整稠密三維模型。

本發(fā)明的同時(shí)定位與稠密三維重建方法包括如下步驟：

1)通過(guò)RGB-D對(duì)準(zhǔn)估計(jì)相機(jī)初始姿態(tài)，使用基于單應(yīng)性的特征匹配優(yōu)化相機(jī)姿態(tài)；

2)運(yùn)行“增量集束調(diào)整”算法，自適應(yīng)地更新、優(yōu)化關(guān)鍵幀的相機(jī)姿態(tài)；

3)將非關(guān)鍵深度圖融合到關(guān)鍵幀，基于關(guān)鍵幀的深度圖重建三維模型，通過(guò)重融合在相機(jī)姿態(tài)改變時(shí)更新三維模型。

通過(guò)RGB-D對(duì)準(zhǔn)估計(jì)相機(jī)初始姿態(tài)，使用基于單應(yīng)性的特征匹配優(yōu)化相機(jī)姿態(tài)的具體步驟為：

對(duì)每個(gè)當(dāng)前幀F(xiàn)_i，首先使用RGB-D對(duì)準(zhǔn)估計(jì)相機(jī)從上一幀到當(dāng)前幀的相對(duì)姿態(tài)T_(i-1)→i，這里只需要一個(gè)相對(duì)粗糙的估計(jì)，因此使用小模糊圖(SBI)估計(jì)兩幀相對(duì)姿態(tài)以在CPU上達(dá)到實(shí)時(shí)運(yùn)行。其中參考文獻(xiàn)列舉的方法為可選用的現(xiàn)有方法。

精確的相機(jī)姿態(tài)是通過(guò)特征匹配獲得的，為處理劇烈旋轉(zhuǎn)和快速運(yùn)動(dòng)，本發(fā)明使用基于單應(yīng)性的特征匹配方法求解相機(jī)姿態(tài)。在劇烈旋轉(zhuǎn)和快速運(yùn)動(dòng)下，兩幀之間的透視變形可能會(huì)很大，以至于基于特征的匹配不能魯棒運(yùn)行。單應(yīng)性可以用來(lái)糾正圖片塊的透視變形，從而使零均值的SSD可行。單應(yīng)性也可以用來(lái)決定一個(gè)較小的搜索區(qū)域避免沿著整個(gè)對(duì)極線(xiàn)搜索。在本發(fā)明中，由于有深度信息z_k以及使用RGB-D對(duì)準(zhǔn)的相機(jī)姿態(tài)估計(jì)C_i＝T_(i-1)→iC_i-1，因此將搜索區(qū)域定為以及

在確定了相對(duì)姿態(tài)T_(i-1)→i以及3D-2D特征匹配的集合χ＝{(X_j,x_j)}后，通過(guò)最小化相對(duì)誤差與重投影誤差來(lái)估計(jì)相機(jī)姿態(tài)C_i：

這里，log(T)將3D剛性變換T∈SE(3)映射到se(3)，并返回R⁶中的向量表示。是標(biāo)準(zhǔn)馬氏距離。是投影函數(shù)，K是已知并且固定的相機(jī)內(nèi)參。z(X)從X中提取出z分量，z_j是x_j在當(dāng)前幀的深度。||∈||_δ是Huber范數(shù)：

如果追蹤失敗，則重新跟蹤特征。否則，則使用新特征的深度信息，用貝葉斯方法對(duì)特征的深度進(jìn)行濾波。對(duì)關(guān)鍵幀F(xiàn)_k和當(dāng)前幀F(xiàn)_i的每一對(duì)特征匹配(x_i,x_j)，獲取x_k深度的倒數(shù)ρ_k：

這里，通過(guò)ρ_i→k，濾波器不斷更新ρ_k和其內(nèi)點(diǎn)的聯(lián)合分布。

最后，本發(fā)明在每一新幀到來(lái)時(shí)決定是否選作關(guān)鍵幀。

所述的運(yùn)行“增量集束調(diào)整”算法，自適應(yīng)地更新、優(yōu)化關(guān)鍵幀的相機(jī)姿態(tài)，其具體步驟為：

當(dāng)有新的關(guān)鍵幀加入或者檢測(cè)到閉合回路的時(shí)候，需要運(yùn)行BA。在前一種情形下，由于只有局部地圖調(diào)整，沒(méi)有必要運(yùn)行全局BA。然而，僅運(yùn)行局部BA傾向于生成次優(yōu)的解，尤其在局部地圖有較大偏差的情況下。因此，需要在BA中引入更多的變量，否則誤差無(wú)法得到完全消除。本發(fā)明提出的增量BA可以獲得和全局BA相近的解，同時(shí)運(yùn)行時(shí)間和BA中改變的變量數(shù)目線(xiàn)性相關(guān)，因此可以大幅減少運(yùn)行時(shí)間。其具體方法如下：

將當(dāng)前相機(jī)運(yùn)動(dòng)參數(shù)定義為C_i∈SE(3)，n_c是相機(jī)個(gè)數(shù)，將當(dāng)前三維點(diǎn)位置定義為X_j∈R³，n_p為三維點(diǎn)的個(gè)數(shù)，將相機(jī)i可見(jiàn)的三維點(diǎn)的集合定義為將三維點(diǎn)j可見(jiàn)的相機(jī)集合定義為用高斯-牛頓算法迭代優(yōu)化：

每次迭代，將當(dāng)前相機(jī)運(yùn)動(dòng)參數(shù)定義為將當(dāng)前三維點(diǎn)位置定義為將相機(jī)運(yùn)動(dòng)參數(shù)的增量定義為將三維點(diǎn)位置的增量定義為將e(C_i,X_j)在處線(xiàn)性展開(kāi)：

其中，表示在上疊加一個(gè)對(duì)應(yīng)的se(3)增量，定義法向方程：

U、V分別是n_c×n_c、n_p×n_p的對(duì)角矩陣塊

對(duì)每個(gè)子矩陣U_i∈R^6×6，V_j∈R^3×3，W為n_c×n_p的塊矩陣：

每個(gè)子矩陣W_ij∈R^6×3，法向方程右側(cè)向量：

每個(gè)子向量u_i∈R⁶，v_j∈R³，定義舒爾補(bǔ)方程

其中，

S＝U-WV^-1W^T

g＝u-WV^-1v

S為n_c×n_c的塊矩陣：

每個(gè)子矩陣S_ij∈R^6×6，舒爾補(bǔ)方程右側(cè)向量

對(duì)每個(gè)子向量g_i∈R⁶，定義輔助向量其中i＝1,…,n_c，定義輔助變量其中j＝1,…,n_p，初始時(shí)刻，令U＝0,V＝0,W＝0,u＝0,v＝0,S＝0,g＝0，每次迭代，運(yùn)行以下步驟：

a)對(duì)每個(gè)三維點(diǎn)j及其可見(jiàn)的相機(jī)如果或上次迭代被更新，則計(jì)算處的并由下式更新各變量：

f)對(duì)每個(gè)三維點(diǎn)j，如果V_j在本次迭代中被更新，則對(duì)每個(gè)三維點(diǎn)j可見(jiàn)的相機(jī)由下式更新各變量：

并對(duì)三維點(diǎn)j可見(jiàn)的相機(jī)對(duì)由下式更新各變量：

g)使用預(yù)處理共軛梯度算法求解上述的舒爾補(bǔ)方程，得到相機(jī)增量對(duì)每個(gè)相機(jī)i，如果則用更新

b)對(duì)每個(gè)三維點(diǎn)j，如果V_j本次迭代被更新，或相機(jī)中存在本次迭代被更新，則計(jì)算如果則用更新

c)如果本次迭代沒(méi)有任何相機(jī)或三維點(diǎn)被更新，或達(dá)到最大迭代次數(shù)，則結(jié)束循環(huán)，否則，重復(fù)步驟a)。

本發(fā)明采用基于體素的三維模型表示，每個(gè)體素保存其截?cái)嗟牡阶罱砻鎺Х?hào)距離(TSDF)，為提高GPU顯存利用率以及可重建場(chǎng)景尺度，采用空間哈希方法。

當(dāng)相機(jī)姿態(tài)發(fā)生調(diào)整時(shí)，相應(yīng)的，需要調(diào)整重建的三維模型。一種簡(jiǎn)單的方法是存儲(chǔ)所有的深度圖，并在每一深度圖對(duì)應(yīng)的相機(jī)姿態(tài)調(diào)整時(shí)重新融合進(jìn)三維模型。然而，這種方法有著內(nèi)存消耗高、計(jì)算量大的不足。為此，本發(fā)明提出基于關(guān)鍵幀的深度圖表示和融合、反融合方法，有效的解決了這一問(wèn)題。具體的：

如果當(dāng)前幀F(xiàn)_i被選為關(guān)鍵幀，則直接將其對(duì)應(yīng)的深度圖D_i融合到模型上。對(duì)每個(gè)體素v，其截?cái)鄮Х?hào)最近表面距離(TSDF)用D(v)表示，權(quán)重用W(v)表示，對(duì)F_i中的每個(gè)像素x，采用如下方法更新其對(duì)應(yīng)的體素v：

其中w_i(x)是像素x的融合權(quán)重。

如果當(dāng)前幀F(xiàn)_i未被選為關(guān)鍵幀，則首先尋找與F_i最匹配的關(guān)鍵幀，記為首先反融合關(guān)鍵幀所對(duì)應(yīng)的深度圖。反融合過(guò)程與融合過(guò)程相似，對(duì)每個(gè)像素x，采用如下方法更新其對(duì)應(yīng)的體素v：

其中w_i(x)是像素x的融合權(quán)重。

隨后，通過(guò)深度圖D_i投影到將其融合到對(duì)F_i的每個(gè)像素x，用y表示其投影在的坐標(biāo)。如果y對(duì)應(yīng)的深度的倒數(shù)與x投影點(diǎn)深度的倒數(shù)(用表示)小于閾值τ_d(本發(fā)明設(shè)置為0.1)，則更新y的深度為：

對(duì)每個(gè)關(guān)鍵幀記錄其被融合的次數(shù)當(dāng)大于某一閾值時(shí)，停止融合。由于F_i與的視角重合較大，除了一些在視角之外的點(diǎn)，通常當(dāng)前幀的大多數(shù)深度都能融合到上。因此該融合策略可以有效地減少深度的冗余性。如果當(dāng)前幀融合后殘余的深度點(diǎn)數(shù)目小于τ(例如2000)，則將這些點(diǎn)丟棄。否則，創(chuàng)建一個(gè)點(diǎn)云集V_i保存未融合的三維點(diǎn)并將其關(guān)聯(lián)到之后，將更新后的關(guān)鍵幀深度圖融合到模型上。如果V_i非空，則將V_i中的三維點(diǎn)投影到將其融合到模型。因此，每個(gè)非關(guān)鍵幀，將會(huì)執(zhí)行兩次融合與一次反融合。由于未融合的點(diǎn)數(shù)目相對(duì)很少，因此其需要的融合時(shí)間也較少。因此，本發(fā)明計(jì)算量基本上略大于通常的融合方法的兩倍。如果較大并且關(guān)鍵幀的數(shù)目不再增加，這意味著RGB-D相機(jī)沒(méi)有掃描新的信息，在這種情況下，簡(jiǎn)單的丟棄D_i。

為進(jìn)一步減少深度數(shù)據(jù)的冗余，如果當(dāng)前幀F(xiàn)_i被選為關(guān)鍵幀，則選擇關(guān)聯(lián)到相鄰關(guān)鍵幀的若干點(diǎn)云集，融合到當(dāng)前幀。如果融合后V_I剩余的點(diǎn)數(shù)小于τ(例如2000)，則將其從模型中反融合并丟棄。否則，將融合到當(dāng)前幀的點(diǎn)從模型中反融合。

當(dāng)關(guān)鍵幀的姿態(tài)被BA調(diào)整時(shí)，需要重新融合關(guān)鍵幀對(duì)應(yīng)的深度圖以及與其關(guān)聯(lián)的點(diǎn)云集。然而，如果被調(diào)整的關(guān)鍵幀數(shù)目較大，重融合會(huì)耗費(fèi)大量的時(shí)間而不能滿(mǎn)足實(shí)時(shí)性的要求。因此，本發(fā)明限制每一幀重融合的次數(shù)，將所有重融合分?jǐn)偟矫恳粠?。特別的，姿態(tài)變化比較大的關(guān)鍵幀對(duì)應(yīng)的深度圖及其關(guān)聯(lián)的點(diǎn)云集將會(huì)被首先重融合。此策略可以保證在BA發(fā)生時(shí)重建部分以恒定的速率運(yùn)行。

為進(jìn)一步加速，可以每?jī)蓭蚋嗳诤弦粠侥Ｐ?，此方法可以極大地加速融合過(guò)程，在筆記本電腦上可以達(dá)到實(shí)時(shí)運(yùn)行。

附圖說(shuō)明

圖1是本發(fā)明的系統(tǒng)框架。

圖2是本發(fā)明基于關(guān)鍵幀的相機(jī)追蹤框架。

圖3是本發(fā)明提出的增量BA與標(biāo)準(zhǔn)BA的性能比較，可以看到，相對(duì)于標(biāo)準(zhǔn)BA，在保留相當(dāng)精度的同時(shí)，本發(fā)明可以極大的加快BA速度。

圖4是本發(fā)明在使用華碩Xtion Pro Live拍攝的數(shù)據(jù)集上運(yùn)行所生成的三維模型。

具體實(shí)施方式

本發(fā)明的系統(tǒng)架構(gòu)如圖1。系統(tǒng)的相機(jī)追蹤部分和三維模型重建部分并行運(yùn)行。對(duì)每個(gè)包含RGB圖像與深度圖的輸入幀，相機(jī)追蹤部分首先結(jié)合RGB-D對(duì)準(zhǔn)和基于單應(yīng)性的特征追蹤方法計(jì)算相機(jī)姿態(tài)，實(shí)現(xiàn)了魯棒的相機(jī)跟蹤。同時(shí)，提取關(guān)鍵幀的ORB特征并進(jìn)行匹配以檢測(cè)全局回路閉合。最后，周期性的在后臺(tái)運(yùn)行增量BA以?xún)?yōu)化相機(jī)姿態(tài)。重建部分在輸入幀追蹤相機(jī)姿態(tài)可靠時(shí)將輸入深度融合到關(guān)鍵幀的深度圖上。為在線(xiàn)調(diào)整稠密的三維表面，系統(tǒng)會(huì)對(duì)關(guān)鍵幀進(jìn)行反融合與在融合。此策略使得在相機(jī)姿態(tài)調(diào)整時(shí)，僅需要重融合關(guān)鍵幀的深度以及一些額外的點(diǎn)云集。由于在相當(dāng)規(guī)模場(chǎng)景下的關(guān)鍵幀數(shù)目是有限的，本系統(tǒng)可以無(wú)限長(zhǎng)時(shí)間運(yùn)行。

本發(fā)明同時(shí)結(jié)合圖片強(qiáng)度和深度信息跟蹤相機(jī)運(yùn)動(dòng)，結(jié)合基于稠密RGB-D和基于稀疏特征的方法使相機(jī)跟蹤更為準(zhǔn)確和魯棒。其基本框架如附圖2所示。對(duì)每個(gè)當(dāng)前幀F(xiàn)_i，首先使用RGB-D對(duì)準(zhǔn)[C.Kerl,J.Sturm,and D.Cremers.Dense visual SLAM for RGB-D cameras.In 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems,Tokyo,Japan,November 3-7,2013,pages 2100–2106,2013.]估計(jì)相機(jī)從上一幀到當(dāng)前幀的相對(duì)姿態(tài)T_(i-1)→i，這里只需要一個(gè)相對(duì)粗糙的估計(jì)，因此我們使用小模糊圖(SBI)[G.Klein and D.W.Murray.Improving the agility of keyframe-based SLAM.In 10th European Conference on Computer Vision,Part II,pages 802–815.Springer,2008.]估計(jì)兩幀相對(duì)姿態(tài)以在CPU上達(dá)到實(shí)時(shí)運(yùn)行。

精確的相機(jī)姿態(tài)是通過(guò)特征匹配獲得的，為處理劇烈旋轉(zhuǎn)和快速運(yùn)動(dòng)，本發(fā)明使用基于單應(yīng)性[H.Liu,G.Zhang,and H.Bao.Robust keyframe-based monocular SLAM for augmented reality.In IEEE International Symposium on Mixed and Augmented Reality,ISMAR 2016.]的特征匹配方法求解相機(jī)姿態(tài)。在劇烈旋轉(zhuǎn)和快速運(yùn)動(dòng)下，兩幀之間的透視變形可能會(huì)很大，以至于基于特征的匹配不能魯棒運(yùn)行。單應(yīng)性可以用來(lái)糾正圖片塊的透視變形，從而使零均值的SSD[G.Klein and D.W.Murray.Parallel tracking and mapping for small AR workspaces.In 6th IEEE/ACM International Symposium on Mixed and Augmented Reality,pages 225–234,2007.]可行。單應(yīng)性也可以用來(lái)決定一個(gè)較小的搜索區(qū)域避免沿著整個(gè)對(duì)極線(xiàn)搜索。在本發(fā)明中，由于有深度信息z_k以及使用RGB-D對(duì)準(zhǔn)的相機(jī)姿態(tài)估計(jì)C_i＝T_(i-1)→iC_i-1，因此將搜索區(qū)域定為以及

在確定了相對(duì)姿態(tài)T_(i-1)→i以及3D-2D特征匹配的集合χ＝{(X_j,x_j)}后，通過(guò)最小化相對(duì)誤差與重投影誤差來(lái)估計(jì)相機(jī)姿態(tài)C_i：

這里，log(T)將3D剛性變換T∈SE(3)映射到se(3)，并返回R⁶中的向量表示。是標(biāo)準(zhǔn)馬氏距離。是投影函數(shù)，K是已知并且固定的相機(jī)內(nèi)參。z(X)從X中提取出z分量，z_j是x_j在當(dāng)前幀的深度。||∈||_δ是Huber范數(shù)：

如果追蹤失敗，則使用[G.Klein and D.W.Murray.Improving the agility of keyframe-based SLAM.In 10th European Conference on Computer Vision,Part II,pages 802–815.Springer,2008.]所述的方法重新跟蹤特征。否則，則使用新特征的深度信息，用貝葉斯方法[G.Vogiatzis and C.Hernández.Video-based,real-time multi-view stereo.Image Vision Comput.,29(7):434–441,2011.]對(duì)特征的深度進(jìn)行濾波。對(duì)關(guān)鍵幀F(xiàn)_k和當(dāng)前幀F(xiàn)_i的每一對(duì)特征匹配(x_i,x_j)，獲取x_k深度的倒數(shù)ρ_k：

這里，通過(guò)ρ_i→k，濾波器不斷更新ρ_k和其內(nèi)點(diǎn)的聯(lián)合分布。

最后，本發(fā)明在每一新幀到來(lái)時(shí)決定是否選作關(guān)鍵幀。

當(dāng)有新的關(guān)鍵幀加入或者檢測(cè)到閉合回路的時(shí)候，需要運(yùn)行BA。在前一種情形下，由于只有局部地圖調(diào)整，沒(méi)有必要運(yùn)行全局BA。然而，僅運(yùn)行局部BA傾向于生成次優(yōu)的解，尤其在局部地圖有較大偏差的情況下。因此，需要在BA中引入更多的變量，否則誤差無(wú)法得到完全消除。本發(fā)明提出的增量BA可以獲得和全局BA相近的解，同時(shí)運(yùn)行時(shí)間和BA中改變的變量數(shù)目線(xiàn)性相關(guān)，因此可以大幅減少運(yùn)行時(shí)間。其具體方法如下：

將當(dāng)前相機(jī)運(yùn)動(dòng)參數(shù)定義為C_i∈SE(3)，n_c是相機(jī)個(gè)數(shù)，將當(dāng)前三維點(diǎn)位置定義為X_j∈R³，n_p為三維點(diǎn)的個(gè)數(shù)，將相機(jī)i可見(jiàn)的三維點(diǎn)的集合定義為將三維點(diǎn)j可見(jiàn)的相機(jī)集合定義為用高斯-牛頓算法迭代優(yōu)化：

每次迭代，將當(dāng)前相機(jī)運(yùn)動(dòng)參數(shù)定義為將當(dāng)前三維點(diǎn)位置定義為將相機(jī)運(yùn)動(dòng)參數(shù)的增量定義為將三維點(diǎn)位置的增量定義為將e(C_i,X_j)在處線(xiàn)性展開(kāi)：

其中，表示在上疊加一個(gè)對(duì)應(yīng)的se(3)增量，定義法向方程：

U、V分別是n_c×n_c、n_p×n_p的對(duì)角矩陣塊

對(duì)每個(gè)子矩陣U_i∈R^6×6，V_j∈R^3×3，W為n_c×n_p的塊矩陣：

每個(gè)子矩陣W_ij∈R^6×3，法向方程右側(cè)向量：

每個(gè)子向量u_i∈R⁶，v_j∈R³，定義舒爾補(bǔ)方程

其中，

S＝U-WV^-1W^T

g＝u-WV^-1v

S為n_c×n_c的塊矩陣：

每個(gè)子矩陣S_ij∈R^6×6，舒爾補(bǔ)方程右側(cè)向量

對(duì)每個(gè)子向量g_i∈R⁶，定義輔助向量其中i＝1,…,n_c，定義輔助變量其中j＝1,…,n_p，初始時(shí)刻，令U＝0,V＝0,W＝0,u＝0,v＝0,S＝0,g＝0,每次迭代，運(yùn)行以下步驟：

a)對(duì)每個(gè)三維點(diǎn)j及其可見(jiàn)的相機(jī)如果或上次迭代被更新，則計(jì)算處的并由下式更新各變量：

h)對(duì)每個(gè)三維點(diǎn)j，如果V_j在本次迭代中被更新，則對(duì)每個(gè)三維點(diǎn)j可見(jiàn)的相機(jī)由下式更新各變量：

并對(duì)三維點(diǎn)j可見(jiàn)的相機(jī)對(duì)由下式更新各變量：

i)使用預(yù)處理共軛梯度算法求解上述的舒爾補(bǔ)方程，得到相機(jī)增量對(duì)每個(gè)相機(jī)i，如果則用更新

b)對(duì)每個(gè)三維點(diǎn)j，如果V_j本次迭代被更新，或相機(jī)中存在本次迭代被更新，則計(jì)算如果則用更新

c)如果本次迭代沒(méi)有任何相機(jī)或三維點(diǎn)被更新，或達(dá)到最大迭代次數(shù)，則結(jié)束循環(huán)，否則，重復(fù)步驟a)

同KinectFusion，本發(fā)明采用基于體素的三維模型表示，每個(gè)體素保存其截?cái)嗟牡阶罱砻鎺Х?hào)距離(TSDF)，為提高GPU顯存利用率以及可重建場(chǎng)景尺度，采用了Nieβner[M.Nieβner,M.S.Izadi,and M.Stamminger.Real-time 3d reconstruction at scale using voxel hashing.ACM Trans.Graph.,32(6):169:1–169:11,2013.]等提出的空間哈希方法。

當(dāng)相機(jī)姿態(tài)發(fā)生調(diào)整時(shí)，相應(yīng)的，需要調(diào)整重建的三維模型。一種簡(jiǎn)單的方法是存儲(chǔ)所有的深度圖，并在每一深度圖對(duì)應(yīng)的相機(jī)姿態(tài)調(diào)整時(shí)重新融合進(jìn)三維模型。然而，這種方法有著內(nèi)存消耗高、計(jì)算量大的不足。為此，本發(fā)明提出基于關(guān)鍵幀的深度圖表示和融合、反融合方法，有效地解決了這一問(wèn)題。具體的：

如果當(dāng)前幀F(xiàn)_i被選為關(guān)鍵幀，則直接將其對(duì)應(yīng)的深度圖D_i融合到模型上。對(duì)每個(gè)體素v，其截?cái)鄮Х?hào)最近表面距離(TSDF)用D(v)表示，權(quán)重用W(v)表示，對(duì)F_i中的每個(gè)像素x，采用如下方法更新其對(duì)應(yīng)的體素v：

其中w_i(x)是像素x的融合權(quán)重。

如果當(dāng)前幀F(xiàn)_i未被選為關(guān)鍵幀，則首先尋找與F_i最匹配的關(guān)鍵幀，記為首先反融合關(guān)鍵幀所對(duì)應(yīng)的深度圖。反融合過(guò)程與融合過(guò)程相似，對(duì)每個(gè)像素x，采用如下方法更新其對(duì)應(yīng)的體素v：

其中w_i(x)是像素x的融合權(quán)重。

隨后，通過(guò)深度圖D_i投影到將其融合到對(duì)F_i的每個(gè)像素x，用y表示其投影在的坐標(biāo)。如果y對(duì)應(yīng)的深度的倒數(shù)與x投影點(diǎn)深度的倒數(shù)(用表示)小于閾值τ_d(本發(fā)明設(shè)置為0.1)，則更新y的深度為：

對(duì)每個(gè)關(guān)鍵幀記錄其被融合的次數(shù)當(dāng)大于某一閾值時(shí)，停止融合。由于F_i與的視角重合較大，除了一些在視角之外的點(diǎn)，通常當(dāng)前幀的大多數(shù)深度都能融合到上。因此該融合策略可以有效的減少深度的冗余性。如果當(dāng)前幀融合后殘余的深度點(diǎn)數(shù)目小于τ(例如2000)，則將這些點(diǎn)丟棄。否則，創(chuàng)建一個(gè)點(diǎn)云集V_i保存未融合的三維點(diǎn)并將其關(guān)聯(lián)到之后，將更新后的關(guān)鍵幀深度圖融合到模型上。如果V_i非空，則將V_i中的三維點(diǎn)投影到將其融合到模型。因此，每個(gè)非關(guān)鍵幀，將會(huì)執(zhí)行兩次融合與一次反融合。由于未融合的點(diǎn)數(shù)目相對(duì)很少，因此其需要的融合時(shí)間也較少。因此，本發(fā)明計(jì)算量基本上略大于通常的融合方法的兩倍。如果較大并且關(guān)鍵幀的數(shù)目不再增加，這意味著RGB-D相機(jī)沒(méi)有掃描新的信息，在這種情況下，我們簡(jiǎn)單的丟棄D_i。

為進(jìn)一步減少深度數(shù)據(jù)的冗余，如果當(dāng)前幀F(xiàn)_i被選為關(guān)鍵幀，則選擇關(guān)聯(lián)到相鄰關(guān)鍵幀的若干點(diǎn)云集，融合到當(dāng)前幀。如果融合后V_I剩余的點(diǎn)數(shù)小于τ(例如2000)，則將其從模型中反融合并丟棄。否則，將融合到當(dāng)前幀的點(diǎn)從模型中反融合。考慮到實(shí)時(shí)計(jì)算的需求，本實(shí)施例只融合5個(gè)點(diǎn)云集。

當(dāng)關(guān)鍵幀的姿態(tài)被BA調(diào)整時(shí)，需要重新融合關(guān)鍵幀對(duì)應(yīng)的深度圖以及與其關(guān)聯(lián)的點(diǎn)云集。然而，如果被調(diào)整的關(guān)鍵幀數(shù)目較大，重融合會(huì)耗費(fèi)大量的時(shí)間而不能滿(mǎn)足實(shí)時(shí)性的要求。因此，本發(fā)明限制每一幀重融合的次數(shù)，將所有重融合分?jǐn)偟矫恳粠?。特別的，姿態(tài)變化比較大的關(guān)鍵幀對(duì)應(yīng)的深度圖及其關(guān)聯(lián)的點(diǎn)云集將會(huì)被首先重融合。此策略可以保證在BA發(fā)生時(shí)重建部分以恒定的速率運(yùn)行。

為進(jìn)一步加速，可以每?jī)蓭蚋鄮诤弦粠侥Ｐ停朔椒梢詷O大地加速融合過(guò)程，在筆記本電腦上可以達(dá)到實(shí)時(shí)運(yùn)行。

在實(shí)驗(yàn)中測(cè)試了本發(fā)明的性能。在配置Intel i7 3.6GHz CPU，8GB內(nèi)存，GTX960顯卡的PC環(huán)境下，無(wú)GPU加速的相機(jī)跟蹤可以達(dá)到10～25毫秒每幀，在融合每個(gè)輸入幀的情況下稠密的重建部分需要50毫秒每幀，整個(gè)系統(tǒng)運(yùn)行速率大概位20fps。在配置為i7 2.6GHz CPU，16GB內(nèi)存，GTX960M顯卡的筆記本電腦上，如果每三幀融合一幀，運(yùn)行速度可以達(dá)到20fps。

在用華碩Xtion Pro Live拍攝的多個(gè)數(shù)據(jù)集上測(cè)試了算法性能。如附圖4所示，本發(fā)明可以構(gòu)建出高質(zhì)量的模型。