第15卷第5期 智能系统学报 Vol.15 No.5 2020年9月 CAAI Transactions on Intelligent Systems Sep.2020 D0:10.11992/tis.201903007 网络出版地址：http:/kns.cnki.net/kcms/detail/23.1538.tp.20191012.1719.006.html 基于kinect的改进RGB-D视觉里程计 朱俊涛，陈强 (上海工程技术大学电子电气工程学院，上海201600) 摘要：针对RGB-D视觉里程计中kinect相机所捕获的图像深度区域缺失的问题，提出了一种基于PnP(per- spective-n-point)和ICP(iterative closest point)的融合优化算法。传统ICP算法迭代相机位姿时由于深度缺失，经 常出现特征点丢失导致算法无法收敛或误差过大。本算法通过对特征点的深度值判定，建立BA优化模型，并 利用g20求解器进行特征点与相机位姿的优化。实验证明了该方法的有效性，提高了相机位姿估计的精度及 算法的收敛成功率，从而提高了RGB-D视觉里程计的精确性和鲁棒性。 关键词：kinect:深度丢失：融合算法；特征点；ICP;PnP;深度值；位姿估计；BA优化模型；g2o 中图分类号：TP242.6文献标志码：A文章编号：1673-4785(2020)05-0943-06 中文引用格式：朱俊涛，陈强.基于kinect的改进RGB-D视觉里程计.智能系统学报，2020,15(5)：943-948 英文引用格式：ZHUJuntao,,CHEN Qiang.Improvement of kinect performance in RGB-D visual odometer.CAAI transactions on intelligent systems,2020,15(5):943-948. Improvement of kinect performance in RGB-D visual odometer ZHU Juntao,CHEN Qiang (Electrical and Electronic Engineering College,Shanghai University of Engineering and Technology,Shanghai 201600,China) Abstract:Kinect is a 3D camera that gives you the depth values associated with every pixel.It uses structured infrared light to determine depth values.Apart from these,you also have access to raw RGB-D data,and even the raw infrared data.Aiming to solve the problem of insufficient depth values for the images captured by Kinect camera in RGB-D visu- al odometer,we propose a fusion optimization algorithm based on Perspective-n-Point and iterative closest point(ICP). Because of the lack of depth values,traditional ICP algorithm often loses feature points when iterating the camera pose; this results in excessive error,or we can say that the algorithm is unable to converge.This algorithm establishes bat al- gorithm optimization model by judging the depth of feature points and optimizes the feature point of poses and camera using g2o solver.Experiments show that the method is effective and improves the accuracy of camera pose estimation and the convergence success rate of the algorithm,thus improving the accuracy and robustness of RGB-D visual odo- meter Keywords:kinect;lack of depth;fusion algorithm;feature points;iterative closest point;perspective-n-point;depth value;pose estimation;BA optimization model;g2o 近年来，视觉里程计(visual odometry,VO)川 机器人、自动驾驶等领域。随着RGB-D深度相 作为视觉SLAM的前端，利用单个或多个视觉传 机的产生，如微软的kinect、Pmdtec的Camcube 等，利用RGB-D的视觉里程计方法得到越来越广 感器，获取图像的信息进而估计出相机运动，给 泛的研究。 后端提供良好的初始值。视觉传感器因其获取信 视觉里程计的实现方法一般按是否需要提取 息丰富、成本低、易携带等诸多优点，被广泛用于 特征分为两种：1)特征点法的前端即基于特征的 收稿日期：2019-03-09.网络出版日期：2019-10-14. 视觉里程计(FVO);2)不提特征的直接法前端即 基金项目：国家自然科学基金项目(61272097)：上海市科技委 员会重点项目(18511101600). 稠密视觉里程计(DVO)。DVO一般用图像帧的 通信作者：陈强.E-mail:sues_chen@sues.edu.cn. 所有像素信息去估计相机位姿，用到了图像一致
DOI: 10.11992/tis.201903007 网络出版地址: http://kns.cnki.net/kcms/detail/23.1538.tp.20191012.1719.006.html 基于 kinect 的改进 RGB-D 视觉里程计 朱俊涛，陈强 （上海工程技术大学 电子电气工程学院，上海 201600） 摘 要：针对 RGB-D 视觉里程计中 kinect 相机所捕获的图像深度区域缺失的问题，提出了一种基于 PnP(perspective-n-point) 和 ICP(iterative closest point) 的融合优化算法。传统 ICP 算法迭代相机位姿时由于深度缺失，经 常出现特征点丢失导致算法无法收敛或误差过大。本算法通过对特征点的深度值判定，建立 BA 优化模型，并 利用 g2o 求解器进行特征点与相机位姿的优化。实验证明了该方法的有效性，提高了相机位姿估计的精度及 算法的收敛成功率，从而提高了 RGB-D 视觉里程计的精确性和鲁棒性。 关键词：kinect；深度丢失；融合算法；特征点；ICP；PnP；深度值；位姿估计；BA 优化模型；g2o 中图分类号：TP242.6 文献标志码：A 文章编号：1673−4785(2020)05−0943−06 中文引用格式：朱俊涛, 陈强. 基于 kinect 的改进 RGB-D 视觉里程计 [J]. 智能系统学报, 2020, 15(5): 943–948. 英文引用格式：ZHU Juntao, CHEN Qiang. Improvement of kinect performance in RGB-D visual odometer[J]. CAAI transactions on intelligent systems, 2020, 15(5): 943–948. Improvement of kinect performance in RGB-D visual odometer ZHU Juntao，CHEN Qiang (Electrical and Electronic Engineering College, Shanghai University of Engineering and Technology, Shanghai 201600, China) Abstract: Kinect is a 3D camera that gives you the depth values associated with every pixel. It uses structured infrared light to determine depth values. Apart from these, you also have access to raw RGB-D data, and even the raw infrared data. Aiming to solve the problem of insufficient depth values for the images captured by Kinect camera in RGB-D visual odometer, we propose a fusion optimization algorithm based on Perspective-n-Point and iterative closest point (ICP). Because of the lack of depth values, traditional ICP algorithm often loses feature points when iterating the camera pose; this results in excessive error, or we can say that the algorithm is unable to converge. This algorithm establishes bat algorithm optimization model by judging the depth of feature points and optimizes the feature point of poses and camera using g2o solver. Experiments show that the method is effective and improves the accuracy of camera pose estimation and the convergence success rate of the algorithm, thus improving the accuracy and robustness of RGB-D visual odometer. Keywords: kinect; lack of depth; fusion algorithm; feature points; iterative closest point; perspective-n-point; depth value; pose estimation; BA optimization model; g2o 近年来，视觉里程计 (visual odometry, VO)[1] 作为视觉 SLAM 的前端，利用单个或多个视觉传 感器，获取图像的信息进而估计出相机运动，给 后端提供良好的初始值。视觉传感器因其获取信 息丰富、成本低、易携带等诸多优点，被广泛用于 机器人、自动驾驶[2] 等领域。随着 RGB-D 深度相 机的产生，如微软的 kinect、Pmdtec 的 Camcube 等，利用 RGB-D 的视觉里程计方法得到越来越广 泛的研究。 视觉里程计的实现方法一般按是否需要提取 特征分为两种：1) 特征点法的前端即基于特征的 视觉里程计 (FVO)；2) 不提特征的直接法前端即 稠密视觉里程计 (DVO)。DVO 一般用图像帧的 所有像素信息去估计相机位姿，用到了图像一致 收稿日期：2019−03−09. 网络出版日期：2019−10−14. 基金项目：国家自然科学基金项目 (61272097)；上海市科技委 员会重点项目 (18511101600). 通信作者：陈强. E-mail：sues_chen@sues.edu.cn. 第 15 卷第 5 期 智 能 系 统 学 报 Vol.15 No.5 2020 年 9 月 CAAI Transactions on Intelligent Systems Sep. 2020
·944· 智能系统学报 第15卷 性假设，最早由Steinbruecker等提出，将问题转 挡、吸收、散斑、反射等原因导致深度区域缺失， 化为能量最小化处理，但是由于其数据及计算量 在实际测试中发现，缺失特征点数在最差情况下 巨大，无法达到实时目的。相对于直接法前端， 将近占总特征点数的三分之一，这就使得单纯使 基于特征点法的前端一直以来被认为是视觉里程 用ICP算法产生很大的误差。 计的主流方法，它具备运行稳定、计算量较小、对 针对以上，本文提出一种基于kinect相机的 光照及动态物体相对不敏感等优点，是目前较为 改进RGB-D视觉里程计方法，由改进ORB算法 成熟的视觉里程计方案。 进行RGB-D图像特征匹配，对kinect所得深度图 FVO通过寻找图像帧的特征点，通过匹配特 进行处理，先用RANSAC筛选出可靠内点，得到 征点来估计相机位姿，特征点拥有稳定的局部图 位姿初始估计，接着对深度缺失的3D-2D特征点 像特征，如著名的尺度不变特征变换(scale-invari-- 对进行PnP优化，对其他3D-3D特征点对进行 ant feature transform,SIFT)、加速鲁棒特征 ICP优化，并将两者转化为一个BA(bundle adjust- (speed--up robust features,.SURF)、ORB等，根据 ment)问题建立优化模型，使用g2o求解器同时进 匹配的特征采用迭代最近点ICP)算法m进行位 行特征点及相机位姿优化，最后实验结果证明了 姿估计，为了减少噪声及误匹配带来的影响，往 该方法的有效性。 往采用由Fischler和Bolles提出的随机误差一致 1改进的RGB-D视觉里程计 性(RANSAC)算法来进行优化，剔除误差匹配。 SIFT充分考虑了图像在变换过程中的尺度旋转 本文提出的基于kinect相机的改进RGB- 等变换，但伴随的极大计算量是里程计无法接受 D视觉里程计框架如图1所示，对相机捕获的彩 的，至今普通的CPU还无法实时计算SIFT特征， 色图用改进的ORB算法进行检测与匹配，并对所 而ORB特征通过改进FAST检测子例，并使用速 得深度图进行分析，当检测深度不为0时，利用 度极快的二进制描述子BRIEF1I使得实时 RANSAC算法剔除3D-3D误差点得到初始位姿 SLAM变成了可能，但ORB也存在尺度光照不变 估计，采用ICP算法进行优化；当检测深度为O 性缺失的问题，文献[I1]提出了改进FAST特征，时，利用RANSAC算法剔除3D-2D误差点得到初 将图像结合高斯金字塔模型实现了FAST角点的 始位姿估计，采用PP算法进行优化。最后，将 尺度不变性。文献[1213]提出通过缩小抽样点总 两者误差项混合转化为一个BA问题建立优化模 量来保证匹配点选取质量，有效地减少迭代次数。 型，使用g20求解器进行特征点及位姿的优化。 然而，kinect相机所捕获的深度图像由于遮 Y RANSAC 深度图 PnP RGB-D 改进ORB 深度 建立BA 位姿 图像 算法 D=07 优化模型 优化 RANSAC 彩色图 ICP 图1改进RGB-D视觉里程计框架 Fig.1 Improved RGB-D visual odometer framework 1.1改进的ORB算法 壮性。 ORB算法是由Rublee等提出的一种改进 在旋转不变性上，ORB特征采用灰度质心法 FAST角点和BRIEF描述子的描述算法，其在特 (intensity centroid)实现，具体步骤如下： 征提取上具有良好的性能。它改进了FAST检测 1)在图像块中定义图像矩为 子不具有方向性的问题，采用二进制描述子BREF m网=∑y1xW p,q=0,1) (1) 使得算法具有极快的运算速度，大大加速了图像 的特征提取环节，其实时性能远远高于SURF与 式中：m为区域的阶矩表示；I(x,y)为角点的灰度值。 SIFT,并且在质量与性能之间也有较好的折中， 2)得到图像的质心C: 本文在其尺度与光照上的缺陷做了一些改进，使 /m10101 (2) 得ORB算法在尺度及光照变化上具有一定的健 moo moo
性假设，最早由 Steinbruecker 等 [3] 提出,将问题转 化为能量最小化处理，但是由于其数据及计算量 巨大，无法达到实时目的。相对于直接法前端， 基于特征点法的前端一直以来被认为是视觉里程 计的主流方法，它具备运行稳定、计算量较小、对 光照及动态物体相对不敏感等优点，是目前较为 成熟的视觉里程计方案。 FVO 通过寻找图像帧的特征点，通过匹配特 征点来估计相机位姿，特征点拥有稳定的局部图 像特征，如著名的尺度不变特征变换 (scale-invariant feature transform，SIFT)[ 4 ] 、加速鲁棒特征 (speed-up robust features，SURF)[5] 、ORB[6] 等，根据 匹配的特征采用迭代最近点 (ICP) 算法[7] 进行位 姿估计，为了减少噪声及误匹配带来的影响，往 往采用由 Fischler 和 Bolles[8] 提出的随机误差一致 性 (RANSAC)算法来进行优化，剔除误差匹配。 SIFT 充分考虑了图像在变换过程中的尺度旋转 等变换，但伴随的极大计算量是里程计无法接受 的，至今普通的 CPU 还无法实时计算 SIFT 特征， 而 ORB 特征通过改进 FAST 检测子[9] ，并使用速 度极快的二进制描述 子 BRIEF [ 1 0 ] 使得实 时 SLAM 变成了可能，但 ORB 也存在尺度光照不变 性缺失的问题，文献 [11]提出了改进 FAST 特征， 将图像结合高斯金字塔模型实现了 FAST 角点的 尺度不变性。文献 [12-13]提出通过缩小抽样点总 量来保证匹配点选取质量，有效地减少迭代次数。 然而，kinect 相机所捕获的深度图像由于遮 挡、吸收、散斑、反射等原因导致深度区域缺失， 在实际测试中发现，缺失特征点数在最差情况下 将近占总特征点数的三分之一，这就使得单纯使 用 ICP 算法产生很大的误差。 针对以上，本文提出一种基于 kinect 相机的 改进 RGB-D 视觉里程计方法，由改进 ORB 算法 进行 RGB-D 图像特征匹配，对 kinect 所得深度图 进行处理，先用 RANSAC 筛选出可靠内点，得到 位姿初始估计，接着对深度缺失的 3D-2D 特征点 对进行 PnP 优化，对其他 3D-3D 特征点对进行 ICP 优化，并将两者转化为一个 BA(bundle adjustment) 问题建立优化模型，使用 g2o 求解器同时进 行特征点及相机位姿优化，最后实验结果证明了 该方法的有效性。 1 改进的 RGB-D 视觉里程计 本文提出的基于 kinect 相机的改进 RGBD 视觉里程计框架如图 1 所示，对相机捕获的彩 色图用改进的 ORB 算法进行检测与匹配，并对所 得深度图进行分析，当检测深度不为 0 时，利用 RANSAC 算法剔除 3D-3D 误差点得到初始位姿 估计，采用 ICP 算法进行优化；当检测深度为 0 时，利用 RANSAC 算法剔除 3D-2D 误差点得到初 始位姿估计，采用 PnP 算法进行优化。最后，将 两者误差项混合转化为一个 BA 问题建立优化模 型，使用 g2o 求解器进行特征点及位姿的优化。 深度图 彩色图 RANSAC ICP Y N RGB-D 图像 改进 ORB 算法 深度 D=0? RANSAC PnP 建立 BA 优化模型 位姿 优化 图 1 改进 RGB-D 视觉里程计框架 Fig. 1 Improved RGB-D visual odometer framework 1.1 改进的 ORB 算法 ORB 算法是由 Rublee 等提出的一种改进 FAST 角点和 BRIEF 描述子的描述算法，其在特 征提取上具有良好的性能。它改进了 FAST 检测 子不具有方向性的问题，采用二进制描述子 BRIEF 使得算法具有极快的运算速度，大大加速了图像 的特征提取环节，其实时性能远远高于 SURF 与 SIFT，并且在质量与性能之间也有较好的折中， 本文在其尺度与光照上的缺陷做了一些改进，使 得 ORB 算法在尺度及光照变化上具有一定的健 壮性。 在旋转不变性上，ORB 特征采用灰度质心法 (intensity centroid) 实现，具体步骤如下： 1) 在图像块中定义图像矩为 mpq = ∑ x,y∈B x p y q I(x, y), p,q = {0,1} (1) 式中：mpq为区域的阶矩表示； I(x, y) 为角点的灰度值。 2) 得到图像的质心 C： C = ( m10 m00 , m01 m00 ) (2) ·944· 智 能 系 统 学 报 第 15 卷
第5期 朱俊涛，等：基于kinect的改进RGB-D视觉里程计 ·945· 3)连接几何中心与质心，得到一个方向向 但由于相机的内参K一般都由厂家标定，为已知 量，定义特征点的方向为 信息，故设其归一化平面齐次坐标为p=[uv1「， 0=arctan(o1/m1o） (3) 此时相机的位姿R、t未知，定义增广矩阵[Rt] 以上方法使得FAST角点具有了旋转描述， 为3×4矩阵，包含了旋转与平移信息，形式为 大大提升了在不同图像之间表达的健壮性，这种 改进的FAST角点称为Oriented FAST。 (6) 尺度问题上，构建高斯图金字塔，如图2，对 1 tu 图像进行不同层次的采样，以获得不同分辨率图 假设有个3D-2D特征点，则可以列出如下的 像，并在金字塔的每一层上检测角点进而实现 线性方程组： ORB对尺度的健壮性。 P 0 0 P T (7) 0 -UN P 3 0 Level 2 由于t一共有12维，因此只需要6组匹配点 Level 1 即可实现矩阵的线性求解，得到变换矩阵后侧试 Level 0 其他匹配对，通过设定阈值选取模型内点并剔除 误差点，当内点足够多时则完成特征点的优化并 图2高斯图像金字塔 得到3D-2D的初始位姿估计（传r为李代数表达）。 Fig.2 Gaussian image pyramid 2)3D-3D模型 在光照问题上，FAST关键点所选用的灰度阈 相比于3D-2D点对，3D-3D点对的参数模型 值T是固定的，这就使得当周边环境对比度变 建立则简单了许多，现有空间点Q=[XYZT, 化时特征点的提取会受到很大的影响，采用文 与之匹配的3D点Q2=X2YZJ',想要找一个欧 献[14-15]的方法，选用一种根据图像对比度的自 式变换R,t,使得： 适应阈值方法，首先计算图像的对比度： 2=RO2+t (8) C=∑oi,P,) (4) 类似于3D-2D模型，R,t共有12维，而3D点 对存在3个约束，故只需4对匹配点即可完成线 式中：i,)=i-孔，即相邻像素灰度差；P(位，)为 性求解，得到3D-3D位姿初始估计50，求解方法 相邻像素间的灰度差为6的像素分布概率。 类似3D-2D模型。 由得到的对比度C设计自适应阈值T: 13BA模型与位姿优化 T=aC (5) PnP和ICP算法都是指通过多对3D与2D或 1.2 RANSAC算法完成初始化 3D与3D匹配点，在已知或者未知相机内参的情 PnP和ICP的迭代优化算法都依赖良好的初 况下，利用最小化误差函数来求解相机外参的算 值选取，初值选取不当会导致迭代陷入局部最优 法。在得到由RANSAC进行优化后的点对和位 或无法收敛，不能达到全局最优效果。RANSAC 姿初始估计后，本文提出一种基于PnP和ICP的 是一种随机参数估计算法，通过反复选取数据中 融合BA优化的算法。 的一组子集来建立参数化模型，并通过设置阈值 在PnP问题中，考虑N个三维空间点P及其 来进行内点的筛选，剔除误差数据，从而获得最 对应的归一化平面点，希望得到相机的位姿 佳模型参数。本文通过RANSAC算法分别对3D- R、t,在进行PP问题时本文采用李代数的方法， 2D点对和3D-3D点对进行粗匹配与初始位姿 因为使用李代数可以构建无约束的优化问题，从 估计，采用直接线性变换(DLT)建立和求解参数 而更方便地通过高斯牛顿法、列文伯格一马夸尔 模型。 特方法等优化算法进行求解。假设空间点坐标为 1)3D-2D模型 P=X,YZ,其对应像素坐标为p,=w,「，像素 现考虑空间点P,它的齐次坐标为 位置与空间点的李代数关系如下： P=[XYZ1]T sipi=Kexp()Pi (9) 由于在相邻帧中由于匹配点深度信息未知， 由于相机位姿及观测点的噪声，式(9)存在一
3) 连接几何中心与质心，得到一个方向向 量，定义特征点的方向为 θ = arctan(m01/m10) (3) 以上方法使得 FAST 角点具有了旋转描述， 大大提升了在不同图像之间表达的健壮性，这种 改进的 FAST 角点称为 Oriented FAST。 尺度问题上，构建高斯图金字塔，如图 2，对 图像进行不同层次的采样，以获得不同分辨率图 像，并在金字塔的每一层上检测角点进而实现 ORB 对尺度的健壮性。 Level 4 Level 3 Level 2 Level 1 Level 0 图 2 高斯图像金字塔 Fig. 2 Gaussian image pyramid 在光照问题上，FAST 关键点所选用的灰度阈 值 T 是固定的，这就使得当周边环境对比度变 化时特征点的提取会受到很大的影响，采用文 献 [14-15] 的方法，选用一种根据图像对比度的自 适应阈值方法，首先计算图像的对比度： C = ∑ δ δ(i, j) 2Pδ(i, j) (4) δ(i, j) = |i− j| Pδ(i, j) δ 式中： ，即相邻像素灰度差； 为 相邻像素间的灰度差为 的像素分布概率。 C T 由得到的对比度 设计自适应阈值 ∗ : T ∗ = αC (5) 1.2 RANSAC 算法完成初始化 PnP 和 ICP 的迭代优化算法都依赖良好的初 值选取，初值选取不当会导致迭代陷入局部最优 或无法收敛，不能达到全局最优效果。RANSAC 是一种随机参数估计算法，通过反复选取数据中 的一组子集来建立参数化模型，并通过设置阈值 来进行内点的筛选，剔除误差数据，从而获得最 佳模型参数。本文通过 RANSAC 算法分别对 3D- 2D 点对和 3D-3D 点对进行粗匹配与初始位姿 估计，采用直接线性变换 (DLT) 建立和求解参数 模型。 1) 3D-2D 模型 现考虑空间点 P ，它的齐次坐标为 P = [X Y Z 1]T 由于在相邻帧中由于匹配点深度信息未知， K p = [u v 1]T R t [R∥t ] 但由于相机的内参 一般都由厂家标定，为已知 信息，故设其归一化平面齐次坐标为 ， 此时相机的位姿 、 未知，定义增广矩阵 为 3×4 矩阵，包含了旋转与平移信息，形式为 s u v 1 = t1 t2 t3 t4 t5 t6 t7 t8 t9 t10 t11 t12 X Y Z 1 (6) 假设有个 3D-2D 特征点，则可以列出如下的 线性方程组： P T 1 0 −u1P T 1 0 P T 1 −v1P T 1 . . . . . . . . . P T N 0 −uN P T N 0 P T N −vN P T N T1 T2 T3 = 0 (7) t ξP ξP 由于 一共有 12 维，因此只需要 6 组匹配点 即可实现矩阵的线性求解，得到变换矩阵后测试 其他匹配对，通过设定阈值选取模型内点并剔除 误差点，当内点足够多时则完成特征点的优化并 得到 3D-2D 的初始位姿估计 ( 为李代数表达)。 2) 3D-3D 模型 Q1 = [X1 Y1 Z1] T Q2 = [X2 Y2 Z2 ] T R, t 相比于 3D-2D 点对，3D-3D 点对的参数模型 建立则简单了许多，现有空间点 ， 与之匹配的 3D 点 ，想要找一个欧 式变换 ，使得： Q1 = RQ2 + t (8) R, t ξQ 类似于 3D-2D 模型， 共有 12 维，而 3D 点 对存在 3 个约束，故只需 4 对匹配点即可完成线 性求解，得到 3D-3D 位姿初始估计 ，求解方法 类似 3D-2D 模型。 1.3 BA 模型与位姿优化 PnP 和 ICP 算法都是指通过多对 3D 与 2D 或 3D 与 3D 匹配点，在已知或者未知相机内参的情 况下，利用最小化误差函数来求解相机外参的算 法。在得到由 RANSAC 进行优化后的点对和位 姿初始估计后，本文提出一种基于 PnP 和 ICP 的 融合 BA 优化的算法。 N P R、t Pi = [Xi Yi Zi ] T pi = [ui vi] T 在 PnP 问题中,考虑 个三维空间点 及其 对应的归一化平面点，希望得到相机的位姿 ，在进行 PnP 问题时本文采用李代数的方法， 因为使用李代数可以构建无约束的优化问题，从 而更方便地通过高斯牛顿法、列文伯格—马夸尔 特方法等优化算法进行求解。假设空间点坐标为 ，其对应像素坐标为 ，像素 位置与空间点的李代数关系如下： si pi = Kexp(ξ ∧ )Pi (9) 由于相机位姿及观测点的噪声，式 (9) 存在一 第 5 期 朱俊涛，等：基于 kinect 的改进 RGB-D 视觉里程计 ·945·
·946· 智能系统学报 第15卷 个误差，将误差求和构建最小二乘问题，然后优 的空间坐标为P=[X'YZ',它的相机投影模型为 化相机位姿： X' Y w=fz+c,v=元+6g (15) ep=min=】 o-iKepen 10) 其中，、、cx、c,为相机内参，为相机出厂时 式中ep为像素坐标误差，是二维向量。 标定。 在ICP问题中，考虑一组数量为M的筛选后 在求导上本文使用李代数的左乘扰动模型 的匹配点对Q=XYZP和Q=[XYZ]I,它的 对左乘扰动量，然后使用链式法则，列写如下： 李代数位姿表达式为 dep ends⊕_eraP (16) Q:exp()0' (11) OP'06E 类似式(10)，可以构建误差函数： 式中：⊕指李代数上的左乘扰动；右边两项，其中 第1项是关于投影点的导数，由式(15)可得 eo=min= ∑le.-exp(Eg (12) j: 0 式中ee为空间坐标误差，是三维向量。 (17) 将式(10)与(12)融合为一个BA问题，构成 72 五维代价函数： 第2项是误差关于位姿李代数的导数，由文献[12] 可推导出： e LKexp()P. OP' (18) min =.-P"] Meo ∑Ie,-ep0f 将式(17)与(18)相乘即可得单个误差雅克比 M (13) 矩阵Jp: 其中，e为五维向量，下面对式(13)进行优化求解。 0-x fX'Y 2 72 在使用高斯牛顿法求解之前，需要知道每个 22 +Ax2 22 -2 a65 误差项关于优化变量专的一阶导数，即雅克比矩 乡-点--， E 22 阵J以及专的初始位姿。的取值为 Jo的推导与之相似，空间点Q=XYZ]T经位姿 6=对产n5+n (14) 变换为Q=XY"Z"「，使用李代数扰动模型即 可得 关于J的形式是本文关键所在，在ep中，像 000-Z"Y" 素坐标误差为二维，位姿为六维，J将是2×6的 0 10Z0-X" 矩阵；在ee中，空间坐标误差为三维，J。将是 B8E 001-YX0 3×6的矩阵，故误差e的雅克比矩阵为5×6矩阵。 综上，可得由N个3D-2D点对与M个3D- 首先分析J的形式，记变换到相机坐标系下 3D点对组成的BA代价函数e的雅克比矩阵J为 0 学 0 -乃出 户X,Y 宁X M 0 0 0 -2 0 M 0 0 0 得到了初始位姿与雅克比矩阵J后调用 2)对于第k次迭代，求出当前的雅克比矩阵 高斯牛顿法，算法流程如下： J与误差e。 1)给定初始值5。 3)求解增量方程H△5=g
个误差，将误差求和构建最小二乘问题，然后优 化相机位姿： eP = min ξ = ∑N i=1 pi − 1 si Kexp(ξ ∧ )Pi 2 (10) 式中 eP 为像素坐标误差，是二维向量。 M Q = [Xi Yi Zi] T Q ′ = [X ′ i Y ′ i Z ′ i ] T 在 ICP 问题中，考虑一组数量为 的筛选后 的匹配点对 和 ，它的 李代数位姿表达式为 Qi = exp(ξ ∧ )Q ′ i (11) 类似式 (10)，可以构建误差函数： eQ = min ξ = ∑M i=1 Qi −exp(ξ ∧ )Q ′ i 2 (12) 式中 eQ 为空间坐标误差，是三维向量。 将式 (10) 与 (12) 融合为一个 BA 问题，构成 五维代价函数： e = min ξ = 1 N eP 1 M eQ = 1 N ∑N i=1 pi − 1 si Kexp(ξ ∧ )Pi 2 1 M ∑M i=1 Qi −exp(ξ ∧ )Q ′ i 2 (13) 其中， e 为五维向量，下面对式 (13) 进行优化求解。 ξ J ξ ξ0 ξ0 在使用高斯牛顿法求解之前，需要知道每个 误差项关于优化变量 的一阶导数，即雅克比矩 阵 以及 的初始位姿 。 的取值为 ξ0 = N M +N ξP + M M +N ξQ (14) J eP JP eQ JQ e 关于 的形式是本文关键所在，在 中，像 素坐标误差为二维，位姿为六维， 将是 2×6 的 矩阵；在 中，空间坐标误差为三维 ， 将是 3×6 的矩阵，故误差 的雅克比矩阵为 5×6 矩阵。 首先分析 JP 的形式，记变换到相机坐标系下 P ′ = [X ′ Y ′ Z ′ ] 的空间坐标为 T，它的相机投影模型为 u = fx X ′ Z ′ +cx , v = fy Y ′ Z ′ +cy (15) fx、f 其中， y、cx、cy 为相机内参，为相机出厂时 标定。 ξ ∧ δξ 在求导上本文使用李代数的左乘扰动模型[16] ， 对 左乘扰动量 ，然后使用链式法则，列写如下： ∂eP ∂δξ = lim δξ→0 eP(δξ ⊕ξ) δξ = ∂eP ∂P′ ∂P ′ ∂δξ (16) 式中： ⊕ 指李代数上的左乘扰动；右边两项，其中 第 1 项是关于投影点的导数，由式 (15) 可得 ∂eP ∂P′ = − fx Z ′ 0 − fxX ′ Z ′2 0 fy Z ′ − fyY ′ Z ′2 (17) 第 2 项是误差关于位姿李代数的导数，由文献 [12] 可推导出： ∂P ′ ∂δξ = [I,−P ′∧ ] (18) JP 将式 (17) 与 (18) 相乘即可得单个误差雅克比 矩阵 ： ∂eP ∂δξ = − fx Z ′ 0 − fxX ′ Z ′2 − fxX ′Y ′ Z ′2 fx + fxX ′2 Z ′2 − fxY ′ Z ′2 0 fy Z ′ − fyY ′ Z ′2 −fy − fyY ′2 Z ′2 fyX ′Y ′ Z ′2 fyX ′ Z ′2 JQ Q ′ = [X ′ Y ′ Z ′ ] T Q ′′ = [X ′′ Y ′′ Z ′′] T 的推导与之相似，空间点 经位姿 变换为 ，使用李代数扰动模型即 可得 ∂eQ ∂δξ = − 1 0 0 0 −Z ′′ Y ′′ 0 1 0 Z ′′ 0 −X ′′ 0 0 1 −Y ′′ X ′′ 0 N M e J 综上，可得由 个 3D-2D 点对与 个 3D- 3D 点对组成的 BA 代价函数 的雅克比矩阵 为 J = ∑N i=1 fx Z ′ i 0 − ∑N i=1 fxX ′ i Z ′2 i − ∑N i=1 fxX ′ iY ′ i Z ′2 i N fx + ∑N i=1 fxX ′2 i Z ′2 i − ∑N i=1 fxY ′′ i Z ′2 i 0 ∑N i=1 fY Z ′ i − ∑N i=1 fyY ′ i Z ′2 i −N fy − ∑N i=1 fyY ′2 i Z ′2 i ∑N i=1 fyX ′ iY ′ i Z ′2 i ∑N i=1 fyX ′ i Z ′2 i M 0 0 0 − ∑M i=1 Z ′′ i ∑M i=1 Y ′′ i 0 M 0 ∑M i=1 Z ′′ i 0 − ∑M i=1 X ′′ i 0 0 M − ∑M i=1 Y ′′ i ∑M i=1 X ′′ i 0 得到了初始位姿 ξ0 与雅克比矩阵 J 后调用 高斯牛顿法，算法流程如下： 1) 给定初始值 ξ0。 J e 2) 对于第 k 次迭代，求出当前的雅克比矩阵 与误差 。 3) 求解增量方程 H∆ξk = g。 ·946· 智 能 系 统 学 报 第 15 卷
第5期 朱俊涛，等：基于kinect的改进RGB-D视觉里程计 ·947· 4)若△5足够小，则停止。否则，令点+1=点+△， 返回2) 2实验与结果 2.1实验环境 为了对本文算法的有效性进行验证，实验计 (a)RANSAC筛选前 算机的配置为：Intel is5双核1.8 GHz CPU,4GB内 存，500GB硬盘，NVIDIA独立显卡，操作系统为 64位的ubuntu14.04,算法基于C+、OpenCV3.0 和g20库进行编写。 2.2改进ORB特征点对比 在基于稀疏特征点法的里程计中，特征点的 数量是反映计算准确性的重要指标之一。与传统 (b)RANSAC筛选后 ORB相比，本文采用改进ORB算法在FAST特征 图4 RANSAC筛选结果 点提取上具有更好的适应性。原FAST特征点识 Fig.4 RANSAC screening result 别方法与改进后的特征点识别方法如图3所示。 2.4直线运动实验 本实验是在Turtlebot移动机器人平台上进行 的，设定直线运动速度0.3m/s,运动距离为3m。 分别进行传统ICP算法、RANSAC+ICP算法和融 合算法估计移动机器人运动轨迹，并使用g20求 解器进行位姿优化求解。g20是一个在SLAM领 域广为使用的优化库，可以将非线性优化与图论 相结合，大大提高系统的实时性与精确性，运动 (a)原ORB特征点检测法 轨迹如图5所示，结果表明，融合算法的平均误差 为1.98%，相比于传统1CP的8.64%与改进后的 RANSAC+HCP算法的5.28%有着更小的误差。 0.0500 其实轨迹 ·融合算法 0.0225 ICP算法 RANSAC+ICP -0.050 -0.0325 (b)改进的ORB特征点检测法 -0.0600 图3特征点对比结果 0 0.51.01.52.02.53.0 X/m Fig.3 Feature point comparison result 从图3可以看出，对于同一关键帧，当光照强 图5直线运动轨迹 Fig.5 The trajectory of linear motion 度不是很强时，改进之后的算法明显适应性更 从表1可以看出，在匹配点对上，直接ICP将 强，相比原算法多检测出了约20%的特征点，增 所有ORB特征点都进行迭代优化，RANSAC筛 强了整个系统的鲁棒性。 选后去除了近乎三分之一的误差点，本文算法在 2.3 RANSAC内点筛选 此基础上加上了被剔除的深度缺失的特征点；在 在基于稀疏特征点法的里程计中，特征点匹 迭代次数上可以看到相比直接ICP算法RANSAC 配精度决定了里程计的性能，图4为室外道路环 筛选过的ICP与本文算法都有了迭代次数明显地 境的两帧图像，图4(a)为未经RANSAC筛选的 减少；在收敛成功次数上本文算法存在显著优 ORB特征点匹配，可以看到有许多特征点的误匹 势，几乎达到了百分百成功率，大大提高了里程 配，这会对优化算法造成极大的误差，图4(b)为 计的鲁棒性；在优化时间上本次实验取的为单一 经RANSAC筛选后的特征匹配，匹配精度得到很 迭代优化的时间，并没有加上特征提取与匹配所 大提高。 花的时间，3种方法优化时间相对比较接近，整体
4) 若 ∆ξk足够小，则停止。否则，令 ξk+1 = ξk + ∆ξk， 返回 2)。 2 实验与结果 2.1 实验环境 为了对本文算法的有效性进行验证，实验计 算机的配置为：Intel i5 双核 1.8 GHz CPU，4 GB 内 存，500 GB 硬盘，NVIDIA 独立显卡，操作系统为 64 位的 ubuntu14.04，算法基于 C++、OpenCV3.0 和 g2o 库进行编写。 2.2 改进 ORB 特征点对比 在基于稀疏特征点法的里程计中，特征点的 数量是反映计算准确性的重要指标之一。与传统 ORB 相比，本文采用改进 ORB 算法在 FAST 特征 点提取上具有更好的适应性。原 FAST 特征点识 别方法与改进后的特征点识别方法如图 3 所示。 (a) 原 ORB 特征点检测法 (b) 改进的 ORB 特征点检测法 图 3 特征点对比结果 Fig. 3 Feature point comparison result 从图 3 可以看出，对于同一关键帧，当光照强 度不是很强时，改进之后的算法明显适应性更 强，相比原算法多检测出了约 20% 的特征点，增 强了整个系统的鲁棒性。 2.3 RANSAC 内点筛选 在基于稀疏特征点法的里程计中，特征点匹 配精度决定了里程计的性能，图 4 为室外道路环 境的两帧图像，图 4(a) 为未经 RANSAC 筛选的 ORB 特征点匹配，可以看到有许多特征点的误匹 配，这会对优化算法造成极大的误差，图 4(b) 为 经 RANSAC 筛选后的特征匹配，匹配精度得到很 大提高。 (a) RANSAC 筛选前 (b) RANSAC 筛选后 图 4 RANSAC 筛选结果 Fig. 4 RANSAC screening result 2.4 直线运动实验 本实验是在 Turtlebot 移动机器人平台上进行 的，设定直线运动速度 0.3 m/s，运动距离为 3 m。 分别进行传统 ICP 算法、RANSAC+ICP 算法和融 合算法估计移动机器人运动轨迹，并使用 g2o 求 解器进行位姿优化求解。g2o 是一个在 SLAM 领 域广为使用的优化库，可以将非线性优化与图论 相结合，大大提高系统的实时性与精确性，运动 轨迹如图 5 所示，结果表明，融合算法的平均误差 为 1.98%，相比于传统 ICP 的 8.64% 与改进后的 RANSAC+ICP 算法的 5.28% 有着更小的误差。 x/m y/m 0 0.5 1.0 1.5 2.0 2.5 3.0 0.050 0 0.022 5 −0.005 0 −0.032 5 −0.060 0 真实轨迹 融合算法 ICP 算法 RANSAC+ICP 图 5 直线运动轨迹 Fig. 5 The trajectory of linear motion 从表 1 可以看出，在匹配点对上，直接 ICP 将 所有 ORB 特征点都进行迭代优化，RANSAC 筛 选后去除了近乎三分之一的误差点，本文算法在 此基础上加上了被剔除的深度缺失的特征点；在 迭代次数上可以看到相比直接 ICP 算法 RANSAC 筛选过的 ICP 与本文算法都有了迭代次数明显地 减少；在收敛成功次数上本文算法存在显著优 势，几乎达到了百分百成功率，大大提高了里程 计的鲁棒性；在优化时间上本次实验取的为单一 迭代优化的时间，并没有加上特征提取与匹配所 花的时间，3 种方法优化时间相对比较接近，整体 第 5 期 朱俊涛，等：基于 kinect 的改进 RGB-D 视觉里程计 ·947·
·948· 智能系统学报 第15卷 里程计算法一般为30ms左右；在3种方法的相 [8]FISCHLER M A,BOLLES R C.Random sample con- 对误差上可以看出，相比直接ICP与RANSAC+ICP, sensus:a paradigm for model fitting with applications to 本文算法在估计相机误差精度上有着显著提高。 image analysis and automated cartography[J].Communica tions of the ACM.1981.24(6):381-395 表13种方法性能对比 [9]ROSTEN E,PORTER R,DRUMMOND T.Faster and bet- Table 1 Performance comparison of three methods ter:a machine learning approach to corner detection[J]. 100次实验平均值 位姿估 IEEE transactions on pattern analysis and machine intelli- 匹配迭代 收敛 优化 相对 计算法 gence,2010,32(1)105-119 点对对次数成功/次时间/ms误差/% [10]CALONDER M.LEPETIT V,STRECHA C,et al.BRIEF: 直接ICP 128.7421.35 65 2.256 8.64 binary robust independent elementary features[C]//Pro- RANSAC+ ceeding of the 11th European Conference on Computer 72.4812.51 81 1.825 5.28 ICP Vision.Heraklion:Springer,2010:778-792. 本文算法 89.6011.73 98 1.934 1.98 [11]翟紫伶.基于高斯尺度空间理论的ORB特征点检 测J.电子世界，2016(10)：192-194 3结束语 [12]葛山峰，于莲芝，谢振.基于ORB特征的目标跟踪算 法J.电子科技，2017,30(2：98-100,104 本文提出一种融合PnP与ICP的RGB-D视 GE Shanfeng,YU Lianzhi,XIE Zhen.A target tracking 觉里程计方法，有效针对kinect深度相机深度区 algorithm based on ORB[J].Electronic science and tech- nology,2017,30(2:98-100,104. 域丢失的缺点，利用改进ORB算法进行特征点提 [13]邢凯盛，凌有铸，陈孟元.ORB特征匹配的误匹配点别 取与匹配，再利用RANSAC算法得到良好的相机 除算法研究[.电子测量与仪器学报，2016,30(8)： 位姿估计初始值，根据特征点深度值的大小进行 1255-1262. PnP和ICP模型建立，并得到一个BA优化模型， XING Kaisheng,LING Youzhu,CHEN Mengyuan.Mis- 利用g20进行迭代求解，实验证明了该方法优秀 matching points elimination algorithm for ORB feature 的收敛成功率以及匹配相机位姿的精度，大大提 matching[J].Journal of electronic measurement and in- 高了视觉里程计的精确性与鲁棒性。 strumentation,2016,30(8):1255-1262. [14]STURM J.ENGELHARD N.ENDRES F.et al.A bench- 参考文献： mark for the evaluation of RGB-D SLAM systems[Cl// Proceedings of 2012 IEEE/RSJ International Conference [1]SCARAMUZZA D,FRAUNDORFER F.Visual odo- on Intelligent Robots and Systems.Vilamoura,Portugal. metry:part I:the first 30 years and fundamentals[J].IEEE 2012:573-580. robotics&automation magazine,2011,18(4):80-92 [15]KERL C,STURM J,CREMERS D.Dense visual SLAM [2]HANE C,HENG L,LEE G H,et al.3D visual perception for RGB-D cameras[Cl//Proceedings of 2013 IEEE/RSJ for self-driving cars using a multi-camera system:calibra- International Conference on Intelligent Robots and Sys- tion,mapping,localization,and obstacle detection[J].Im- tems.Tokyo,Japan,2013:2100-2106. age and vision computing,2017,68:14-27. [16]高翔，张涛.视觉SLAM十四讲[M.北京：电子工业出 [3]STEINBRUCKER F.STURM J.CREMERS D.Real-time 版社，2017：72-76 visual odometry from dense RGB-D images[C]//Proceed- ings of 2011 IEEE International Conference on Computer 作者简介： Vision Workshops.Barcelona,Spain:2011:719-722. 朱俊涛，硕士研究生，主要研究方 [4]NG P C,HENIKOFF S.SIFT:predicting amino acid 向为机器人。 changes that affect protein function[J].Nucleic acids re- search,2003.31(13):3812-3814. [5]BAY H,ESS A,TUYTELAARS T,et al.Speeded-up ro- bust features(SURF)[J].Computer vision and image un- derstanding,2008,110(3y:346-359. [6]RUBLEE E.RABAUD V.KONOLIGE K.et al.ORB:an 陈强，教授，主要研究方向为城市 efficient alternative to SIFT or SURF[C]//Proceedings of 管网探测理论与方法、仪器设备研发 2011 IEEE International Conference on Computer Vision. 与应用。获得多项省部级科技进步奖 Barcelona,Spain:2011:2564-2571. 与科技成果。发表学术论文50余篇。 [7]BESL P J,MCKAY N D.A method for registration of 3-D shapes[J].IEEE transactions on pattern analysis and ma- chine intelligence,1992,14(2):239-256
里程计算法一般为 30 ms 左右；在 3 种方法的相 对误差上可以看出，相比直接 ICP 与 RANSAC+ICP， 本文算法在估计相机误差精度上有着显著提高。 表 1 3 种方法性能对比 Table 1 Performance comparison of three methods 位姿估 计算法 100次实验平均值 匹配 点对/对 迭代 次数 收敛 成功/次 优化 时间/ms 相对 误差/% 直接ICP 128.74 21.35 65 2.256 8.64 RANSAC+ ICP 72.48 12.51 81 1.825 5.28 本文算法 89.60 11.73 98 1.934 1.98 3 结束语 本文提出一种融合 PnP 与 ICP 的 RGB-D 视 觉里程计方法，有效针对 kinect 深度相机深度区 域丢失的缺点，利用改进 ORB 算法进行特征点提 取与匹配，再利用 RANSAC 算法得到良好的相机 位姿估计初始值，根据特征点深度值的大小进行 PnP 和 ICP 模型建立，并得到一个 BA 优化模型， 利用 g2o 进行迭代求解，实验证明了该方法优秀 的收敛成功率以及匹配相机位姿的精度，大大提 高了视觉里程计的精确性与鲁棒性。 参考文献： SCARAMUZZA D, FRAUNDORFER F. Visual odometry: part I: the first 30 years and fundamentals[J]. IEEE robotics & automation magazine, 2011, 18(4): 80–92. [1] HÄNE C, HENG L, LEE G H, et al. 3D visual perception for self-driving cars using a multi-camera system: calibration, mapping, localization, and obstacle detection[J]. Image and vision computing, 2017, 68: 14–27. [2] STEINBRÜCKER F, STURM J, CREMERS D. Real-time visual odometry from dense RGB-D images[C]//Proceedings of 2011 IEEE International Conference on Computer Vision Workshops. Barcelona, Spain: 2011: 719−722. [3] NG P C, HENIKOFF S. SIFT: predicting amino acid changes that affect protein function[J]. Nucleic acids research, 2003, 31(13): 3812–3814. [4] BAY H, ESS A, TUYTELAARS T, et al. Speeded-up robust features (SURF)[J]. Computer vision and image understanding, 2008, 110(3): 346–359. [5] RUBLEE E, RABAUD V, KONOLIGE K, et al. ORB: an efficient alternative to SIFT or SURF[C]//Proceedings of 2011 IEEE International Conference on Computer Vision. Barcelona, Spain: 2011: 2564−2571. [6] BESL P J, MCKAY N D. A method for registration of 3-D shapes[J]. IEEE transactions on pattern analysis and machine intelligence, 1992, 14(2): 239–256. [7] FISCHLER M A, BOLLES R C. Random sample consensus: a paradigm for model fitting with applications to image analysis and automated cartography[J]. Communications of the ACM, 1981, 24(6): 381–395. [8] ROSTEN E, PORTER R, DRUMMOND T. Faster and better: a machine learning approach to corner detection[J]. IEEE transactions on pattern analysis and machine intelligence, 2010, 32(1): 105–119. [9] CALONDER M, LEPETIT V, STRECHA C, et al. BRIEF: binary robust independent elementary features[C]//Proceeding of the 11th European Conference on Computer Vision. Heraklion: Springer, 2010: 778−792. [10] 翟紫伶. 基于高斯尺度空间理论的 ORB 特征点检 测 [J]. 电子世界, 2016(10): 192–194. [11] 葛山峰, 于莲芝, 谢振. 基于 ORB 特征的目标跟踪算 法 [J]. 电子科技, 2017, 30(2): 98–100, 104. GE Shanfeng, YU Lianzhi, XIE Zhen. A target tracking algorithm based on ORB[J]. Electronic science and technology, 2017, 30(2): 98–100, 104. [12] 邢凯盛, 凌有铸, 陈孟元. ORB 特征匹配的误匹配点剔 除算法研究 [J]. 电子测量与仪器学报, 2016, 30(8): 1255–1262. XING Kaisheng, LING Youzhu, CHEN Mengyuan. Mismatching points elimination algorithm for ORB feature matching[J]. Journal of electronic measurement and instrumentation, 2016, 30(8): 1255–1262. [13] STURM J, ENGELHARD N, ENDRES F, et al. A benchmark for the evaluation of RGB-D SLAM systems[C]// Proceedings of 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems. Vilamoura, Portugal, 2012: 573−580. [14] KERL C, STURM J, CREMERS D. Dense visual SLAM for RGB-D cameras[C]//Proceedings of 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems. Tokyo, Japan, 2013: 2100−2106. [15] 高翔, 张涛. 视觉 SLAM 十四讲 [M]. 北京: 电子工业出 版社, 2017: 72−76. [16] 作者简介： 朱俊涛，硕士研究生，主要研究方 向为机器人。 陈强，教授，主要研究方向为城市 管网探测理论与方法、仪器设备研发 与应用。获得多项省部级科技进步奖 与科技成果。发表学术论文 50 余篇。 ·948· 智 能 系 统 学 报 第 15 卷
