R-Fans-16激光雷达运行LOAM代码及问题解决

前言

激光SLAM的算法有很多，其中，2D激光SLAM的算法有gmapping、Hector和cartographer等，3D激光SLAM的算法则有LOAM、A-LOAM和LeGO-LOAM等。目前，越来越多的科研人员专注于3D激光SLAM的研究，在越来越多的领域中得到应用。前段时间结束了激光SLAM和视觉SLAM相关理论的学习，决定选择LOAM算法作为激光SLAM上手项目实验。

R-Fans-16激光雷达

介绍

R-Fans-16激光雷达是北京北科天绘科技有限公司的一款16线激光雷达，根据厂商提供的R-Fans用户手册，查得R-Fans-16型号激光雷达相关参数如下：

最初的操作流程

R-Fans-16激光雷达驱动包准备

我一开始使用的是厂商提供的驱动包rfans_driver，并在Ubuntu中创建工作空间。
步骤如下：

1. 在Ubuntu的home下建立工作空间，并将驱动包rfans_driver复制到src文件夹下
2. 编译驱动包
3. .bashrc文件下配置工作环境
4. 配置电脑网络IP地址

LOAM代码准备

LOAM代码在GitHub上有很多版本，我使用的是蓝鲸ROS机器人论坛提供的loam_velodyne代码，代码链接： https://github.com/BluewhaleRobot/loam_velodyne.git
代码准备步骤：

1. 在Ubuntu的home下建立工作空间，并将loam_velodyne功能包clone到src文件夹下。即：

git clone https://github.com/BluewhaleRobot/loam_velodyne.git

2. 编译驱动包，编译时一定要用下面指令编译

$ catkin_make -DCMAKE_BUILD_TYPE=Release

3. .bashrc文件下配置工作环境
4. 启动雷达驱动程序后，重开终端输入rostopic list和rostopic info /话题查看数据类型pointcloud2的话题

5. 根据上一步得到pointcloud2对应的话题是/rfans_driver/rfans_points，在loam_velodyne功能包下的xiaoqiang_lungu_off.launch文件中，将：

remap from="/multi_scan_points" to="/rslidar_points" 

修改为：

remap from="/multi_scan_points" to="/rfans_driver/rfans_points" 

6. 再用catkin_make -DCMAKE_BUILD_TYPE=Release指令编译

遇到的问题

根据上述步骤，驱动包rfans_driver驱动R-Fans-16激光雷达跑LOAM算法的建图结果是一直在螺旋旋转。
结果如图所示：

问题分析与解决

分析可能性

一、可能是驱动包产生点云的坐标系（Fixed frame）与loam_velodyne中点云地图的坐标系（Fixed frame）不一致
分析：驱动包产生点云的坐标系（Fixed frame）是world，而loam_velodyne中点云地图的坐标系（Fixed frame）是camera_init，猜测是Fixed frame不一致所产生。
实验：通过在IDE中全局搜索camera_init，并用world替换camera_init。
结果：还是产生一样的错误，故不是该原因。
另：通过分析loam_velodyne程序，可知该程序是订阅pointcloud2的话题并且将该话题的数据重新处理再创建新的frame和stamp的pointcloud2数据，见common.h。

二、可能是TF问题
分析：SLAM定位的过程就是将传感器的位置、传感器采集周围物体相对于其本身的位置和空间坐标系建立联系。若能形成完整的TF树，则表示位姿转换过程正确。
实验：roslaunch loam_velodyne xiaoqiang_lungu_off.launch（运行loam_velodyne程序）时，重新打开终端输入rqt查看TF树建立情况。
结果：即TF树与loam_velodyne代码中配置TF树一样，故不是TF的问题。

三、可能是驱动包产生点云数据无时间戳
分析：查看loam_velodyne代码时，发现原本的pointcloud2数据需要重新处理再创建为新的stamp和frame的pointcloud2数据，见common.h。
实验：在loam_velodyne代码中，输入新的stamp对应代码位置，用ROS_INFO输出到终端上查看stamp信息。
结果：发布的stamp（时间戳）结果都是0，故有可能是无时间戳影响。
再实验：在rfans_driver代码中，写入相关stamp数据。然后运行loam_velodyne代码，其地图旋转问题还是没有解决！！！
再结果：无时间戳是地图旋转的问题之一，但还有其他原因。

问题解决

最后通过ROS_INFO输出每次scan时的位姿信息情况，发现位姿漂移的相当的厉害。结合LOAM原理与loam_velodyne相关代码分析，LOAM算法计算位姿是根据点云数据中的角点和平面点作为特征点去计算，角点和平面点的提取公式及其对应代码是固定的。所以结论就是R-Fans-16的驱动包rfan_driver有问题！

实验与解决：在GitHub上下载其他版本的R-Fans-16的驱动包并运行（此驱动包中launch文件需要修改参数），相关链接： https://github.com/BluewhaleRobot/rfans_driver.
结果：问题得到解决！（撒花）

LOAM建图结果