从零开始学定位 --- 使用kaist数据集进行LIO-SAM建图

之前的文章只是将数据的雷达在rviz中进行可视化了，并没有实际用起来。

这篇文章将使用Kaist Urban08 数据使用LIO-SAM进行三维点云地图的构建。

1 clone 工程

仓库的地址是
https://github.com/xiangli0608/Learning_localization_from_scratch_ws
代码已经写好，直接编译就可以了。

由于放入了LIO_SAM，所以需要依赖 gtsam 4.0.2 。

2 bag的转录

之前说过可以用 file_player的形式将数据放出来，但是没有通过bag播放数据方便。所以这一步先将kaist数据集转成bag文件。

转录使用的工具是 kaist2bag，这个工具已经放在工程里了，可以直接用，在 src/kaist_tool/kaist2bag。

这是别人的开源项目，这里直接用了，首先感谢一下这个老哥。

kaist2bag/README.md 里有说怎么进行使用，这里简单说明一下。

2.1 修改路径

首先更改 src/kaist_tool/kaist2bag/config/config.ymal

将前2行进行更改, dataset是数据集的路径，save_to是新生成的bag保存的路径

dataset: "/home/touchair/kaist_urban_dataset/urban08"
save_to: "/home/touchair/kaist_urban_dataset/urban08/bag"
1
2

在这个config里还可以进行是否输出某些topic的选择。

2.2 生成bag

之后通过如下命令执行launch roslaunch kaist2bag kaist2bag.launch

执行之后程序会运行一段时间，运行结束之后会在 svae_to 文件夹生成很多个bag, 每个topic一个bag。

接下来就是对这些bag进行合并。

2.3 合并bag

上个步骤生成的bag是一个topic一个bag，所以想要跑的话需要将这些bag合并一下。

合并的命令为 rosrun kaist2bag mergebag.py merged.bag ...
merged.bag是最后输出的bag， 以及之后的bag的名称，是需要合并的bag，这里可以使用绝对路径也可以使用相对路径。

通过这步，咱就将kaist的数据集转成了bag。

这一部分参考了勇哥知乎上的文章
KAIST数据集转换为rosbag https://zhuanlan.zhihu.com/p/544766790

3 运行LIO-SAM进行建图

3.1 修改bag名字

需要将bag的名字修改到 src/mapping/LIO-SAM/launch/run.launch 中的 bag_filename 变量中。

3.2 运行LIO-SAM

之后运行lio-sam，运行之前别忘了要source一下。
roslaunch lio_sam run.launch

之后就会出现已经配置好的rviz界面，如下所示：

4 输出轨迹进行evo评估

想要评估建图效果，可以用建图的轨迹与数据集提供的真值做对比，进行精度分析。

代码里目前实现了输出轨迹文件的功能。evo需要的是txt格式的轨迹，数据集提供的真值是csv格式的，所以需要做一下转换。

4.1 将kaist数据集提供的真值转成tum格式

这一步需要安装依赖项
pip3 install numpy scipy
已经填写在 install_dependence.sh 文件中。
执行步骤为

./src/scripts/kaist2evo.py -p /media/trunk/Trunk/0-LX/Kaist/Urban08

-p 后边接的是数据集的文件夹，之后可以加 -o 加输出轨迹文件的地址。

执行之后会在Urban08文件夹下生成2个txt文件，分别是 tum_ground_truth.txt 与 tum_vrs_gps.txt。

• tum_ground_truth.txt 是将 global_pose.csv 转成的tum格式的轨迹文件
• tum_vrs_gps.txt 是将 sensor_data/vrs_gps.csv 转成的tum格式的轨迹文件

当然，这个我已经转好了，放在了 src/doc/ground_truth 文件夹下，不再需要自己转了。

4.2 进行evo轨迹对比

当lio-sam结束建图之后，会在 Downloads/LOAM 文件夹下生成pcd地图，同时会在 src/doc/mapping_results 文件夹下生成 tum 格式的轨迹文件。

然后将 src/doc/ground_truth/tum_ground_truth.txt 与 src/doc/mapping_results/tum_lio_sam_pose.txt 这两个文件，分别填在 src/scripts/evo.sh 的 txt1 与 txt2 中

在执行 src/scripts/evo.sh 即可绘制轨迹图，ape图，rpe图。

可以看到，z方向上还是偏差比较大的，之后存在改进空间。

5 代码的改动内容

5.1 添加ring字段

由于lio-sam需要ring字段的数据，但是Kaist数据集中的vlp并没有这个字段，这时ring字段可以自己算一下再添加到点云中。

  float angle;
  uint16_t ring;
  while (!file.eof()) {
      PointXYZIR point;
      file.read(reinterpret_cast<char *>(&point.x), sizeof(float));
      file.read(reinterpret_cast<char *>(&point.y), sizeof(float));
      file.read(reinterpret_cast<char *>(&point.z), sizeof(float));
      file.read(reinterpret_cast<char *>(&point.intensity), sizeof(float));
      // 先计算角度，根据角度得到ring，然后将ring添加到点云中
      angle = atan2(point.z, sqrt(point.x * point.x + point.y * point.y)) / M_PI * 180 + 15;
      ring = round(angle / 2);
      point.ring = ring;
      pcl_cloud.points.push_back(point);
  }
1
2
3
4
5
6
7
8
9
10
11
12
13
14

5.2 注释掉lio-sam中time字段检查

暂时将lio-sam中的time字段检查给注释掉了。每个点的时间戳现在还没有考虑好怎么做。

没有time字段，自然也就没有做运动畸变去除。运动畸变去除这里也注释掉了。

5.3 订阅左右2个雷达数据

KAIST数据集有两个激光雷达，左右对称分布，且安装位置和地面有一定倾斜，但是IMU安装位置是平行于地面的。

之前lio-sam原版代码只订阅一个点云都话题。当只使用Kaist数据集的1个点云数据进行建图的效果不太好，所以将lio-sam改成订阅2个点云话题了。

具体的代码处理逻辑如下：

• 订阅2个雷达数据
• 将左右激光雷达通过lidar到IMU的外参转换到IMU系下，以减少其他坐标转换带来的影响
• 再完成2个点云的合并得到一个range image

具体代码改动如下所示

回调从1个回调 cloudHandler，改成了 pointCloudLeftHandler 与 pointCloudRightHandler 2个回调

右雷达为辅雷达，其回调如下所示。

    void pointCloudRightHandler(const sensor_msgs::PointCloud2ConstPtr &rightPointCloud) {
        std::lock_guard<std::mutex> lock1(veloLock);
        cachePointCloudRightQueue.push_back(*rightPointCloud);
        if (cachePointCloudRightQueue.size() < 5) {
            return;
        }

        currentPointCloudRightMsg = std::move(cachePointCloudRightQueue.front());
        cachePointCloudRightQueue.pop_front();
        pcl::moveFromROSMsg(currentPointCloudRightMsg, *pointCloudRightIn);

        if (pointCloudRightIn->is_dense == false) {
            ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");
            ros::shutdown();
        }

        pcl::PointCloud<PointXYZIRT>::Ptr pointCloudOut(new pcl::PointCloud<PointXYZIRT>());
        pointCloudOut = transformPointCloud(pointCloudRightIn, rightLidarToImu);
        pointCloudRightQueue.push_back(pointCloudOut);
        return;
    }

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

先进行数据的缓存，然后转成pcl格式，再调用transformPointCloud函数将雷达从雷达坐标系下转到imu坐标系下。

左侧点云为主雷达，就是之前lio-sam的雷达回调，但是多了个先转到imu坐标系下，与点云合并这2个操作。

其中合并点云的代码如下所示

    void mergePointCloud()
    {
        std::lock_guard<std::mutex> lock1(veloLock);

        if(pointCloudLeftQueue.size() > 3 && pointCloudRightQueue.size() > 3)
        {
            pointCloudLeft = std::move(pointCloudLeftQueue.front());
            pointCloudLeftQueue.pop_front();
            pointCloudRight = std::move(pointCloudRightQueue.front());
            pointCloudRightQueue.pop_front();
            *pointCloudFull = *pointCloudLeft + *pointCloudRight;
        }
        else
        {
            ROS_DEBUG("Waiting for point cloud data ...");
            return;
        }
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

5.5 EKF节点替换

原版的LIO-SAM使用EKF节点输出gps数据转换的Odometry。

现在修改成使用原始GNSS的数据，进行转换，输出里程计以供mapOptmization节点使用。

转换的顺序如下：LLA --> ECEF --> ENU，坐标系示意图如下

最终得到ENU系下的gps坐标，代码如下

//  convert  LLA to XYZ
Eigen::Vector3d lla = gtools.GpsMsg2Eigen(*msg);
Eigen::Vector3d ecef = gtools.LLA2ECEF(lla);
Eigen::Vector3d enu = gtools.ECEF2ENU(ecef);
1
2
3
4

也可以使用GeographicLib库替换，代码非常简单

GeographicLib::LocalCartesian geoConverter;
 //初始化
if(!init)
{
	geoConverter.Reset(gps_msg_ptr->latitude, gps_msg_ptr->longitude, gps_msg_ptr->altitude);
	init = true;
}

geoConverter.Forward(gps_msg_ptr->latitude, gps_msg_ptr->longitude, gps_msg_ptr->altitude
1
2
3
4
5
6
7
8
9

5.6 输出轨迹

为了评估建图效果，需要将建图轨迹输出并和真值对比。所以代码里添加了将优化后的轨迹输出的功能。

具体函数如下

    // 输出轨迹到txt
    void saveGlobalPath() 
    {
      std::string lio_sam_path = ros::package::getPath("lio_sam");
      int npos_1 = lio_sam_path.find_last_of("/");
      std::string path = lio_sam_path.substr(0, npos_1);
      int npos_2 = path.find_last_of("/");
      // src文件夹
      std::string src_path = path.substr(0, npos_2);

      std::ofstream tum_pose(src_path + "/doc/mapping_results" +
                          "/tum_lio_sam_pose.txt");
      tum_pose.setf(std::ios::scientific, std::ios::floatfield);
      for (auto ite = globalPath.poses.begin(); ite != globalPath.poses.end();
           ite++) {
        geometry_msgs::PoseStamped pose_stamped = *ite;
        // tum格式的轨迹
        tum_pose << pose_stamped.header.stamp << " "
              << pose_stamped.pose.position.x << " "
              << pose_stamped.pose.position.y << " "
              << pose_stamped.pose.position.z << " "
              << pose_stamped.pose.orientation.x << " "
              << pose_stamped.pose.orientation.y << " "
              << pose_stamped.pose.orientation.z << " "
              << pose_stamped.pose.orientation.w << std::endl;
      }
      tum_pose.close();
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28

这个函数会在结束程序的时候执行，也就是按 ctrl c 之后才会执行。保存轨迹的目录是 src/doc/mapping_results 文件夹里，保存的文件名是 tum_lio_sam_pose.txt 。

5.7 代码中一些其他的改动

• 注释掉了运动畸变去除的代码
• 对平移与旋转的预测部分的代码进行了更改，但是还存在一些问题
• 对imu的频率，是否使用里程计，是否使用gps，添加了参数控制是否使用
• 加入了左右激光雷达到IMU的外参的参数

6 待优化与实现项

6.1 待优化项

• 计算time字段，然后进行运动畸变的去除
• 平移与旋转的预测部分的代码还存在一些问题
• 解决imu的零偏问题，初始时进行零偏估计
• 添加GPS约束后初始一段时间内偶发不收敛的情况，这是个待修复的bug
• Urban08这个数据集的最后会出现高度差，这时回环找不到，就优化不过来，需要解决

6.2 待实现项

如果哪位同学下载了Kaist的别的数据集，可以帮忙测试下，别的数据集需要改一下 LIO-SAM/config/params.yaml 中的雷达到imu的坐标变换，如果做的话可以加我微信，一起聊聊。

Contributors

这篇文章的代码实现是由 赵焕峰，周勇，李维 三位同学完成的，感谢这两位同学的贡献。

结语

这篇文章的代码其实早都写完了，但是一直没写文章。一方面是工作事情很多，下班也晚，一方面也是自己懒了。

接下来的计划是通过这个不太完美的地图跑一下定位。

先从odom与imu的融合开始。

REFERENCES

• 1 KAIST数据集转换为rosbag
• 2 从零开始学定位 — 基于LIO-SAM的建图