本发明涉及机器人视觉和图像处理，特别是基于ORBSLAM八叉树建图方法。
背景技术：
SLAM技术的发展促进了定位(Localization)、跟踪(Tracking)路径规划(Path Planning)技术的发展对无人机、无人驾驶、机器人等热门研究领域产生了重大影响。其中，ORB-SLAM是一种基于ORB三维定位和地图构建算法的特征(SLAM)。该算法由Raul Mur-Artal，J.M.M.Montiel和Juan D.Tardos于2015年发表在IEEE Transactions on Robotics。后来扩展到Stereo和RGB-D sensor上，叫ORBSLAM2，ORB-SLAM基于PTAM架构增加了地图初始化和闭环检测的功能，优化了关键帧选择的方法，具有良好的处理速度、跟踪效果和地图精度SLAM行业优秀的开源算法框架。
然而，虽然ORB-SLAM稀疏点云可以根据特征点进行研究和定位，但框架不提供构建地图部分的模块，而是在具体应用中，地图不仅用于辅助定位，还需要一些应用层。例如：机器人导航、避障、交互、局部三维重建等，机器人需要知道地图中不能通过的地方，这需要建立一密集的地图。
技术实现要素：
本发明要解决的技术问题是为现有技术的缺陷提供基础ORBSLAM八叉树建图方法。
解决本发明技术问题所采用的技术解决方案是：基于ORBSLAM八叉树建图方法包括以下步骤：
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-10之间cm；
然后在ROS对应时间和位置的彩色深度图像逐一保存，并按时间顺序命名；
按照ORB-SLAM2所需的数据格式，相对处理上一步采集的图，然后在主机上PC上启动ORB-SLAM2.输入采集处理后的数据运行；
最后根据ORB-SLAM输出2，获取相机位置信息和关键帧图像信息，其中关键帧图像作为下一步的输入。
根据上述方案，步骤2.1)将关键帧图像信息转换为点云信息，具体如下：
在图像中设置空间点的像素坐标[u,v,d]，对点云信息中的每个点Xi，有r,g,b,x,y,z共6个重量，分别表示点的颜色和空间位置；
点云空间位置(x,y,z)以下形式转换图像中的像素坐标；
z＝d/s
x＝(u-cx)·z/fx
y＝(v-cy)·z/fy
其中，fx,fy指相机在x,y两轴上的焦距，Cx,Cy指相机坐标系下的光圈中心，s指深度图的缩放因子，(u，v，d)其中，d深度信息；
设置相机位置为[x,y,z,qx,qy,qz,qw]，其中x,y,z,qx,qy,qz,qw分别表示相机的三维空间位置(x,y,z)，以及四元数的相机姿势；
然后对点云信息进行位置转换，获取最终点云信息；
其中，
R3×3是一个3×旋转矩阵3，t3×1是3×位移矢量1。
根据上述方案，步骤2.3)滤波对点云地图包括以下步骤:
2.3.1)在生成每帧点云时，去掉深度值d＝0和d>7000的测量点；
2.3.2)使用统计滤波器去除孤立点；
2.3.3)使用体素滤波器(Voxel Filter)下采样。
采用以下方法将点云地图转换为八叉树地图：
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是本发明实施例ORBSLAM算法框架图2；
图4是本发明实施例JS-R型机器人平台和KinectV1模型图；
图5是本发明实施例收集的数据帧和工作空间文件示意图；
图6是本发明实施例ORBSLAM2运行示意图；
图7是本发明实施例的稠密点云建图示意图；
图8是本发明实施例的八叉树建图示意图。
具体实施方法
为了更清楚地了解本发明的目的、技术方案和优点，以下结合实施例进一步详细说明了本发明。应该理解，这里描述的具体实施例仅用于解释本发明，而不限制本发明。
如图1所示，本发明基于ORBSLAM八叉树建图方法包括以下步骤：
步骤S1：基于ORB-SLAM2视觉里程计，输入数据帧，运行相机位置和关键帧位置信息：
步骤S11:数据采集和预处理:启动JS-R机器人的上位机通过ROS由于深度相机的初始化需求，本实施例的平行移动间距控制在5-10cm，旋转角度尽量不要太大；Kinect传感器接收的图像信息(彩色图、深度图)利用ROS保存成一帧一帧的图片，颜色图与深度图相匹配，如图2所示。为方便有序，彩色图和深度图按顺序统一命名为0001.png”，“00002.png..“00xxx.png，分别放在color”和“depth两个文件夹。请注意，数据帧之间的时间和空间之间的距离不应过大，否则在此链接中ORBSLAM运行初始化线程时容易丢帧，导致相机位置计算失败。
步骤S12.生成彩色深度图索引txt文件：在上一步的基础上，需要生成一个ORBSLAM2官方规定的txt文本用于表示图像数据集的内容和关系。参照例子所示的格式0rgb/0.png 0depth/0.png,1rgb/1.png 1depth/1.png,.编辑和保存它。
步骤S13:制作相机参数文件:Kinect v有一个深度相机RGB彩色摄像头，红外线CMOS相机和红外发射器，如图4所示。红外相机CMOS相机和红外发射器以左右水平分布。其颜色图和深度图ROS主题和数据格式为:(1)RGB图像：/camera/rgb/image_color，ROS数据格式：sensor_msgs/Image，OPENCV数据格式：Mat，图像尺寸：640*像素数据类型为480UC3。(2)深度图像:/camera/depth/image，ROS数据格式：sensor_msgs/Image，OPENCV数据格式：Mat，图像尺寸：640*像素数据类型480：32FC。
在ROS平台上使用camera_calibration该功能包基于张正友的校准方法，通过角度检测匹配实际物理坐标和图像坐标进行校准。该功能包将自动生成.yaml相机参数文件。按照ORBSLAM例2中的参数文件(如./Examples/TUM1.yaml)，制作新的相机参数文件，如jsr.yaml”。
步骤S14：基于ORB-SLAM2的RGBD如图3所示，相机数据输入ORBSLAM2官方数据输入格式，输入上述步骤中获得的数据帧和相机参数文件。图5是本发明实施例收集的数据帧和工作空间文件示意图。接着，启动ORBSLAM2.开始进入三个线程:跟踪线程，从图像中提取ORB根据上一帧的姿势估计，通过全球重置初始位置，跟踪重建的局部地图，优化位置，然后根据预设条件和规则确定新的关键帧。局部地图线程，包括插入关键帧，验证最新生成的地图点并筛选，然后生成新的地图点，使用局部捆绑调整(Local BA)，最后再对插入的关键帧进行筛选，去除多余的关键帧。回环检测线程主要分为闭环检测和闭环校正两个过程。首先使用闭环检测DBoW探测，然后通过Sim3算计算相似变换。闭环校正，主要是闭环融合和Essential Graph的图优化，最终得到相机运行轨迹，CameraTrajectory.txt；如图6，是本实施例的ORBSLAM2的运行界面。
步骤S2：点云地图构建与点云滤波优化：
步骤S21：计算机中图像的表示：
在一张灰度图中，每个像素位置(x,y)对应到一个灰度值I，所以一张宽度为w，高度为h的图像，数学形式可以记成一个矩阵，如式(7)：
I(x,y)∈Rw×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领域应用层的研究具有重要意义。
应当理解的是，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，而所有这些改进和变换都应属于本发明所附权利要求的保护范围。