一种手持SLAM装置，数据采集、存储及发布方法

一种手持slam装置，数据采集、存储及发布方法
技术领域
1.本发明涉及slam的多传感器融合的领域，具体为一种手持slam装置，数据采集、存储及发布方法。


背景技术：

2.随着slam(simultaneous localization and mapping，同步定位与建图技术)技术的发展，单传感器定位技术已经无法满足复杂多变的实时环境的需求，多传感器融合定位技术，如lidar传感器和相机传感器以及imu(inertial measurement unit，惯性测量单元)融合的lvi-sam算法、gnss(global navigation satellite system,全球导航卫星系统)与ins(inertial navigation system，惯性导航系统)形成的gnss/ins组合定位技术、lidar传感器和imu融合的floam算法等，它们相比于单传感器定位技术，都取得了很好的定位与建图效果。在当下slam的发展中，多传感器融合定位方法已成为主流方法。对于多传感器融合定位技术，数据的采集、存储与发布是很有必要的。通过采集多传感器数据并存储可用作传感器的标定，可以直观地查看传感器数据、通过发布采集的多传感器数据可用作测试算法等。
3.在手持slam装置方面，国内外已出现相关的集成性产品。如华测导航提供的geoslam手持式slam装置、hexagon品牌下的leica测量系统的blk20go手持机集成了全景相机与激光雷达，但所述装置大多用于室内；其背包装置leicapegasus:backpack集成了4个全景相机、两个16线的velodyne激光雷达、gnss和imu。上述的装置用户使用率低，价格昂贵，体型大，不易携带；且所述传感器只实现物理层面的集成，并未实现数据层面的融合。在slam领域的传感器数据采集、存储与发布方面，目前均是采用bag文件存储以及数据发布，所述方法存储的文件不能很好的查看原始数据状态，所述方法在数据发布时不方便进行单帧调整，不易转载它用。在一些开源的成熟slam算法中，一般只有数据读取程序，往往只能读取公开数据集的传感器数据以测试算法，没有数据采集方法，不能实现数据采集、存储与发布的功能。用法不灵活，用途比较单一。无法满足我们采集自身想要环境数据，在复杂多变环境下测试算法有效性的需求。本发明提出的一种手持slam装置，数据采集、存储及发布方法，将lidar传感器、相机传感器和span-igm s1组合导航设备组合形成可手持式slam装置，装置精巧，方便用户采集数据；提出的数据采集、存储及发布方法具有实时采集、传感器消息实时可视化；存储方便有序、存储数据易查看，易转载它用；发布灵活等特点。本发明总体具有高效快速，兼容性强，作业成本低，适用范围广等特点，在多传感器数据的实时采集与存储、发布、方便它用方面具有明显优势。
4.在ros(robot operating system，机器人操作系统)领域，最小的进程(process)单元就是节点(node)。一个软件包中可以包含多个可执行文件，所述可执行文件在程序运行之后就会形成进程，所述进程在ros中被称为节点。可以将节点设置成发布者(publisher)，发布具有某种消息类型的话题，也可以将节点设置成订阅者(subscriber)，订阅具有某种消息类型的话题。节点与节点之间通过话题进行通讯。本发明就是通过节点
间发布订阅带有传感器消息类型的话题的原理，实现多传感器数据的采集、存储与发布。
5.本发明提出了一种手持slam装置，数据采集、存储及发布方法。其中，一种手持slam装置，包括小觅双目相机(mynt eye s1030)、16线激光雷达(velodyne vlp-16)、span-igm s1组合导航设备、gnss柱状天线、4g通讯模块、处理器(nvidia agx xavier)、平板显示器等。所述gnss柱状天线、4g通讯模块与所述span-igm s1组合导航设备相连，所述的小觅双目相机、16线激光雷达、span-igm s1组合导航设备、平板显示器均与处理器相连，可实现装置的小型化。对于数据采集而言，首先通过在ros下安装vlp驱动、安装mynt eye sdk ros驱动、配置串口工具minocom，通过ros方式同时启动上述各传感器驱动，并利用实现上述各传感器的软同步；在处理器创建节点订阅上述传感器的传感器消息以采集数据；提出一种通过时间同步器的方式实现采集数据的时间戳对齐方法；对于数据存储而言，提出一种数据存储与可视化多线程方法，存储数据同时显示所述16线激光雷达点云数据与所述小觅双目相机图像信息；将采集的数据存放在自定义路径的data文件夹下，所述data文件夹路径下包括gnss文件夹、leftimg文件夹、lidar文件夹、rightimg文件夹，分别存放采集的gnss数据和imu数据、所述双目相机数据以及所述激光雷达数据，所述双目相机数据以及所述激光雷达数据以实时时间戳命名；存储方便有序、存储数据易查看，易转载它用；对于数据发布而言，提出一种基于时间戳排序的多传感器数据发布方法，按时间顺序与指定频率发布所述各传感器类型的消息，实现多传感器数据发布。


技术实现要素：

6.为解决上述技术问题，本发明提出了一种手持slam装置，数据采集、存储及发布方法，提出一种通过时间同步器的方式实现采集数据的时间戳对齐方法，获得时间戳对齐之后的传感器数据，方便slam领域多传感器融合的发展；在数据存储阶段，提出一种数据存储与可视化多线程方法，实时存储、传感器消息实时可视化；在数据发布阶段，提出一种基于时间戳排序的多传感器数据发布方法，按时间顺序与指定频率发布所述各传感器类型的消息。
7.本发明提供一种手持slam装置，包括小觅双目相机、16线激光雷达、span-igm s1组合导航设备、gnss柱状天线、4g通讯模块、处理器和平板显示器：所述gnss柱状天线、4g通讯模块与所述span-igm s1组合导航设备相连，所述的小觅双目相机、16线激光雷达、span-igm s1组合导航设备、平板显示器均与处理器相连。
8.本发明提供一种手持slam装置的数据采集、存储及发布方法，包括以下步骤：
9.s1.连接上述传感器、处理器与平板显示器，ros下安装上述传感器驱动；
10.具体如下；
11.s11.连接上述传感器、处理器与平板显示器，所述gnss柱状天线、4g通讯模块与所述span-igm s1组合导航设备相连，所述的小觅双目相机、16线激光雷达、span-igm s1组合导航设备、平板显示器均与处理器相连，所述4g通讯模块实现接收gnss基准站差分数据或自己搭建的gnss基准站差分数据流的功能；
12.s12.ros下安装vlp驱动、安装mynt eye sdk ros驱动、配置串口工具minocom，分别实现所述处理器与所述16线激光雷达、所述小觅双目相机、所述span-igm s1组合导航设备的gnss rtk和imu数据传送功能；
13.s2.采集双目相机图像数据、激光点云数据、gnss数据、imu数据；
14.具体如下；
15.s21.ros下启动上述传感器驱动；
16.s22.创建节点订阅带有所述传感器消息的话题并发布；
17.所述的步骤s22中,包括以下步骤：创建时间同步器、创建回调函数、创建指针；
18.s3.将s2中采集的数据进行存储；
19.具体如下；
20.s31.数据存储线程；
21.s32.数据可视化线程；
22.s4.将s3中存储的数据发布；
23.具体如下；
24.s41.将存储的所述传感器数据的时间戳排序；
25.s42.按照时间顺序发布各传感器数据。
26.3.根据权利要求1所述的一种手持slam装置，数据采集、存储及发布方法，其特征在于：
27.所述gnss基准站差分数据包括千寻cors基准站差分数据流。
28.4.根据权利要求1所述的一种手持slam装置，数据采集、存储及发布方法，其特征在于：所述的步骤s12中,包括以下具体步骤：对于ros下安装vlp驱动，首先配置所述处理器的有线连接的ip地址，其次安装所述ros版本的velodyne驱动,并进行编译、连接好处理器与所述16线激光雷达并供电、打开网页，网址处输入192.168.1.201，若看到所述16线激光雷达的参数界面即实现所述16线激光雷达与所述处理器的配置；用户可根据需要，在所述参数界面设置所述16线激光雷达发布点云消息的频率；对于ros下安装mynt eye sdk ros驱动，从myntai官网下载mynt-eye-s-sdk包，ros下进行编译即可，用户可根据需要，在所述mynt-eye-s-sdk包的配置文件中设置所述小觅双目相机发布图像消息的频率；对于配置串口工具minicom,首先配置4g通讯模块实现千寻cors基准站差分数据流的功能，其次在ros下安装串口工具minicom,配置串口设备名称与波特率，实现所述处理器与所述span-igm s1组合导航设备的配置。
29.5.根据权利要求1所述的一种手持slam装置，数据采集、存储及发布方法，其特征在于：
30.所述的步骤s31中,包括以下具体步骤：创建节点订阅话题、创建回调函数、创建队列容器存储所述传感器消息并保存至指定路径的文件夹下。
31.6.根据权利要求1所述的一种手持slam装置，数据采集、存储及发布方法，其特征在于：
32.所述的步骤s32中,包括以下具体步骤：将采集的所述小觅双目相机左相机的图像消息变换到opencv(open source computer vision library，计算机视觉库)数据类型下，将采集的所述16线激光雷达的激光数据变换到pcl(point cloud library，点云库)点云数据类型下，根据所述16线激光雷达与所述小觅双目相机的外参数，将所述点云数据投影到采集的所述小觅双目相机左相机的图像上，显示所述左相机的图像数据的同时显示所述点云数据，实现可视化的功能。
33.7.根据权利要求1所述的一种手持slam装置，数据采集、存储及发布方法，其特征在于：
34.所述的步骤s41中,包括以下步骤：创建容器存储所述传感器消息的时间戳、排序语句对所述容器的时间戳排序。
35.有益效果：
36.针对所述bag文件存储以及数据发布方法的存储的文件不能很好的查看原始数据状态、数据发布时不方便进行单帧调整、不易转载它用的问题，本发明提出了一种手持slam装置，数据采集、存储及发布方法。其中，一种手持slam装置，包括小觅双目相机(mynt eye s1030)、16线激光雷达(velodyne vlp-16)、span-igm s1组合导航设备、gnss柱状天线、4g通讯模块、处理器(nvidia agx xavier)、平板显示器等，可实现装置的小型化。对于数据采集而言，首先通过在ros下安装vlp驱动、安装mynt eye sdk ros驱动、配置串口工具minocom，安装的驱动放在同一文件夹下，通过ros方式同时启动上述各传感器，实现上述各传感器的软同步；在处理器创建节点订阅上述传感器的传感器消息以采集数据；提出一种通过时间同步器的方式实现采集数据的时间戳对齐方法；对于数据存储而言，提出一种数据存储与可视化多线程方法，存储数据同时显示所述16线激光雷达点云数据与所述小觅双目相机图像信息；将采集的数据存放在自定义路径的data文件夹下，所述data文件夹路径下包括gnss文件夹、leftimg文件夹、lidar文件夹、rightimg文件夹，分别存放采集的gnss数据和imu数据、所述双目相机数据以及所述激光雷达数据，所述双目相机数据以及所述激光雷达数据以实时时间戳命名；存储方便有序、存储数据易查看，易转载它用；对于数据发布而言，提出一种基于时间戳排序的多传感器数据发布方法，按时间顺序与指定频率发布所述各传感器类型的消息，实现多传感器数据发布。
附图说明
37.图1是本发明手持slam装置实施例结构示意图；
38.图2是本发明手持slam装置设计图；
39.图3是本发明数据采集、存储与发布的方法流程图；
40.图4是本发明可视化实施例示意图。
41.标记说明：
42.1、gnss柱状天线；2、16线激光雷达；3、小觅双目相机；4、4g通讯模块；5、span-igm s1组合导航设备；6、平板显示器；7、处理器。
具体实施方式
43.下面结合附图与具体实施方式对本发明作进一步详细描述：
44.如图1所示，本发明以一种手持slam装置，数据采集、存储及发布方法。其中，一种手持slam装置，包括小觅双目相机3、16线激光雷达2、span-igm s1组合导航设备5、gnss柱状天线1、4g通讯模块4、处理器7、平板显示器6等。所述gnss柱状天线、4g通讯模块与所述span-igm s1组合导航设备相连，所述的小觅双目相机、16线激光雷达、span-igm s1组合导航设备、平板显示器均与处理器相连。图2是本发明手持slam装置设计图。
45.本发明中，在行进过程中，载体通过上述感器获取各种类型的消息，所述的16线激
光雷达获取周围环境的点云数据，所述的小觅双目相机获取周围环境的图像信息，所述的span-igm s1组合导航设备获取gnss数据和imu数据信息等。通过安装对应传感器产品的ros驱动并进行相应配置，通过ros方式同时启动上述各传感器驱动，实现上述各传感器的软同步；在处理器创建节点订阅上述传感器的传感器消息以采集数据；通过时间同步器的方式对齐采集数据的时间戳，获得时间戳对齐之后的传感器数据；通过数据存储与可视化多线程方法，存储数据同时显示所述16线激光雷达点云数据与所述小觅双目相机图像信息；将采集的数据存放在自定义路径的data文件夹下，所述data文件夹路径下包括gnss文件夹、leftimg文件夹、lidar文件夹、rightimg文件夹，分别存放采集的gnss数据和imu数据、所述双目相机数据以及所述激光雷达数据，所述双目相机数据以及所述激光雷达数据以实时时间戳命名；用户需要发布数据时，通过一种基于时间戳排序的多传感器数据发布方法，按时间顺序与指定频率发布所述各传感器类型的消息，实现多传感器数据发布。
46.(一)连接上述传感器、处理器与平板显示器，ros下安装上述传感器驱动
47.1.连接上述传感器、处理器与平板显示器。所述gnss柱状天线、4g通讯模块与所述span-igm s1组合导航设备相连，所述的小觅双目相机、16线激光雷达、span-igm s1组合导航设备、平板显示器均与处理器相连；
48.2.ros下安装vlp驱动、安装mynt eye sdk ros驱动、配置串口工具minocom，分别实现所述处理器与所述16线激光雷达、所述小觅双目相机、所述span-igm s1组合导航设备的数据传送功能；所述4g通讯模块实现接收gnss基准站差分数据(如千寻cors基准站差分数据流)或自己搭建的gnss基准站差分数据流的功能。
49.对于ros下安装vlp驱动，首先配置所述处理器的有线连接的ip地址，其次安装所述ros版本的velodyne驱动,并进行编译、连接好处理器与所述16线激光雷达并供电、打开网页，网址处输入192.168.1.201，若看到所述16线激光雷达的参数界面即实现所述16线激光雷达与所述处理器的配置；用户可根据需要，在所述参数界面设置所述16线激光雷达发布点云消息的频率；对于ros下安装mynt eye sdk ros驱动，从myntai官网下载mynt-eye-s-sdk包，ros下进行编译即可，用户可根据需要，在所述mynt-eye-s-sdk包的配置文件中设置所述小觅双目相机发布图像消息的频率；对于配置串口工具minicom,首先配置4g通讯模块实现千寻cors基准站差分数据流的功能，其次在ros下安装串口工具minicom,配置串口设备名称与波特率，实现所述处理器与所述span-igm s1组合导航设备的配置。
50.(二)采集双目相机图像数据、激光数据、gnss数据、imu数据
51.1.ros下启动上述传感器驱动。通过ros方式同时启动上述各传感器驱动，实现上述各传感器的软同步。
52.2.创建节点订阅带有所述传感器消息的话题并发布。首先创建订阅者分别订阅带有所述传感器消息的话题；其次创建时间同步器，采集时间戳对齐的所述各传感器消息；然后创建回调函数，通过指创建针指向所述各传感器消息地址，将采集的时间戳对齐的所述各传感器消息通过话题发布出去。这样就发布了时间戳对齐的所述传感器数据，实现数据采集功能。
53.(三)数据存储
54.1.数据存储线程。首先创建节点订阅带有所述时间戳对齐的传感器消息的话题；其次创建回调函数，在所述回调函数中创建队列容器存储所述传感器消息的数据，将所述
队列容器的数据通过文件流读写文件的方式存放在自定义路径的data文件夹下，所述data文件夹路径下包括gnss文件夹、leftimg文件夹、lidar文件夹、rightimg文件夹，分别存放采集的gnss数据和imu数据、所述双目相机数据以及所述激光雷达数据，所述双目相机数据以及所述激光雷达数据以实时时间戳命名；存储方便有序、存储数据易查看，易转载它用。
55.2.数据可视化线程。将采集的所述小觅双目相机左相机的图像消息变换到opencv数据类型下，将采集的所述16线激光雷达的激光数据变换到pcl点云数据类型下，根据所述16线激光雷达与所述小觅双目相机的内，外参数，将所述点云数据投影到采集的所述小觅双目相机左相机的图像上，显示所述左相机的图像数据的同时显示所述点云数据，实现可视化的功能。设点云在激光雷达定义的坐标系下的空间位置为(x，y，z)
t
，点云在相机坐标系下空间位置为(xc，yc，zc)
t
；k是相机内参，k∈r3×3；r，t分别是所述激光坐标系到所述相机坐标系的旋转矩阵和平移向量，r∈r3×3，t∈r1×3。(u，v，1)
t
是点云投影在图像上的投影点。将点云由激光雷达坐标系变换到相机坐标系：
[0056][0057]
再由相机坐标系变换到归一化平面坐标系，并投影到像素平面上：
[0058][0059]
即可将所述点云数据投影到采集的所述小觅双目相机左相机的图像上，显示所述左相机的图像数据的同时显示所述点云数据，实现可视化的功能。
[0060]
在数据存储线程与数据可视化线程中添加互斥锁，实现多线程执行保护机制。当所述队列容器中采集的所述传感器消息的规模大小到达一定程度后，就去执行可视化线程，执行完又去执行数据存储线程。实现数据存储同时显示所述16线激光雷达点云数据与所述小觅双目相机图像信息的功能。
[0061]
(四)数据发布
[0062]
1.将存储的所述传感器数据的时间戳排序。首先创建容器存储所述各传感器消息的时间戳，其次通过排序语句对所述容器的各传感器消息时间戳按从小到大的顺序排序，得到按照时间排序的时间戳容器。
[0063]
2.按照时间顺序发布各传感器数据。以所述按照时间排序的时间戳容器的规模大小为判断条件，将所述各传感器时间戳与所述容器内按照时间排序的时间戳进行匹配，通过循环语句按照文件流读写文件的方式发布带有所述各传感器消息的话题，实现数据发布功能。用户可根据自身需求，设置发布频率。
[0064]
以上所述，仅是本发明的较佳实施例而已，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。