之前的文章只是将数据的雷达在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"

在这个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 <bag_file_1> ... <bag_file_8>
merged.bag是最后输出的bag，<bag_file_1> 以及之后的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);}

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;}

先进行数据的缓存，然后转成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;}}

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);

也可以使用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

5.6 输出轨迹

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

具体函数如下

    // 输出轨迹到txtvoid 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();}

这个函数会在结束程序的时候执行，也就是按 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的建图

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

python箱线图_从零开始学Python-matplotlib系列（III）：箱线图
原标题:从零开始学Python-matplotlib系列(III):箱线图前言今天继续我们的python绘图系列,针对离散变量我们可以使用常见的条形图和饼图完成数据的可视化工作,那么,针对数值型变 ...
从零开始学定位 --- kaist数据集体验
1 Kaist数据集简介选择数据集选择了好几天,最终选择了kaist数据集,这个数据集中有轮速计,imu, gps, 16线雷达,这些传感器满足了我认为多传感器融合定位的需要. 简要介绍一下Kai ...
从零开始学Python（六）组合数据类型
从零开始学Python(六) 组合数据类型思维导图: 一.集合类型及操作 1.集合类型定义集合是多个元素的无序组合: ①集合类型与数学中的集合概念一致 ②集合元素之间无序,每个元素唯一,不存在相同 ...
Cartographer建图和纯定位
(一)cartographer在ros下安装参考: 要求: 64-bit, modern CPU (e.g. 3rd generation i7) 16 GB RAM Ubuntu 18.04 (Bi ...
初识视觉SLAM 用相机解决定位和建图问题
分享一下我老师大神的人工智能教程!零基础,通俗易懂!http://blog.csdn.net/jiangjunshow 也欢迎大家转载本篇文章.分享知识,造福人民,实现我们中华民族伟大复兴! 引言:视 ...
从零开始学前端：定位 --- 今天你学习了吗？（CSS：Day17）
从零开始学前端:程序猿小白也可以完全掌握!-今天你学习了吗?(CSS) 复习:从零开始学前端:显示隐藏与文本溢出 - 今天你学习了吗?(CSS:Day16) 文章目录从零开始学前端:程序猿小白也可以 ...
《Python深度学习从零开始学》简介
#好书推荐##好书奇遇季#深度学习入门书<Python深度学习从零开始学>,京东当当天猫都有发售.从模型和实验入手,快速掌握深度学习技术. 业内大咖强力推荐!!!武汉大学信息管理学院教授 ...
从零开始学人脸检测之Retinaface篇（内含魔改版GhostNet+mbv2）
从零开始学人脸检测之Retinaface篇(内含魔改版) 代码已开源,欢迎白嫖和star: github.com/pengtougu/Retinaface_Ghost 一.论文解读论文研读工作由Sa ...
70.打印所有Spring boot载入的bean【从零开始学Spring Boot】
[从零开始学习Spirng Boot-常见异常汇总] 问题的提出: 我们在开发过程当中,我们可能会碰到这样的问题:No qualifying bean 就是我们定义的bean无法进行注入,那到底是什 ...

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