CN110264563A - 一种基于orbslam2的八叉树建图方法 - Google Patents

Abstract

本发明公开了一种基于ORBSLAM2的八叉树建图方法，包括以下步骤：1)根据ORB‑SLAM2的RGBD相机数据视觉里程计，得到相机位姿信息和关键帧图像信息；2)构建点云地图；2.1)将关键帧图像信息转换为点云信息；2.2)根据相机位姿信息和点云信息进行位姿转换完成点云拼接，获得点云地图；2.3)对点云地图进行滤波，去除Kinect传感器量程之外的无效测量点以及距离均值过大的离群点云点，然后利用降采样对点云地图进行轻量化处理；3)将点云地图转换为八叉树地图。本发明方法对原始ORBSLAM2进行了扩充性研究，得到的八叉树地图相比于一般的稀疏地图应用性更强，存储代价低，更重要的是此方法对于ORBSLAM2在后续应用层上的研究具有重要的实际意义。

Description

一种基于ORBSLAM2的八叉树建图方法

技术领域

本发明涉及机器人视觉与图像处理领域，尤其涉及一种基于ORBSLAM2的八叉树建图方法。

背景技术

SLAM技术的发展推动了定位(Localization)、跟踪(Tracking)以及路径规划(Path Planning)技术的发展，进而对无人机、无人驾驶、机器人等热门研究领域产生重大影响。其中，ORB-SLAM是一种基于ORB特征的三维定位与地图构建算法(SLAM)。该算法由Raul Mur-Artal，J.M.M.Montiel和Juan D.Tardos于2015年发表在IEEE Transactions onRobotics。后来扩展到Stereo和RGB-D sensor上，即被称为ORBSLAM2，ORB-SLAM基于PTAM架构，增加了地图初始化和闭环检测的功能，优化了关键帧选取的方法，在处理速度、追踪效果和地图精度上效果都不错，属于视觉SLAM业界的优秀开源算法框架。

然而，虽然ORB-SLAM2可以基于特征点得到稀疏点云用来研究定位，但是该框架并没有提供构建地图部分的模块，而恰恰在具体应用中，地图的用途不仅仅用于辅助定位，还有一些应用层的需求。如：机器人导航、避障、交互，局部三维重建等，机器人需要知道地图中哪些地方不可通过，哪些地方可以通过，而这就需要建立一种稠密的地图。

发明内容

本发明要解决的技术问题在于针对现有技术中的缺陷，提供一种基于ORBSLAM2的八叉树建图方法。

本发明解决其技术问题所采用的技术方案是：一种基于ORBSLAM2的八叉树建图方法，包括以下步骤：

1)数据采集

根据ORB-SLAM2的RGBD相机数据视觉里程计，得到相机位姿信息和关键帧图像信息；

2)构建点云地图

2.1)将关键帧图像信息转换为点云信息；

2.2)根据相机位姿信息和点云信息进行位姿转换完成点云拼接，获得点云地图；

2.3)对点云地图进行滤波，去除Kinect传感器量程之外的无效测量点以及距离均值过大的离群点云点，然后利用降采样对点云地图进行轻量化处理；

3)将点云地图转换为八叉树地图。

按上述方案，所述步骤1)中，根据ORB-SLAM2的RGBD相机数据视觉里程计，得到相机位姿信息和关键帧图像信息，具体如下：

启动机器人移动平台，并开启控制PC，调试好主机PC，此时订阅Kinect在ROS下的话题，可以实时看到所处环境的图像信息，利用控制PC对机器人进行移动，注意Kinect传感器的特性，在完成一次移动中，旋转角度应控制在5°以内，平移距离控制在5至10cm；

然后在ROS下将对应时刻和位置的彩色深度图像对一一保存，并按时间次序命名；

按照ORB-SLAM2所需的数据格式，将上步采集的图相对进行相应处理，然后在主机PC上启动ORB-SLAM2，输入采集并处理后的数据运行；

最后根据ORB-SLAM2的输出，得到相机位姿信息和关键帧图像信息，其中关键帧图像是作为下一处理步骤的输入。

按上述方案，所述步骤2.1)将关键帧图像信息转换为点云信息，具体如下：

设一个空间点在图像中的像素坐标为[u,v,d]，对点云信息中的每一个点X_i，有r,g,b,x,y,z一共6个分量，分别表示该点的颜色与空间位置；

点云空间位置(x,y,z)与图像中的像素坐标采用下式进行转换；

z＝d/s

x＝(u-c_x)·z/f_x

y＝(v-c_y)·z/f_y

其中，f_x,f_y指相机在x,y两个轴上的焦距，C_x,C_y指在相机坐标系下相机的光圈中心，s指深度图的缩放因子，(u，v，d)为该点在关键帧图像中的像素坐标，其中，d为深度信息；

设相机位姿为[x,y,z,q_x,q_y,q_z,q_w]，其中x,y,z,q_x,q_y,q_z,q_w分别表示相机的三维空间位置(x,y,z)，以及相机的姿态四元数的四个量；

然后对点云信息进行位姿转换，获得最终的点云信息；

其中，

R_3×3是一个3×3的旋转矩阵，t_3×1是3×1的位移矢量。

按上述方案，所述步骤2.3)对点云地图进行滤波包括以下步骤：

2.3.1)在生成每帧点云时，去掉深度值d＝0和d>7000的测量点；

2.3.2)利用统计滤波器方法去除孤立点；

2.3.3)利用体素滤波器(Voxel Filter)进行下采样。

按上述方案，所述步骤3)将点云地图转换为八叉树地图，采用以下方法：

3.1)利用PCL的Octomap包，将上步中得到的点云转换成.bt类型的八叉树地图，此时的地图是不带颜色的，需要进行下一步的处理；

3.2)根据Octomap提供的ColorOcTree函数，对转换后的点云加入色彩信息；

3.3)利用Octomap库，将步骤3.2)中的点云地图转换为.ot类型的八叉树地图。

本发明产生的有益效果是：在ORBSLAM2框架上提出了一种针对RGBD相机数据的八叉树地图构建的方法，该方法理论可靠，实践可行。由于ORB-SLAM2得到的是稀疏特征点地图，只能用于研究定位并且对于周围环境的描述不够，更重要的是，其对于移动机器人导航作用不大。基于此，本方法构建的稠密地图，提供了地图本身的可视化需求，让我们能够大致了解环境的样子。地图针对一般点云图数据冗余的缺点进行了滤波优化和降采样，以三维八叉树的方式存储，能够极大降低地图的存储规模，同时能使得我们能够快速浏览场景的各个角落；并且弥补了ORB-SLAM2在研究地图应用上的不足，能较好的符合当今快速发展的移动机器人行业对于SLAM的整体性要求。整体来讲，本地图构建方法对原始ORBSLAM2进行了扩充性研究，得到的八叉树地图相比于一般的稀疏地图应用性更强，存储代价低，更重要的是此方法对于ORBSLAM2在后续应用层上的研究具有重要的实际意义。

附图说明

下面将结合附图及实施例对本发明作进一步说明，附图中：

图1是本发明实施例的方法流程图；

图2是本发明实施例的软硬件实验平台示意图；

图3是本发明实施例中ORBSLAM2的算法框架图；

图4是本发明实施例中JS-R型机器人平台以及KinectV1模型图；

图5是本发明实施例采集到的数据帧和工作空间文件示意图；

图6是本发明实施例的ORBSLAM2运行示意图；

图7是本发明实施例的稠密点云建图示意图；

图8是本发明实施例的八叉树建图示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。

如图1所示，本发明提供一种基于ORBSLAM2的八叉树建图方法，包括如下步骤：

步骤S1：基于ORB-SLAM2的视觉里程计，并输入数据帧，运行得到相机位姿和关键帧位姿信息：

步骤S11：数据的采集与预处理：启动JS-R机器人的上位机，通过ROS来控制移动平台，移动机器人平台，这里由于深度相机的初始化需求，本实施例的平行移动间距控制在5-10cm，并且旋转角度尽可能不要太大；将Kinect传感器接收的图像信息(彩色图、深度图)利用ROS保存成一帧一帧的图片，彩色图与深度图进行相应匹配，如图2。为了方便并且有序，统一将彩色图、深度图按顺序命名为“00001.png”，“00002.png”，...“00xxx.png”，并分别放在“color”和“depth”两个文件夹中。注意，此环节应格外注意数据帧之间时间和空间的距离不应太大，否则在ORBSLAM2运行开始的初始化线程时容易丢帧，造成相机位姿计算失败。

步骤S12：生成彩色深度图索引txt文件：在上一步的基础上，需要生成一个ORBSLAM2官方规定的txt文本，用来表示图像数据集内容和关系的。参照例子所示的格式“0rgb/0.png 0depth/0.png,1rgb/1.png 1depth/1.png,...”，对其进行编辑和保存。

步骤S13：制作相机参数文件：Kinect v1深度相机拥有一个RGB彩色摄像头，一个红外线CMOS摄像机和一个红外发射器，如图4。相机的红外线CMOS摄像机和红外发射器以左右水平的方式分布。其彩色图以及深度图在ROS中的话题以及数据格式为：(1)RGB图像：/camera/rgb/image_color，ROS数据格式：sensor_msgs/Image，OPENCV数据格式：Mat，图像尺寸：640*480，像素数据类型：8UC3。(2)深度图像：/camera/depth/image，ROS数据格式：sensor_msgs/Image，OPENCV数据格式：Mat，图像尺寸：640*480，像素数据类型：32FC。

在ROS平台上使用camera_calibration包进行相机的标定，该功能包基于张正友标定法，通过角点检测匹配实际物理坐标与图像坐标实现标定，该功能包会自动生成.yaml相机参数文件。按照ORBSLAM2例子中的参数文件(如./Examples/TUM1.yaml)，制作一个新的相机参数文件，如“jsr.yaml”。

步骤S14：基于ORB-SLAM2的RGBD相机数据输入，如图3，按照ORBSLAM2官方规定的数据输入格式，将上面步骤中得到的数据帧以及相机参数文件进行输入。图5，是本发明实施例采集到的数据帧和工作空间文件示意图。接着，启动ORBSLAM2，开始进入三个线程：跟踪线程，从图像中提取ORB特征，根据上一帧进行姿态估计，通过全局重定位初始化位姿，然后跟踪已经重建的局部地图，优化位姿，再根预设条件及规则确定新的关键帧。局部地图线程，这一部分包括对关键帧的插入，验证最近生成的地图点并进行筛选，然后生成新的地图点，使用局部捆集调整(Local BA)，最后再对插入的关键帧进行筛选，去除多余的关键帧。回环检测线程，这一部分主要分为两个过程，分别是闭环探测和闭环校正。闭环检测先使用DBoW进行探测，然后通过Sim3算法计算相似变换。闭环校正，主要是闭环融合和EssentialGraph的图优化，最终得到相机运行轨迹，CameraTrajectory.txt；如图6，是本实施例的ORBSLAM2的运行界面。

步骤S2：点云地图构建与点云滤波优化：

步骤S21：计算机中图像的表示：

在一张灰度图中，每个像素位置(x,y)对应到一个灰度值I，所以一张宽度为w，高度为h的图像，数学形式可以记成一个矩阵，如式(7)：

I(x,y)∈R^w×h (7)

为了让计算机能表达整个实数空间，所以要在某个范围内，对图像进行量化。我们用0-255之间整数(即一个unsigned char，一个字节)来表达图像的灰度大小。那么，一张宽度为640，高度为480分辨率的灰图度就可以这样表示：

unsigned char image[480][640] (8)

式(8)中，第一个下标则是指数组的行，而第二个下标是列。

传统像素坐标系的定义方式，一个像素坐标系原点位于图像的左上角，X轴向右，Y轴向下(也就是前面所说的坐标(u，v，d)中u和v分别对应的坐标轴)。如果它还有第三个轴的话，根据右手法则，Z轴是向前的。这种定义方式是与相机坐标系一致的。我们平时说的图像的宽度和列数，对应着X轴，而图像的行数或高度，则对应着它的Y轴。

一个灰度像素可以用八位整数记录，也就是一个0-255之间的值。当我们要记录的信息更多时，一个字节不够。在RGB-D相机的深度图中，记录了各个像素离相机的距离。这个距离通常以毫米为单位，RGB-D相机的量程通常在十几米范围左右，超过了255的最大值范围。这时需要采用十六位整数(C++中的unsigned short)来记录一个深度图的信息，也就是位于0至65536之间的值。换算成毫米的话，最大可以表示65米，满足一个RGB-D相机使用了。

步骤S22：点云拼接：

首先，程序提供了10张RGB-D图像，并且知道了每个图像的内参和外参。根据RGB-D图像和相机内参，我们可以计算任何一个像素在相机坐标系下的位置。同时，根据相机位姿，计算这些像素在世界坐标系下的位置。把所有像素的空间坐标求出来，然后将上面准备好的10对图像(一一对应好了的彩色图、深度图)，位于slam/densemap/corner1201文件中。在color/下有0.png到9.png10张彩色图，在depth/下有10张对应的深度图。同时，CameraTrajectory.txt文件给出了10张图像的相机位姿(以Twc形式)。例如第一对图的外参为：[0:228993；0:00645704；0:0287837；0:0004327；0:113131；0:0326832；0:993042]；

下面完成两件事：(1)根据内参计算一对RGB-D图像对应的点云；(2)根据各张图的相机位姿(也就是外参)，把点云加起来，组成地图，即完成了点云的拼接。

本实施例的点云库使用PCL(Point Cloud Library)。安装完成后，PCL的头文件将安装在/usr/include/pcl-1.7中，库文件位于/usr/lib/中。将点云拼接程序进行编译，同样的，在编译之前确保Eigen库、Opencv库安装完好。最后，运行可执行程序，生成的点云保存为shiyanshi1201.pcd文件。

步骤S23：点云滤波优化：

上面一步中我们初步得到了稠密的点云图，而在实际建图当中，我们还会对点云加一些滤波处理，获得更好的视觉效果。在实际操作中，主要使用两种滤波器：外点去除滤波器以及降采样滤波器。主要改变的部分如下：

(1)在生成每帧点云时，去掉深度值d太大或无效的点。这主要是考虑到Kinect的有效量程，超过量程之后的深度值会有较大误差。针对本发明，经试验后，d取5000-8000效果较好。

(2)利用统计滤波器方法去除孤立点。该滤波器统计每个点与它最近N个点的距离值的分布，去除距离均值过大的点。这样，我们保留了那些“粘在一起”的点，去掉孤立的噪声点。针对本发明，经试验后，N取40-80效果较好。

(3)最后，利用体素滤波器(Voxel Filter)进行降采样。由于多个视角存在视野重叠，在重叠区域会存在大量的位置十分相近的点。这会无益地占用许多内存空间。体素滤波保证在某个一定大小的立方体(或称体素)内仅有一个点，相当于对三维空间进行了降采样，从而节省了很多存储空间。针对本发明，经试验后，S取0.001-0.1效果较好。

运行可执行程序，生成的点云保存为shiyanshi120101.pcd文件。针对实验室一角的点云建图效果如图7。

步骤S3：面向应用层的八叉树地图转换。

步骤S31：安Octo-map：

具体安装及编译，请参考网上官方教程，编译如果没有给出任何警告，那么就是编译成功，进入下面一步。

步骤32：转换pcd到Octomap：

在编译之后，它会产生一个可执行文件，叫做pcd2octomap，放在代码根目录的bin/文件夹下。在代码根目录下这样调用：

bin/pcd2octomap data/xxx.pcd data/xxx.bt

其源代码为：src/pcd2octomap.cpp，这份代码将命令行参数1作为输入文件，参数2作为输出文件，把输入的pcd格式点云转换成octomap格式的点云。

步骤33：加入色彩信息：

上步中，我们将pcd点云转换为Octo-map。但是pcd点云是有颜色信息的，Octomap提供了ColorOcTree类，能够帮你存储颜色信息。大部分代码和上步是一样的，除了把OcTree改成ColorOcTree，以及调用integrateNodeColor来混合颜色之外。代码修改后会编译出pcd2colorOctomap这个程序，完成带颜色的转换。同时，后缀名改成了.ot文件。调用格式：

bin/pcd2colorOctomap data/xxx.pcd data/xxx.ot

此部分，针对上面一步得到的功能还比较初级的点云地图，利用PCL包里的Octo-map库，将其转换为更具有应用性以及存储代价更小(本实施例点云图大小8.9MB，而八叉树地图105Kb左右，近乎90倍的压缩率)的八叉树地图，针对实验室一角的八叉树建图效果如图8。

本发明公开了一种基于ORB-SLAM2的八叉树建图方法，设计机器人视觉与图像处理领域。该方法主要包括三个部分：基于ORB-SLAM2的RGBD相机数据视觉里程计、点云地图构建与点云滤波优化部分、面向应用层的八叉树地图转换部分。第一部分又包括数据采集与预处理、启动ORB-SLAM2的三个运行线程，从而得到基于ORB-SLAM2的优化后相机位姿；第二部分针对上面一步得到的相机位姿信息以及深度相机模型及数学表达式构建点云，在生成每帧点云时，去掉深度值太大或者无效的点，再利用统计滤波器去除孤立的点，最后利用体素滤波器进行降采样，节省存储空间。第三部分，针对上面一步得到的比较初级的点云地图，利用PCL包里的Octo-map库，将其转换为可以拿来应用以及存储代价更小的八叉树地图。本发明提供基于ORB-SLAM2的八叉树建图方法，是在原有ORBSLAM2框架上进行自行主导和研究开发出的一种针对RGBD相机数据的稠密地图构建的方法，并且没有涉及三维视觉对于计算机或者硬件平台的GPU硬性要求，该方法理论充分，实践可行，构建的八叉树地图相比于一般的稀疏地图应用性更强，存储代价低得多，更重要的是此方法对于ORBSLAM2乃至视觉SLAM领域应用层的研究具有重要意义。

应当理解的是，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，而所有这些改进和变换都应属于本发明所附权利要求的保护范围。

Claims (5)

1.一种基于ORBSLAM2的八叉树建图方法，其特征在于，包括以下步骤：

1)数据采集

根据ORB-SLAM2的RGBD相机数据视觉里程计，得到相机位姿信息和关键帧图像信息；

2)构建点云地图

2.1)将关键帧图像信息转换为点云信息；

2.2)根据相机位姿信息和点云信息进行位姿转换完成点云拼接，获得点云地图；

2.3)对点云地图进行滤波，去除Kinect传感器量程之外的无效测量点以及距离均值过大的离群点云点，然后利用降采样对点云地图进行轻量化处理；

3)将点云地图转换为八叉树地图。

2.根据权利要求1所述的基于ORBSLAM2的八叉树建图方法，其特征在于，所述步骤1)中，根据ORB-SLAM2的RGBD相机数据视觉里程计，得到相机位姿信息和关键帧图像信息，具体如下：

启动机器人移动平台，实时观察所处环境的图像信息，控制机器人进行移动，在完成一次控制移动过程中，旋转角度应控制在5°以内，平移距离控制在5至10cm；

然后将采集的对应时刻和位置的彩色图像和深度图像组成的图像对一一保存，并按时间次序命名；

按照ORB-SLAM2所需的数据格式，将上步采集的图像对进行相应处理，然后启动ORB-SLAM2，输入采集并处理后的数据运行；

最后根据ORB-SLAM2的输出，得到相机位姿信息和关键帧图像信息，其中关键帧图像是作为下一处理步骤的输入。

3.根据权利要求1所述的基于ORBSLAM2的八叉树建图方法，其特征在于，所述步骤2.1)将关键帧图像信息转换为点云信息，具体如下：

设一个空间点在图像中的像素坐标为[u,v,d]，对点云信息中的每一个点X_i，有r,g,b,x,y,z一共6个分量，分别表示该点的颜色与空间位置；

点云空间位置(x,y,z)与图像中的像素坐标采用下式进行转换；

z＝d/s

x＝(u-c_x)·z/f_x

y＝(v-c_y)·z/f_y

其中，f_x,f_y指相机在x,y两个轴上的焦距，C_x,C_y指在相机坐标系下相机的光圈中心，s指深度图的缩放因子，(u，v，d)为该点在关键帧图像中的像素坐标，其中，d为深度信息；

设相机位姿为[x，y，z，q_x，q_y，q_z，q_r]，其中，x,y,z,q_x,q_y,q_z,q_w分别表示相机的三维空间位置(x，y，z)，以及相机的姿态四元数的四个量；

然后对点云信息进行位姿转换，获得最终的点云信息；

其中，

R_3×3是一个3×3的旋转矩阵，t_3×1是3×1的位移矢量。

4.根据权利要求1所述的基于ORBSLAM2的八叉树建图方法，其特征在于，所述步骤2.3)对点云地图进行滤波，包括以下步骤：

2.3.1)在生成每帧点云时，去掉深度值d＝0和d>7000的测量点；

2.3.2)利用统计滤波器方法去除孤立点；

2.3.3)利用体素滤波器进行下采样。

5.根据权利要求1所述的基于ORBSLAM2的八叉树建图方法，其特征在于，所述步骤3)将点云地图转换为八叉树地图，采用以下方法：

3.1)利用PCL的Octomap包，将上步中得到的点云转换成.bt类型的八叉树地图，此时的地图是不带颜色的，需要进行下一步的处理；

3.2)根据Octomap提供的ColorOcTree函数，对转换后的点云加入色彩信息；

3.3)利用Octomap库，将步骤3.2)中的点云地图转换为.ot类型的八叉树地图。