Computer Engineering and Applications 计算机工程与应用 2014，50（24） 199 实时三维重建算法的实现 ——基于 Kinect 与单目视觉 SLAM 的三维重建 夏文玲 1，顾照鹏 2，杨唐胜 2 XIA Wenling1, GU Zhaopeng2, YANG Tangsheng2 1.湖南大学 电气与信息工程学院，长沙 410000 2.北京大学深圳研究院 信息工程学院，广东 深圳 518000 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 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. Abstract：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. Key words：monocular vision; 3-D reconstruction; Simultaneous Localization and Mapping（SLAM）; Kinect 摘 要：作为计算机视觉技术的一个重要分支，基于单目视觉的三维重建技术以其要求简单、成本低廉、易于实现等优 点，得到了越来越多的关注。在室内环境下就智能机器人的同步定位以及环境地图创建（SLAM）算法展开了研究，引 入 RGB-D 相机 Kinect 直接获取 3D 场景的深度信息，实现了一种基于单目视觉 SLAM 与 Kinect 的实时三维重建方法。 关键词：单目视觉；三维重建；同步定位以及环境地图创建；Kinect 文献标志码：A 中图分类号：TP391 doi：10.3778/j.issn.1002-8331.1301-0306 1 引言 基于视觉的三维重建技术，是指利用数字摄像机作 为图像传感器，综合运用图像处理、视觉计算等技术进行 非接触三维测量，用计算机程序获取物体的三维信息[1]。 在计算机内生成物体三维表示主要有两类方法，一类是 利 用 几 何 建 模 软 件 ，如 3DMAX、Maya、AutoCAD、UG 等，另一类则是通过一定的手段获取真实物体的几何形 状，主要包括数据获取、预处理、点云拼接和特征分析等 步骤。 目前在单目视觉的同步定位与地图创建（Monocular Simultaneous Location and Mapping，MonoSLAM）系统 中[2]，3D 场景深度信息的获取是十分困难的，Davison 研 究团队推出反向深度算法获取深度信息 [3]，而采用深度 相机主动获取场景中的深度信息，在很大程度上降低了 该算法的复杂度。目前深度相机主要分为两类，一类 是 TOF 相机，一类即为 Kinect（Kinect for Xbox 360）[4]。 Kinect 是由微软开发的应用于 Xbox 360 主机的周边设 备。在基于 Kinect 实现的 RGB-D 摄像机 SLAM 系统研 基金项目：中国医学科学研究院北京协和医院“863”计划（No.754214019）。 作者简介：夏文玲（1987—），女，硕士研究生，主要研究方向为计算机视觉导航与定位、嵌入式系统；顾照鹏（1981—），男，博士 研究生，讲师，主要研究方向为基于视觉的实时定位系统；杨唐胜（1967—），男，博士研究生，副教授，主要研究方向为 智能算法、智能仪器。E-mail：xia1234zhao@126.com 收稿日期：2013-01-28 修回日期：2013-04-11 文章编号：1002-8331（2014）24-0199-05 CNKI 网络优先出版：2013-05-13，http://www.cnki.net/kcms/detail/11.2127.TP.20130513.1601.003.html 

200 2014，50（24） Computer Engineering and Applications 计算机工程与应用 究方面，Nikolas 利用 Kinect 提供的 RGB-D 相机实现了 一个实时的视觉 SLAM 系统，并将这一系统集成到 ROS （Robot Operating System）中[5]。Henry 等利用 Kinect 实 现了一个交互的实时三维重建系统 RGB-D Mapping[6-7]， 该系统不进行实时的 ICP[8]配准，而是根据用户需要，选 取不同的关键帧进行配准，其重建出的点云能很好地与 建筑物俯视图相吻合，但该系统实际运行速度不高，未 进行 ICP 配准运行帧率为 5 frame/s，而进行 ICP 配准后 单帧处理时间至少需要 500 ms。微软研究院的 Kinect- Fusion 系统给出了一种基于 GPU 并计算的实时的定位 与重建系统，实现了动态场景的增强现实应用，该系统 利用 GPU 实现了实时 ICP 配准，并同步生成环境地图的 三维表面[9-10]。 由于 ICP 算法计算复杂度高，而利用 GPU 加速的 ICP 算法对于硬件要求较高，制约了其使用范围。由于目前 的单目视觉 SLAM 算法的实时性和准确性都有一定保 证，因此本文设计一种基于扩展卡尔曼滤波（EKF）的单 目视觉 SLAM 与 Kinect 的室内环境实时三维重建方法， 并与基于光流跟踪算法（Kanade Lucas Tomasi，KLT）[11] 与 Kinect 实现的三维重建方法相比较，实验结果证明该 方法具有实时性好，一致性强等优点。 2 Kinect 工作原理 Kinect 有三个镜头，中间的镜头是 RGB 彩色摄影机， 左右两边镜头分别为红外线发射器和红外 CMOS 摄影 机所构成的 3D 结构光深度感应器。Kinect 还搭配了追 焦技术，底座马达会随着对焦物体移动跟着转动。 2.1 Kinect 红外摄像机坐标系下深度的计算 Kinect 利用红外激光投影和红外摄像机组成一组 红外结构光视觉设备，计算空间点相对于红外摄像机的 深度，如图 1 所示为其原理图。首先由红外激光投影机 向环境中投影红外随机点结构光，然后利用红外摄像机 进行接收，并在每一像素周围的±64 像素范围内，利用 9×9 或者 7×7 的图像 patch，利用 NCC（Normalized Cross Correlation）对图像随机点进行匹配，获得原始视差，进 而利用红外相机的内参数以及基线长度获得环境三维 点在红外相机下的深度信息，如公式（1）所示： IR /d = b ´ f 为深度值，b 为基线长度，f Z IR 其中 Z 焦距，d 为视差。红外摄像机的内参数矩阵 K IR IR （1） 为红外摄像机的 ： IR K IR = æ ç ç çç è f uIR 0 0 0 f vIR 0 u v ö ÷ ÷ ÷÷ ø 0IR 0IR 1 ) 为红外摄像机焦距，(u uIR , f ( f 主点坐标。 vIR （2） , v 0IR 0IR ) 为红外摄像机 红外激光 投影仪 αβ RGB 相机 红外 相机 P 图 1 Kinect 原理图 2.2 彩色摄像机坐标下的坐标计算 Kinect 的 RGB 摄像头采集到的是环境的彩色图像， RGB 摄像头的内参数矩阵为 K ： （3） u v 0 f vRGB 0 RGB ö ÷ ÷ ÷÷ ø 0RGB 0RGB 1 K RGB = æ ç ç çç è f uRGB 0 0 , f uRGB ) 为 RGB 摄像机焦距，(u 上式中 ( f ) 为 RGB 摄像机光心。IR 摄像机与 RGB 摄像机的刚体变化 T =[R ] 。 t 0RGB 0RGB vRGB ,v RGB - IR RGB - IR RGB - IR 对于空间中的一点 P ，其在彩色摄像机坐标下的三 维坐标计算流程如图 2 所示，由 Kinect 获得 P 点的视差 d ，根据式（1）计算得到 P 点的深度 Z ，进一步通过红 外摄像机内参数获得 P 点在红外摄像机下的三维坐标： IR ) IR IR 0IR uIR = X bf )/d = b(u - u 0IR (v - v f d ì ïï í Y ïï î 通过 IR 摄像机与 RGB 摄像机的旋转平移关系得到 P 点在 RGB 摄像机坐标下的三维坐标，最后引入 RGB 摄像机内参数计算得到图像坐标 (u ) 。 ,v vIR （4） RGB RGB ö ÷ ÷÷÷ ø + t IR - RGB ö ÷ ÷÷÷ ø u v RGB RGB X Y Z æ ç ççç è 1 RGB æ ç ç çç è Z RGB = R IR - RGB æ ç ççç è X Y Z ö ÷ ÷ ÷÷ ø RGB RGB 1 = K RGB æ ç ççç è IR IR IR X Y Z ö ÷ ÷÷÷ ø RGB RGB RGB （5） （6） IR 摄像机内参数 以及 Kinect 基线 IR 摄像机与 RGB 摄像机 的刚体变换 T RGB - IR RGB 摄像机 内参数 点 P 在 Kinect 下 的原始视差 d IR 摄像机坐标下的深度以及 三维坐标 (X Y IR Z IR IR ) RGB 摄像机下的三维坐标 (X Y RGB RGB Z ) RGB RGB 摄像机坐标下的 图像坐标 (u ) v RGB RGB 图 2 RGB 摄像机坐标下的空间点坐标计算流程 

夏文玲，顾照鹏，杨唐胜：实时三维重建算法的实现 2014，50（24） 201 3 基于 Kinect 的单目视觉 EKF-SLAM 框架 程为： 在视觉 SLAM 系统中，摄像机的状态 x 由世界坐 c 标系下摄像机坐标位置 rT 、旋转四元数 qT 、线速度 vT 、 角速度 ωT 组成： = C i æ çç è u v i i ö ÷÷ ø + η i = æ ç ç ç çç ç è f u f v ´ x z ic ´ y z ic ic + u 0 ic + v 0 ö ÷ ÷ ÷ ÷÷ ÷ ø + η i c = (rTqTvTωT)T （7） x 地图特征状态向量由特征点的三维世界坐标组成： （8） f i 摄像机状态向量和特征点状态向量组成 SLAM 系 y i = (x z i )T i 统状态向量： f T 1 x = (x T （9） c 本文中 SLAM 系统中采用匀速模型作为过程模型， f T i f T n f T 2 )T 得到相机状态预测方程[12]： + (v r t t ´ q((ω q t = t + 1 æ ç ç ç çç ç è ö ÷ ÷ ÷ ÷÷ ÷ ø r t + 1 q v ω æ ç ç ç çç ç è ]T 为过程噪声。 t + 1 t + 1 v t ω t x ct + 1 = 其中 [n n v ω )Dt + n v + n )Dt ω t + n v + n ω ö ÷ ÷ ÷ ÷÷ ÷ ø （10） 空间任何一点 P 在图像上的成像位置可以用针孔 模型近似表示，如图 3 所示。即任何点 P 在图像上的投 影位置 p，为光心 O 与 P 点的连线 OP 与图像平面的交 点，有如下关系式： x = y = ì ï ï í ï ï î C C fX Z C fY Z C 其中 (x, y) 为 P 点在成像平面下的坐标，(X 空间点 P 在摄像机坐标下的坐标。 Y C Z C C ) 为 Y c Y ′ （13） （14） 其中： x ic y z æ ç ççç è ic = R*X W + t ö ÷ ÷÷÷ ø ic ) 为 Kinect 彩色摄像机焦距，(u v u f ( f 色摄像机光心，R、t 为彩色摄像机的外参数，(x )T 为特征点在摄像机坐标下的三维坐标，X W 为特征点在 世界坐标系下的坐标。 ) 为 Kinect 彩 y v z ic ic ic 0 0 利用 Kinect 获得特征点在彩色摄像机坐标下的三 维坐标，根据当前 SLAM 输出摄像机位姿 (r ) 初始 化特征点在世界坐标系下的三维位置。设新增特征点 为 f ，其对应在 Kinect 彩色摄像机下的坐标估计和方 n + 1 差为： q t t X RGB = x y z æ ç ççç è RGB RGB RGB ö ÷ ÷÷÷ ø RGB æ σ 2 ç x ç ç çç ç è )X t σ z （15） （16） σ 2 y RGB ö ÷ ÷ ÷ ÷÷ ÷ ø σ 2 z RGB + r （17） 为 Kinect 的输出转换到彩色摄像机 t f 其中 σ n + 1 x RGB = R(q σ y RGB RGB RGB 坐标下的三个方向的标准差。新增特征点之前的系统 状态向量和协方差分别为 x 和 P ，则新增特征点 f 之后系统状态变为： n + 1 （11） P X X RGB RGB = 成像平面 F O 1 X c P (xy) x′ P(X Y Z ) c c c P′ = æ ç ç çç ç ç è x′ = æ ç è x f n + 1 ö ÷ ø P P( ¶f n + 1 ¶x P ¶f n + 1 ¶x P( ¶f n + 1 ¶x )T + ¶f ¶X （18） ö ÷ ÷ （19） ÷÷ ÷ ÷ ø )T )T P ( X X RGB RGB ¶f ¶X n + 1 RGB ¶f n + 1 ¶x n + 1 RGB O O′ Z c x 1 P (xy) y 1 虚拟成像平面 图 3 针孔成像模型 本文中使用的是特征地图，观测量由一组成功匹配 的地图特征点的图像坐标组成： C = (C 1 C 2  C )T n 其中 n 表示匹配成功的特征点数目，C i 个特征点的图像坐标。引入测量噪声 η i = (u （12） )T 为第 i 得到测量方 v i i 整个系统流程图如图 4 所示，在定位过程中，若当 前彩色摄像机可见重建点云总数小于设定阈值（像素总 数的 2/3），就利用当前彩色摄像机 SLAM 算法得到的外 参数 T ] 将彩色摄像机下的三维点云调整到 世界坐标系下，并加入重建点云中。否则认为场景变化 不大，不对重建点云进行调整。 =[R t cw cw cw 4 实验及结果分析 本文实验场景为办公室，使用深度摄像机 Kinect， RGB 相机图像分辨率为 640×480。使用 Fast 角点检测 算法[13]，特征描述子为 11×11 的图像 Patch。本文程序基 

202 2014，50（24） Computer Engineering and Applications 计算机工程与应用 Kinect 图像&深度 MonoSLAM 是否初始化 N MonoSLAM 初始化 =[R T ] （a）正前方视图 （b）右前方视图 初始化 T 1W 加入空间点云 失败处理 重定位 N Y MonoSLAM Tracting 成功定位 Y 获得当前帧外参数 T cw 投影获得可见点云所 占图像范围 N 可见点总数 <2/3 图像 Y 利用 T 调整当前帧的 三维点云到世界坐标系 cw 并加入点云集 MonoSLAM 跟踪 成功点数<4 N Y Fast 角点检测 利用当前图像深度信息 初始化角点三维坐标 图 4 系统流程图 于 VS2010 的 C++实现上述算法，所有实验都是在 Intel 双核 2.93 GHz CPU 的计算机上运行。为了验证本文方 法的实时性和稳定性，手持 Kinect 在场景中进行自由运 动采集数据，并在系统运行时实时地利用 EKF-SLAM 算法计算出摄像机位置和姿态，运动轨迹如图 5 所示， 实时三维重建实验结果如图 6 所示。 －0.4 －0.6 －0.8 Z －1.0 －1.2 0.15 0.10 Y 0.05 0.2 0.1 0 －0.05 －0.3 －0.2 －0.1 0 X 图 5 运动轨迹 利用光流跟踪算法可以快速获取相邻帧之间的特 征点匹配，由于 Kinect 可以得到图像特征点相应的深度 信息，从而可以得到相邻帧之间三维点的匹配关系。基 于已获得的三维点匹配结果后，优化相邻帧之间的刚体 变换参数。在上述相同的环境条件以及处理方式下，利 用 Fast 角点检测，将重建点云投影到彩色摄像机下，当可 （c）左前方视图 （d）右上方视图 图 6 不同视角下基于 Kinect 与单目 视觉 SLAM 的三维重建结果 见点云总数小于设定阈值（像素总数的 2/3），将当前的三 维点云调整到世界坐标系下，并加入重建点云中。这样 就得到了基于光流跟踪与 Kinect 的三维重建结果，如图 7 所示。由于随着特征点数目的增多，视觉 SLAM 算法的 时间复杂度会逐渐增大，本文实验中 SLAM 算法检测到 的特征点数目限制在 100 以内。此时帧率为 30 frame/s。 而基于 Kinect 与光流跟踪的三维重建算法由于 KTL 光 流跟踪的运算量较大，帧率只能达到 15 frame/s。 （a）正前方视图 图 7 基于光流跟踪与 Kinect 的三维重建结果 （b）左前方视图 基于上述实验，选择环境中 21×14.85 的黑色方格和 电脑两个物体，查看它们在两种方法下的三维重建细节 图对比，由 Kinect 的 RGB 相机拍摄的真实场景的图片， 如图 8 所示。 （a）基于 SLAM 算法细节图 （b）基于光流跟踪 算法细节图 （c）基于 SLAM 算法细节图 （d）基于光流跟踪 算法细节图 （e）显示场景图 1 （f）显示场景图 2 图 8 三维重建细节图对比 

夏文玲，顾照鹏，杨唐胜：实时三维重建算法的实现 2014，50（24） 203 如图 8 所示，（a）、（c）为基于 SLAM 的三维重建结 果细节图，（b）、（d）为基于光流跟踪的三维重建细节图， （e）、（f）为由 RGB 相机拍摄的真实场景的图片，由图 8 明显看出（b）中坐标产生了偏移，（d）中细节出现匹配错 误；（a）与（c）没有较大的误差。 基于上述实验结果，可以看出基于 Kinect 与单目视 觉 SLAM 算法实现的三维重建方法在保证系统实时运 行的前提下，不同帧加入的点云匹配之间，没有出现较 大的不一致现象。而基于光流跟踪与 Kinect 的三维重 建方法中，由于光流匹配中存在着错误匹配，并且光流 跟踪没有考虑到 Loop-Closure 问题，因此得到的三维重 建有漂移现象，出现了重建结果错误的情况。 5 结束语 本文针对基于 Kinect 与单目视觉 SLAM 的三维重 建展开研究，并与基于 Kinect 与光流跟踪算法结果进行 比较，实验结果表明该方法具有实时性好，一致性强等 优点，在一定程度上提高了地图重建点云的精度。目前 由于 Kinect 自身精度的原因，本文方法主要用在室内场 景的实时三维重建。在未来的工作中，将在现有工作的 基础上，在算法实现、系统集成、室外环境的三维重建算 法等方面开展进一步的研究。 参考文献： [1] 佟帅，徐晓刚，易成涛，等.基于视觉的三维重建技术综述[J]. 计算机应用研究，2011，28（7）：2012-2015. [2] 顾照鹏，董秋雷.基于部分惯性传感器信息的单目视觉的 同步定位与地图创建方法[J].计算机辅助设计与图形学学 报，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 深度图像快速修复算法[J]. 上海大学学报，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 Ubiq- uitous 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：real- time dynamic 3D surface reconstruction and interaction[C]// Proceedings of Association for Computing Machinery’s Special Interest Group on Computer Graphics and Inter- active 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 Sym- posium 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]//Inter- national 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]//Proceed- ings 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. （上接 142 页） [5] Shao M W，Zhang W X.Dominance relation and rules in an incomplete ordered information system[J].International Journal of Intelligent Systems，2005，20（1）：13-27. 及其属性约简方法[J].厦门大学学报：自然科学版，2009， 48（4）：482-488. [11] 陈子春，刘鹏惠，秦克云.集值信息系统基于优势关系下 的知识约简[J].计算机科学，2009，36（12）：176-193. [6] 张腾飞，魏立力.集中有序集值信息系统[J].计算机工程与 [12] 林耀进，李进金，林梦雷.优势关系下的集值序值信息系 应用，2014，50（16）:140-145. [7] 邓聚龙.灰理论基础[M].武汉：华中科技大学出版社，2002. [8] Deng J L.Control problems of grey system[J].System & 统[J].计算机应用，2011，31（12）：3240-3246. [13] 张文修，仇国芳.基于粗糙集的不确定决策[M].北京：清华 大学出版社，2005. Control Letter，1982，1（5）：288-294. [14] 廖启明，龙鹏飞.基于属性重要性的粗糙集属性约简方法[J]. [9] 吴顺祥.灰色粗糙集模型及其应用[M].北京：科学出版社， 计算机工程与应用，2013，49（15）：130-132. 2009. [15] 林耀进，李进金，吴顺祥，等.不完备灰色信息系统的粗集 [10] 吴顺祥，林理华，周志文.基于灰色信息系统的优势关系 模型[J].计算机应用，2010，30（2）：3374-3376.