夏文玲，顧照鵬，楊唐勝

1.湖南大學(xué) 電氣與信息工程學(xué)院，長(zhǎng)沙 410000

2.北京大學(xué)深圳研究院 信息工程學(xué)院，廣東 深圳 518000

實(shí)時(shí)三維重建算法的實(shí)現(xiàn)
——基于Kinect與單目視覺SLAM的三維重建

夏文玲1，顧照鵬2，楊唐勝2

1.湖南大學(xué) 電氣與信息工程學(xué)院，長(zhǎng)沙 410000

2.北京大學(xué)深圳研究院 信息工程學(xué)院，廣東 深圳 518000

1 引言

基于視覺的三維重建技術(shù)，是指利用數(shù)字?jǐn)z像機(jī)作為圖像傳感器，綜合運(yùn)用圖像處理、視覺計(jì)算等技術(shù)進(jìn)行非接觸三維測(cè)量，用計(jì)算機(jī)程序獲取物體的三維信息[1]。在計(jì)算機(jī)內(nèi)生成物體三維表示主要有兩類方法，一類是利用幾何建模軟件，如3DMAX、Maya、AutoCAD、UG等，另一類則是通過一定的手段獲取真實(shí)物體的幾何形狀，主要包括數(shù)據(jù)獲取、預(yù)處理、點(diǎn)云拼接和特征分析等步驟。

目前在單目視覺的同步定位與地圖創(chuàng)建（Monocular Simultaneous Location and Mapping，MonoSLAM）系統(tǒng)中[2]，3D場(chǎng)景深度信息的獲取是十分困難的，Davison研究團(tuán)隊(duì)推出反向深度算法獲取深度信息[3]，而采用深度相機(jī)主動(dòng)獲取場(chǎng)景中的深度信息，在很大程度上降低了該算法的復(fù)雜度。目前深度相機(jī)主要分為兩類，一類是TOF相機(jī)，一類即為Kinect（Kinect for Xbox 360）[4]。Kinect是由微軟開發(fā)的應(yīng)用于Xbox 360主機(jī)的周邊設(shè)備。在基于Kinect實(shí)現(xiàn)的RGB-D攝像機(jī)SLAM系統(tǒng)研究方面，Nikolas利用Kinect提供的RGB-D相機(jī)實(shí)現(xiàn)了一個(gè)實(shí)時(shí)的視覺SLAM系統(tǒng)，并將這一系統(tǒng)集成到ROS（Robot Operating System）中[5]。Henry等利用Kinect實(shí)現(xiàn)了一個(gè)交互的實(shí)時(shí)三維重建系統(tǒng)RGB-D Mapping[6-7]，該系統(tǒng)不進(jìn)行實(shí)時(shí)的ICP[8]配準(zhǔn)，而是根據(jù)用戶需要，選取不同的關(guān)鍵幀進(jìn)行配準(zhǔn)，其重建出的點(diǎn)云能很好地與建筑物俯視圖相吻合，但該系統(tǒng)實(shí)際運(yùn)行速度不高，未進(jìn)行ICP配準(zhǔn)運(yùn)行幀率為5 frame/s，而進(jìn)行ICP配準(zhǔn)后單幀處理時(shí)間至少需要500 ms。微軟研究院的Kinect-Fusion系統(tǒng)給出了一種基于GPU并計(jì)算的實(shí)時(shí)的定位與重建系統(tǒng)，實(shí)現(xiàn)了動(dòng)態(tài)場(chǎng)景的增強(qiáng)現(xiàn)實(shí)應(yīng)用，該系統(tǒng)利用GPU實(shí)現(xiàn)了實(shí)時(shí)ICP配準(zhǔn)，并同步生成環(huán)境地圖的三維表面[9-10]。

由于ICP算法計(jì)算復(fù)雜度高，而利用GPU加速的ICP算法對(duì)于硬件要求較高，制約了其使用范圍。由于目前的單目視覺SLAM算法的實(shí)時(shí)性和準(zhǔn)確性都有一定保證，因此本文設(shè)計(jì)一種基于擴(kuò)展卡爾曼濾波（EKF）的單目視覺SLAM與Kinect的室內(nèi)環(huán)境實(shí)時(shí)三維重建方法，并與基于光流跟蹤算法（Kanade Lucas Tomasi，KLT）[11]與Kinect實(shí)現(xiàn)的三維重建方法相比較，實(shí)驗(yàn)結(jié)果證明該方法具有實(shí)時(shí)性好，一致性強(qiáng)等優(yōu)點(diǎn)。

2 Kinect工作原理

Kinect有三個(gè)鏡頭，中間的鏡頭是RGB彩色攝影機(jī)，左右兩邊鏡頭分別為紅外線發(fā)射器和紅外CMOS攝影機(jī)所構(gòu)成的3D結(jié)構(gòu)光深度感應(yīng)器。Kinect還搭配了追焦技術(shù)，底座馬達(dá)會(huì)隨著對(duì)焦物體移動(dòng)跟著轉(zhuǎn)動(dòng)。

2.1 Kinect紅外攝像機(jī)坐標(biāo)系下深度的計(jì)算

Kinect利用紅外激光投影和紅外攝像機(jī)組成一組紅外結(jié)構(gòu)光視覺設(shè)備，計(jì)算空間點(diǎn)相對(duì)于紅外攝像機(jī)的深度，如圖1所示為其原理圖。首先由紅外激光投影機(jī)向環(huán)境中投影紅外隨機(jī)點(diǎn)結(jié)構(gòu)光，然后利用紅外攝像機(jī)進(jìn)行接收，并在每一像素周圍的±64像素范圍內(nèi)，利用9×9或者7×7的圖像patch，利用NCC（Normalized Cross Correlation）對(duì)圖像隨機(jī)點(diǎn)進(jìn)行匹配，獲得原始視差，進(jìn)而利用紅外相機(jī)的內(nèi)參數(shù)以及基線長(zhǎng)度獲得環(huán)境三維點(diǎn)在紅外相機(jī)下的深度信息，如公式（1）所示：

其中ZIR為深度值，b為基線長(zhǎng)度，fIR為紅外攝像機(jī)的焦距，d為視差。紅外攝像機(jī)的內(nèi)參數(shù)矩陣KIR：

(fuIR,fvIR)為紅外攝像機(jī)焦距，(u0IR,v0IR)為紅外攝像機(jī)主點(diǎn)坐標(biāo)。

圖1 Kinect原理圖

2.2 彩色攝像機(jī)坐標(biāo)下的坐標(biāo)計(jì)算

Kinect的RGB攝像頭采集到的是環(huán)境的彩色圖像，RGB攝像頭的內(nèi)參數(shù)矩陣為KRGB：

上式中(fuRGB,fvRGB)為RGB攝像機(jī)焦距，(u0RGB,v0RGB)為RGB攝像機(jī)光心。IR攝像機(jī)與RGB攝像機(jī)的剛體變化TRGB-IR=[RRGB-IR，tRGB-IR]。

對(duì)于空間中的一點(diǎn)P，其在彩色攝像機(jī)坐標(biāo)下的三維坐標(biāo)計(jì)算流程如圖2所示，由Kinect獲得P點(diǎn)的視差d，根據(jù)式（1）計(jì)算得到P點(diǎn)的深度ZIR，進(jìn)一步通過紅外攝像機(jī)內(nèi)參數(shù)獲得P點(diǎn)在紅外攝像機(jī)下的三維坐標(biāo)：

通過IR攝像機(jī)與RGB攝像機(jī)的旋轉(zhuǎn)平移關(guān)系得到P點(diǎn)在RGB攝像機(jī)坐標(biāo)下的三維坐標(biāo)，最后引入RGB攝像機(jī)內(nèi)參數(shù)計(jì)算得到圖像坐標(biāo)(uRGB,vRGB)。

圖2 RGB攝像機(jī)坐標(biāo)下的空間點(diǎn)坐標(biāo)計(jì)算流程

3 基于Kinect的單目視覺EKF-SLAM框架

在視覺SLAM系統(tǒng)中，攝像機(jī)的狀態(tài)xc由世界坐標(biāo)系下攝像機(jī)坐標(biāo)位置rT、旋轉(zhuǎn)四元數(shù)qT、線速度vT、角速度ωT組成：

地圖特征狀態(tài)向量由特征點(diǎn)的三維世界坐標(biāo)組成：

攝像機(jī)狀態(tài)向量和特征點(diǎn)狀態(tài)向量組成SLAM系統(tǒng)狀態(tài)向量：本文中SLAM系統(tǒng)中采用勻速模型作為過程模型，得到相機(jī)狀態(tài)預(yù)測(cè)方程[12]：

其中[nv，nω]T為過程噪聲。

空間任何一點(diǎn)P在圖像上的成像位置可以用針孔模型近似表示，如圖3所示。即任何點(diǎn)P在圖像上的投影位置 p，為光心O與P點(diǎn)的連線OP與圖像平面的交點(diǎn)，有如下關(guān)系式：

其中(x,y)為P點(diǎn)在成像平面下的坐標(biāo)，(XC，YC，ZC)為空間點(diǎn)P在攝像機(jī)坐標(biāo)下的坐標(biāo)。

圖3 針孔成像模型

本文中使用的是特征地圖，觀測(cè)量由一組成功匹配的地圖特征點(diǎn)的圖像坐標(biāo)組成：

其中n表示匹配成功的特征點(diǎn)數(shù)目，Ci=(uivi)T為第i個(gè)特征點(diǎn)的圖像坐標(biāo)。引入測(cè)量噪聲ηi得到測(cè)量方

(fufv)為Kinect彩色攝像機(jī)焦距，(u0v0)為Kinect彩色攝像機(jī)光心，R、t為彩色攝像機(jī)的外參數(shù)，(xicyiczic)T為特征點(diǎn)在攝像機(jī)坐標(biāo)下的三維坐標(biāo)，XW為特征點(diǎn)在世界坐標(biāo)系下的坐標(biāo)。

利用Kinect獲得特征點(diǎn)在彩色攝像機(jī)坐標(biāo)下的三維坐標(biāo)，根據(jù)當(dāng)前SLAM輸出攝像機(jī)位姿(rt，qt)初始化特征點(diǎn)在世界坐標(biāo)系下的三維位置。設(shè)新增特征點(diǎn)為 fn+1，其對(duì)應(yīng)在Kinect彩色攝像機(jī)下的坐標(biāo)估計(jì)和方差為：

其中σxRGB，σyRGB，σzRGB為Kinect的輸出轉(zhuǎn)換到彩色攝像機(jī)坐標(biāo)下的三個(gè)方向的標(biāo)準(zhǔn)差。新增特征點(diǎn)之前的系統(tǒng)狀態(tài)向量和協(xié)方差分別為 x和P，則新增特征點(diǎn) fn+1之后系統(tǒng)狀態(tài)變?yōu)椋?/p>

整個(gè)系統(tǒng)流程圖如圖4所示，在定位過程中，若當(dāng)前彩色攝像機(jī)可見重建點(diǎn)云總數(shù)小于設(shè)定閾值（像素總數(shù)的2/3），就利用當(dāng)前彩色攝像機(jī)SLAM算法得到的外參數(shù)Tcw=[Rcw，tcw]將彩色攝像機(jī)下的三維點(diǎn)云調(diào)整到世界坐標(biāo)系下，并加入重建點(diǎn)云中。否則認(rèn)為場(chǎng)景變化不大，不對(duì)重建點(diǎn)云進(jìn)行調(diào)整。

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

圖4 系統(tǒng)流程圖

本文實(shí)驗(yàn)場(chǎng)景為辦公室，使用深度攝像機(jī)Kinect，RGB相機(jī)圖像分辨率為640×480。使用Fast角點(diǎn)檢測(cè)算法[13]，特征描述子為11×11的圖像Patch。本文程序基于VS2010的C++實(shí)現(xiàn)上述算法，所有實(shí)驗(yàn)都是在Intel雙核2.93 GHz CPU的計(jì)算機(jī)上運(yùn)行。為了驗(yàn)證本文方法的實(shí)時(shí)性和穩(wěn)定性，手持Kinect在場(chǎng)景中進(jìn)行自由運(yùn)動(dòng)采集數(shù)據(jù)，并在系統(tǒng)運(yùn)行時(shí)實(shí)時(shí)地利用EKF-SLAM算法計(jì)算出攝像機(jī)位置和姿態(tài)，運(yùn)動(dòng)軌跡如圖5所示，實(shí)時(shí)三維重建實(shí)驗(yàn)結(jié)果如圖6所示。

圖5 運(yùn)動(dòng)軌跡

圖6 不同視角下基于Kinect與單目視覺SLAM的三維重建結(jié)果

利用光流跟蹤算法可以快速獲取相鄰幀之間的特征點(diǎn)匹配，由于Kinect可以得到圖像特征點(diǎn)相應(yīng)的深度信息，從而可以得到相鄰幀之間三維點(diǎn)的匹配關(guān)系。基于已獲得的三維點(diǎn)匹配結(jié)果后，優(yōu)化相鄰幀之間的剛體變換參數(shù)。在上述相同的環(huán)境條件以及處理方式下，利用Fast角點(diǎn)檢測(cè)，將重建點(diǎn)云投影到彩色攝像機(jī)下，當(dāng)可見點(diǎn)云總數(shù)小于設(shè)定閾值（像素總數(shù)的2/3），將當(dāng)前的三維點(diǎn)云調(diào)整到世界坐標(biāo)系下，并加入重建點(diǎn)云中。這樣就得到了基于光流跟蹤與Kinect的三維重建結(jié)果，如圖7所示。由于隨著特征點(diǎn)數(shù)目的增多，視覺SLAM算法的時(shí)間復(fù)雜度會(huì)逐漸增大，本文實(shí)驗(yàn)中SLAM算法檢測(cè)到的特征點(diǎn)數(shù)目限制在100以內(nèi)。此時(shí)幀率為30 frame/s。而基于Kinect與光流跟蹤的三維重建算法由于KTL光流跟蹤的運(yùn)算量較大，幀率只能達(dá)到15 frame/s。

圖7 基于光流跟蹤與Kinect的三維重建結(jié)果

基于上述實(shí)驗(yàn)，選擇環(huán)境中21×14.85的黑色方格和電腦兩個(gè)物體，查看它們?cè)趦煞N方法下的三維重建細(xì)節(jié)圖對(duì)比，由Kinect的RGB相機(jī)拍攝的真實(shí)場(chǎng)景的圖片，如圖8所示。

圖8 三維重建細(xì)節(jié)圖對(duì)比

如圖8所示，（a）、（c）為基于SLAM的三維重建結(jié)果細(xì)節(jié)圖，（b）、（d）為基于光流跟蹤的三維重建細(xì)節(jié)圖，（e）、（f）為由RGB相機(jī)拍攝的真實(shí)場(chǎng)景的圖片，由圖8明顯看出（b）中坐標(biāo)產(chǎn)生了偏移，（d）中細(xì)節(jié)出現(xiàn)匹配錯(cuò)誤；（a）與（c）沒有較大的誤差。

基于上述實(shí)驗(yàn)結(jié)果，可以看出基于Kinect與單目視覺SLAM算法實(shí)現(xiàn)的三維重建方法在保證系統(tǒng)實(shí)時(shí)運(yùn)行的前提下，不同幀加入的點(diǎn)云匹配之間，沒有出現(xiàn)較大的不一致現(xiàn)象。而基于光流跟蹤與Kinect的三維重建方法中，由于光流匹配中存在著錯(cuò)誤匹配，并且光流跟蹤沒有考慮到Loop-Closure問題，因此得到的三維重建有漂移現(xiàn)象，出現(xiàn)了重建結(jié)果錯(cuò)誤的情況。

5 結(jié)束語(yǔ)

本文針對(duì)基于Kinect與單目視覺SLAM的三維重建展開研究，并與基于Kinect與光流跟蹤算法結(jié)果進(jìn)行比較，實(shí)驗(yàn)結(jié)果表明該方法具有實(shí)時(shí)性好，一致性強(qiáng)等優(yōu)點(diǎn)，在一定程度上提高了地圖重建點(diǎn)云的精度。目前由于Kinect自身精度的原因，本文方法主要用在室內(nèi)場(chǎng)景的實(shí)時(shí)三維重建。在未來(lái)的工作中，將在現(xiàn)有工作的基礎(chǔ)上，在算法實(shí)現(xiàn)、系統(tǒng)集成、室外環(huán)境的三維重建算法等方面開展進(jìn)一步的研究。

[1]佟帥，徐曉剛，易成濤，等.基于視覺的三維重建技術(shù)綜述[J].計(jì)算機(jī)應(yīng)用研究，2011，28（7）：2012-2015.

[2]顧照鵬，董秋雷.基于部分慣性傳感器信息的單目視覺的同步定位與地圖創(chuàng)建方法[J].計(jì)算機(jī)輔助設(shè)計(jì)與圖形學(xué)學(xué)報(bào)，2012，24（2）.

[3]Civera J，Davison A J.Inverse depth parametrization for monocular SLAM[J].IEEE Transactions on Robotics，2008，24（5）：932-945.

[4]王奎，安平，張兆楊，等.Kinect深度圖像快速修復(fù)算法[J].上海大學(xué)學(xué)報(bào)，2012，18（5）.

[5]Engelhard N，Endres F，hess J，et al.Real-time 3D visual SLAM with a hand-held RGB-D camera[C]//Proceedings of the RGB-D Workshop on 3D Perception in Robotics at the European Robotics Forum，2011.

[6]Henry P，Krainin M，Herbst E，et al.RGB-D Mapping：using depth cameras for dense 3D modeling of indoor environments[C]//Proceedings of the 12th International Symposium on Experimental Robotics（ISER），2010.

[7]Du H，Henry P，Ren X，et al.Interactive 3D modeling of indoor environments with a consumer depth camera[C]// Proceedings of the 13th International Conference on Ubiquitous Computing，2011：75-84.

[8]Besl P J，McKay N D.A method for registration of 3-D shapes[J].IEEE Trans on Pattern Analysis and Machine Intelligence，1992，14（2）：239-256.

[9]Izadi S，Newcombe R A，Kim D，et al.KinectFusion：realtime dynamic 3D surface reconstruction and interaction[C]// Proceedings of Association for Computing Machinery’s Special Interest Group on Computer Graphics and Interactive Techniques（ACM SIGGRAPH 2011），2011.

[10]Izadi S，Kim D，Hilliges O，et al.KinectFusion：real-time 3D reconstruction and interaction using a moving depth camera[C]//Proceedings of the 24th Annual ACM Symposium on User Interface Software and Technology，2011：559-568.

[11]Lucas B D，Kanade T.An iterative image registration technique with an application to stereo vision[C]//International Joint Conference on Artificial Intelligence，1981：674-675.

[12]Montiel J M M，Civera J，Davison A J.Unified inverse depth parametrization for monocular SLAM[C]//Proceedings of Robotics：Science and Systems，2006.

[13]Rosten E，Porter R，Drummond T.Faster and better：a machine learning approach to corner detection[J].IEEE Trans on Pattern Analysis and Machine Intelligence，2010，32.

XIA Wenling1,GU Zhaopeng2,YANG Tangsheng2

1.College of Electrical and Information Engineering,Hunan University,Changsha 410000,China
2.School of Computer and Information Engineering,Peking University Shenzhen Graduate School,Shenzhen,Guangdong 518000,China

As an important branch of the computer vision technology,the 3-D reconstruction techniques based on the Monocular vision get more and more attention,for it’s simple,low cost and easy to implement.This paper launches a study on the algorithm of the SLAM（Simultaneous Localization and Mapping）by introducing the RGB-D camera Kinect to obtain the depth information of the 3D scene.An algorithm of the 3-D reconstruction based on the Kinect and monocular vision SLAM is achieved.

monocular vision;3-D reconstruction;Simultaneous Localization and Mapping（SLAM）;Kinect

作為計(jì)算機(jī)視覺技術(shù)的一個(gè)重要分支，基于單目視覺的三維重建技術(shù)以其要求簡(jiǎn)單、成本低廉、易于實(shí)現(xiàn)等優(yōu)點(diǎn)，得到了越來(lái)越多的關(guān)注。在室內(nèi)環(huán)境下就智能機(jī)器人的同步定位以及環(huán)境地圖創(chuàng)建（SLAM）算法展開了研究，引入RGB-D相機(jī)Kinect直接獲取3D場(chǎng)景的深度信息，實(shí)現(xiàn)了一種基于單目視覺SLAM與Kinect的實(shí)時(shí)三維重建方法。

單目視覺；三維重建；同步定位以及環(huán)境地圖創(chuàng)建；Kinect

A

TP391

10.3778/j.issn.1002-8331.1301-0306

XIA Wenling,GU Zhaopeng,YANG Tangsheng.Real-time 3-D reconstruction algorithm based on Kinect and MonoSLAM.Computer Engineering and Applications,2014,50（24）：199-203.

中國(guó)醫(yī)學(xué)科學(xué)研究院北京協(xié)和醫(yī)院“863”計(jì)劃（No.754214019）。

夏文玲（1987—），女，碩士研究生，主要研究方向?yàn)橛?jì)算機(jī)視覺導(dǎo)航與定位、嵌入式系統(tǒng)；顧照鵬（1981—），男，博士研究生，講師，主要研究方向?yàn)榛谝曈X的實(shí)時(shí)定位系統(tǒng)；楊唐勝（1967—），男，博士研究生，副教授，主要研究方向?yàn)橹悄芩惴ā⒅悄軆x器。E-mail：xia1234zhao@126.com

2013-01-28

2013-04-11

1002-8331（2014）24-0199-05

CNKI網(wǎng)絡(luò)優(yōu)先出版：2013-05-13，http∶//www.cnki.net/kcms/detail/11.2127.TP.20130513.1601.003.html

◎信號(hào)處理◎