泡泡点云时空】interactive_slam：三维激光SLAM 的交互式地图校正框架

泡泡点云时空，带你精读点云领域顶级会议文章

标题：interactive_slam：Interactive Map Correction for 3D Graph SLAM

作者：Kenji Koide & Active Intelligent Systems Laboratory, Toyohashi University of Technology, Japan & Robot Innovation Research Center, National Institute of Advanced Industrial Science and Technology, Japan

来源：https://github.com/SMRT-AIST/interactive_slam

编译：石成玉

审核：郑森华

欢迎个人转发朋友圈；其他机构或自媒体如需转载，后台留言申请授权

interactive_slam简介

       interactive_slam是日本丰桥技术科技大学和日本国家先进工业科学技术研究所联合开发的一款基于3D激光雷达的开源建图框架。与现有的纯自动激光SLAM框架相比，它可以通过手动增减一些约束信息进行地图优化，从而更好地完成点云地图的制作。
       作者团队的目标是开发一款半自动的激光SLAM框架，允许用户以相对少的操作来直观且方便地修正地图（如：错误的里程计约束、错误的回环约束、地图重影等）。目前该框架提供了以下几个地图修正功能：

• [手动和自动]回环检测与修正

• [手动]基于平面的地图校正

• [手动]多地图融合

• [自动]位姿联合优化

    Github地址：

    https://github.com/SMRT-AIST/interactive_slam

    该框架基于ROS系统。可以通过读入hdl_graph_slam生成的位姿图（Pose Graph）或任一基于ROS里程计格式消息的程序（如LeGO-LOAM）生成的轨迹数据来构建地图。

安装方法

     首先安装GL3W, GLFW, Dear ImGui, portable-file-dialog, OpenMP, PCL, g2o等依赖库。具体依赖项的安装方法与链接网址可以见github相关页面。
       之后再通过以下命令进行安装编译。

注意：ROS Kinetic版本编译需要额外进行一些设置，详见github上的相关说明。

实例1

例1 基于hdl_graph_slam制作地图
       在本例中，我们通过hdl_graph_slam生成的轨迹（位姿图）来构建地图并进行修正。
示例包文件：hdl_400_dumped.tar.gz（示例文件可以直接通过hdl_graph_slam生成，或通过github上提供的链接进行下载）
1. 启动interactive_slam
rosrun interactive_slam interactive_slam

2. 打开轨迹地图目录

Menubar -> File -> Open -> New map -> 选择 hdl_400_dumped 目录

3. 进行自动回环检测
Menubar -> Graph -> Automatic loop detection
设置回环检测参数然后按下“Start”按钮：
扫描匹配

• 方法：NDT_OMP（利用Openmp优化的并行NDT版本）

• 匹配得分阈值：0.5

回环检测

• 距离阈值：20（两帧点云在该距离内的可进行回环匹配）

• 累计距离阈值（每隔多少米进行一次回环）：25或0.5（稠密回环）
  

进行回环之前：

进行回环优化之后（距离阈值 = 25）：
（墙的重影被修复）

进行稠密的回环约束优化之后（距离阈值 = 0.5）：

4. 保存修正的地图：
Menubar -> File -> Save -> Save map data -> 选择目标路径
 
5. 导出点云：
Menubar -> File -> Save -> Export point cloud -> 选择目标路径
之后可以使用PCD查看器（例如，pcl_viewer或CloudCompare）来查看导出的点云。

实例2

例2 基于其它ROS包生成轨迹数据制作地图
       在本例中，我们使用LeGO-LOAM生成的轨迹以及相应rosbag文件来构建地图。
示例包文件：hdl_400.bag.tar.gz
安装：

注意：你需要在LeGO-LOAM中编辑“utility.h”内容以适配相应的雷达传感器。

生成轨迹数据：
启动轨迹生成器：
roslaunch odometry_saver online.launch dst_directory:=/tmp/odometry points_topic:=/velodyne_points odom_topic:=/integrated_to_init endpoint_frame:=base_link origin_frame:=map

• dst_directory: 目标路径

• points_topic: 订阅的点云话题

• odom_topic: 订阅的里程计话题(nav_msgs/Odometry)

• endpoint_frame: 里程终点TF帧

• origin_frame: 里程起点TF帧

启动LeGO-LOAM并播放rosbag： 

roslaunch lego_loam run.launch
rosbag play --clock hdl_400.bag
       播放完rosbag后，按Ctrl+C终止odometry_saver和LeGO-LOAM。你将在目标目录中看到轨迹数据： 
/tmp/odometry
├── 1570513628_110666000.pcd
├── 1570513628_110666000.odom
├── 1570513628_161063000.pcd
├── 1570513628_161063000.odom
├── 1570513628_211459000.pcd
├── 1570513628_211459000.odom

通过轨迹数据生成地图：
rosrun interactive_slam odometry2graph
Menubar -> File -> Open -> ROS -> 选择 /tmp/odometry

       可以调整参数以更改关键帧间隔和下采样分辨率：

（下采样分辨率=0.1，关键帧距离=2.0）
注意：过多或密集的关键帧会影响建图优化的处理速度

保存地图：
Menubar -> File -> Save -> 选择目标路径
       之后就可以参照例1使用interactive_slam来打开和编辑创建的地图了。

实例3

例3 基于平面的地图校正与地图合并

       在本例中，我们使用手动回环、全局优化和平面约束等方法来修正一个有缺陷的地图，之后将它和另外一个地图进行融合。
示例包文件：centrair_100_corrected.tar.gz和centrair_101_graph.tar.gz

读取地图数据
打开'centrair_101_graph':
Menubar -> File -> Open -> New map -> 选择'centrair_101_graph'
       这时你可以看到一个由LeGO-LOAM轨迹拼成的点云地图，地图存在着一些错误和扭曲（在Shader setting窗口中禁用z_clipping以查看整个地图）。之后我们通过几个修复处理来修正整个地图。

手动回环
       手动选择两个关键帧来生成回环约束。
左键点击一个关键帧 -> Loop begin -> 右键点击另一个关键帧 -> Loop end

       接下来你将看到一个子窗口用来估算两个选定关键帧之间的相对位姿。

       首先，使用基于FPFH的位姿估计方法进行初始值猜测：Auto align -> OK

       使用这种全局匹配方法有时可能会失败。如果失败，使用“PX、PY、PZ、RX、RY、RZ”来手动调节两个关键帧的相对位姿。关键帧点云大致对齐后，执行扫描匹配以进行更精细的匹配：Scan matching -> OK

       最后检查点云是否正确对齐，点击Add edge将相对位姿约束添加到位姿图中。
注：一些参数设置可以详见PCL文档说明(FPFH, Registration)。
       将回环边添加进位姿图后，将执行图优化，可以看到地图的全局一致性得到了极大的改善。

自动回环
       为提高地图的局部一致性（解决重影问题），可以执行自动回环检测匹配。 
Menubar -> Graph -> Automatic loop detection -> Start

       在此过程中，关键帧将自动与它附近的关键帧进行比较，如果它们彼此匹配良好，则在它们之间形成一条回环约束。计算完所有关键帧后，按Stop终止自动回环检测。

图优化
       由于轨迹数据中的一些误差，整个地图仍然存在着较大的失真。通过优化相对位姿约束来提高地图质量。
Menubar -> Graph -> Edge refinement -> Start

       该过程会自动选择误差较大的边，并与该边连接的关键帧之间执行扫描匹配。我们使用位姿图优化估计的相对位姿作为初始解猜测。由于估计出的相对位姿与真实位姿接近，因此具有良好初始解猜测的扫描匹配可能会收敛到更好的结果。如果在重新执行扫描匹配之后，关键帧之间的匹配得分有所提高，则相对姿势约束将根据新的位姿进行更新。

平面约束
       经过一系列修复后，我们发现地图仍然存在一些扭曲。接下来创建几个平面约束来让地面变得平坦：

右键点击地面上的某个点 -> Plane detection

       选择一些平面候选点，调节一些参数然后点击Perform。地图上的橙色点将用于接下来的平面检测，判断它们是否都在同一平面上。如否，则调整“平滑度阈值”并重新按Perform。然后，点击Detect，执行基于RANSAC的平面检测。彩虹色的点表示检测到的平面内点。如果它们都在检测到的平面上，点击Add edge将平面顶点约束添加到位姿图中。

  要使平面成为基准地平面（nx=0，ny=0，nz=1，d=0）：

右键单击平面 -> Prior -> Ground (UnixZ, Zero)

       然后，地图将向与XY平面对齐的方向优化。

       再添加几个平面顶点约束，使地面完全平坦。

       若要使点云地图中的走廊笔直，可以在走廊两侧添加两个新的墙平面约束：

右击一个平面-> Loop begin -> 右击另一个平面 -> Loop end

按如下配置参数并按Add Edge：

• 平面模式：平行

• 信息矩阵的值大小：1000.0 （相当于设置平面可信度）

       现在，走廊看起来很直。可以添加更多的墙平面约束以进一步提高地图质量。

多地图融合
Menubar -> File -> Open -> Merge map

       在每个地图的点云帧之间添加一条边（为了进行优化，所有顶点都必须在一个图优化结构中）。

       第二个地图的第一个顶点已经被固定，从而防止地图与另一个地图融合。取消该顶点的固定并执行图优化。
Menubar -> Graph editor -> Unfix Vertex 343
Menubar -> Graph -> Optimize

       执行自动回环检测优化将两个地图融合为一个地图。
Menubar -> Graph -> Automatic loop detection -> OK

       至此，有关于interactive_slam的内容已经介绍完。还等什么，这么优秀的开源项目，赶紧下载源代码试试吧！

如果你对本文感兴趣，想要下载完整文章进行阅读，可以关注【泡泡机器人SLAM】公众号。

欢迎来到泡泡论坛，这里有大牛为你解答关于SLAM的任何疑惑。

有想问的问题，或者想刷帖回答问题，泡泡论坛欢迎你！

泡泡网站：www.paopaorobot.org

泡泡论坛：http://paopaorobot.org/bbs/