第2期 张毅，等：基于图优化的移动机器人视觉SLAM ·293· 了一种基于局部闭环与全局闭环约束的点云优化算 法，一共产生了929个匹配特征点，但误匹配点过 法，同时完成对位姿与稠密化点云的优化。算法具 多。因此，通过人工制定规则的筛选，去除了部分 体步骤如下： 误匹配，点，此时的匹配个数为250。为进一步提升 l)封装构造数据帧Frame,根据帧间的关联程 匹配精读，通过RANSAC算法对图4(b)进行进一 度，筛选出关键帧序列。 步的匹配筛选。最终，产生了97个经筛选后的匹配 2)执行帧间配准。完成相机位姿初始估计。 点，实验中整个匹配所耗时间为0.0035s。 3)在相机观测约束下，直接应用式(6)，构建如 图3(a)所示的图优化。求得优化后的相机的位姿 变化(R,),将原始点云进行坐标系变换，实现点云的 拼接融合。 4)执行闭环检测。通过动态计算闭环帧筛选 阈值，得到匹配质量较高的闭环帧候选序列。 (a)特征匹配算法后 5)闭环检测结束后，遍历闭环帧候选序列，在 当前帧与闭环候选帧之间，进行Si3(相似变换矩 阵)求解与优化。当某一候选帧通过了Sim3求解 与优化，则认为闭环检测成功，跳出对闭环顿候选 序列的遍历。求得对应相似变换矩阵为 s-RcSOR3CRIcR (12) b)人工规则筛选后 6)位姿优化的传播。当前帧坐标系与世界坐 标系之间的变换在5)中已经确定并优化。通过一 级相连关系的传播，可以确定这些相连的关键帧坐 标系与世界坐标系之间的相似变换矩阵。 7)点云优化。利用6)中的相似变换矩阵S,将 原始点云的世界坐标系转换到当前闭环帧处的坐标 (C)RANSAC算法匹配 系，进行点云的拼接融合，实现点云的优化。 图4特征提取与配准实验结果 5实验结果及分析 Fig.4 The experimental results of feature extraction and matching 5.1实验平台 5.2.2回环检测约束 为了验证算法的有效性，并保证本文实验的一 通过回环检测，可以增加帧间的约束，进一步 致可对比性，故采用了知名的TUM标准数据集四 提升系统的鲁棒性，进而提升整个SLAM的轨迹与 并选用了基于Kinect视觉传感器采集的室内基准 地图的质量。在本文算法中，通过筛选关键帧之后 数据包。该基准数据包中包含Kinect产生的彩色 进行回环检测，并以PNP求解的结果作为对边的估 图像和深度图像序列，以及机器人的真实运动位 计，增加到图优化中，如图5所示。 姿。本文实验数据处理平台为一台ntel双核2.4GHz 主频的PC机，运行Ubuntu14.04操作系统。程序 均设定以30帧/秒的速度同时读取数据包中的RGB 数据和深度数据。 5.2室内环境下的三维视觉SLAM 5.2.1特征提取与特征点配准 对两帧图像进行ORB特征提取后，在利用特 征匹配算法(FLANN)获得相邻帧间对应关系的基 础上，利用RANSAC算法对点云进行匹配，匹配结 图5回环检测产生的约束 果如图4(c)所示。在图4(a)中，通过特征匹配算 Fig.5 Loop closure of constraint了一种基于局部闭环与全局闭环约束的点云优化算 法，同时完成对位姿与稠密化点云的优化。算法具 体步骤如下： 1) 封装构造数据帧 Frame，根据帧间的关联程 度，筛选出关键帧序列。 2) 执行帧间配准。完成相机位姿初始估计。 (R,t) 3) 在相机观测约束下，直接应用式 (6)，构建如 图 3(a) 所示的图优化。求得优化后的相机的位姿 变化 ，将原始点云进行坐标系变换，实现点云的 拼接融合。 4) 执行闭环检测。通过动态计算闭环帧筛选 阈值，得到匹配质量较高的闭环帧候选序列。 5) 闭环检测结束后，遍历闭环帧候选序列，在 当前帧与闭环候选帧之间，进行 Sim3(相似变换矩 阵) 求解与优化。当某一候选帧通过了 Sim3 求解 与优化，则认为闭环检测成功，跳出对闭环帧候选 序列的遍历。求得对应相似变换矩阵为 S = [ sR t 0 1 ] ,R ∈ SO(3),s ∈ R,t ∈ R 3 (12) 6) 位姿优化的传播。当前帧坐标系与世界坐 标系之间的变换在 5) 中已经确定并优化。通过一 级相连关系的传播，可以确定这些相连的关键帧坐 标系与世界坐标系之间的相似变换矩阵。 7) 点云优化。利用 6) 中的相似变换矩阵 S ，将 原始点云的世界坐标系转换到当前闭环帧处的坐标 系，进行点云的拼接融合，实现点云的优化。 5 实验结果及分析 5.1 实验平台 为了验证算法的有效性，并保证本文实验的一 致可对比性，故采用了知名的 TUM 标准数据集[12] ， 并选用了基于 Kinect 视觉传感器采集的室内基准 数据包。该基准数据包中包含 Kinect 产生的彩色 图像和深度图像序列，以及机器人的真实运动位 姿。本文实验数据处理平台为一台 Intel 双核 2.4 GHz 主频的 PC 机，运行 Ubuntu 14.04 操作系统。程序 均设定以 30 帧/秒的速度同时读取数据包中的 RGB 数据和深度数据。 5.2 室内环境下的三维视觉 SLAM 5.2.1 特征提取与特征点配准 对两帧图像进行 ORB 特征提取后，在利用特 征匹配算法 (FLANN) 获得相邻帧间对应关系的基 础上，利用 RANSAC 算法对点云进行匹配，匹配结 果如图 4(c) 所示。在图 4(a) 中，通过特征匹配算 法，一共产生了 929 个匹配特征点，但误匹配点过 多。因此，通过人工制定规则的筛选，去除了部分 误匹配点，此时的匹配个数为 250。为进一步提升 匹配精读，通过 RANSAC 算法对图 4(b) 进行进一 步的匹配筛选。最终，产生了 97 个经筛选后的匹配 点，实验中整个匹配所耗时间为 0.003 5 s。 (a) ➥ᒭࡥ䙹ッ∁ऺ (b) 人工规则筛选后 (c) RANSAC ッ∁ࡥ䙹 图 4 特征提取与配准实验结果 Fig. 4 The experimental results of feature extraction and matching 5.2.2 回环检测约束 通过回环检测，可以增加帧间的约束，进一步 提升系统的鲁棒性，进而提升整个 SLAM 的轨迹与 地图的质量。在本文算法中，通过筛选关键帧之后 进行回环检测，并以 PNP 求解的结果作为对边的估 计，增加到图优化中，如图 5 所示。 图 5 回环检测产生的约束 Fig. 5 Loop closure of constraint 第 2 期 张毅，等：基于图优化的移动机器人视觉 SLAM ·293·