之前的文章只是将数据的雷达在rviz中进行可视化了，并没有实际用起来。
这篇文章将使用Kaist Urban08 数据使用LIO-SAM进行三维点云地图的构建。
仓库的地址是
https://github.com/xiangli0608/Learning_localization_from_scratch_ws
代码已经写好，直接编译就可以了。
由于放入了LIO_SAM，所以需要依赖 gtsam 4.0.2 。
之前说过可以用 file_player的形式将数据放出来，但是没有通过bag播放数据方便。所以这一步先将kaist数据集转成bag文件。
转录使用的工具是 kaist2bag，这个工具已经放在工程里了，可以直接用，在 src/kaist_tool/kaist2bag。
这是别人的开源项目，这里直接用了，首先感谢一下这个老哥。
kaist2bag/README.md 里有说怎么进行使用，这里简单说明一下。
首先更改 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的选择。
之后通过如下命令执行launch roslaunch kaist2bag kaist2bag.launch
执行之后程序会运行一段时间，运行结束之后会在 svae_to 文件夹生成很多个bag, 每个topic一个bag。
接下来就是对这些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
需要将bag的名字修改到 src/mapping/LIO-SAM/launch/run.launch 中的 bag_filename 变量中。
之后运行lio-sam，运行之前别忘了要source一下。
roslaunch lio_sam run.launch
之后就会出现已经配置好的rviz界面，如下所示：
想要评估建图效果，可以用建图的轨迹与数据集提供的真值做对比，进行精度分析。
代码里目前实现了输出轨迹文件的功能。evo需要的是txt格式的轨迹，数据集提供的真值是csv格式的，所以需要做一下转换。
这一步需要安装依赖项
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。
当然，这个我已经转好了，放在了 src/doc/ground_truth 文件夹下，不再需要自己转了。
当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方向上还是偏差比较大的，之后存在改进空间。
由于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);
}
暂时将lio-sam中的time字段检查给注释掉了。每个点的时间戳现在还没有考虑好怎么做。
没有time字段，自然也就没有做运动畸变去除。运动畸变去除这里也注释掉了。
KAIST数据集有两个激光雷达，左右对称分布，且安装位置和地面有一定倾斜，但是IMU安装位置是平行于地面的。
之前lio-sam原版代码只订阅一个点云都话题。当只使用Kaist数据集的1个点云数据进行建图的效果不太好，所以将lio-sam改成订阅2个点云话题了。
具体的代码处理逻辑如下：
具体代码改动如下所示
回调从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;
}
}
原版的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
为了评估建图效果，需要将建图轨迹输出并和真值对比。所以代码里添加了将优化后的轨迹输出的功能。
具体函数如下
// 输出轨迹到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();
}
这个函数会在结束程序的时候执行，也就是按 ctrl c 之后才会执行。保存轨迹的目录是 src/doc/mapping_results 文件夹里，保存的文件名是 tum_lio_sam_pose.txt 。
如果哪位同学下载了Kaist的别的数据集，可以帮忙测试下，别的数据集需要改一下 LIO-SAM/config/params.yaml 中的雷达到imu的坐标变换，如果做的话可以加我微信，一起聊聊。
这篇文章的代码实现是由 赵焕峰，周勇，李维 三位同学完成的，感谢这两位同学的贡献。
这篇文章的代码其实早都写完了，但是一直没写文章。一方面是工作事情很多，下班也晚，一方面也是自己懒了。
接下来的计划是通过这个不太完美的地图跑一下定位。
先从odom与imu的融合开始。