当前位置:   article > 正文

lego_loam、lio_sam运行kitti(完成kitti2bag、evo测试)_evo工具 lio-sam

evo工具 lio-sam

目录

一、工作空间的创建,功能包的编译等等

二、lego_loam运行、记录traj轨迹

三、evo对比使用

四、kitti2bag转换

五、lio_sam


一、工作空间的创建,功能包的编译等等

https://blog.csdn.net/qq_40528849/article/details/124705983

二、lego_loam运行、记录traj轨迹

1.运行 launch 文件

roslaunch lego_loam run.launch

注意:参数“/ use_sim_time”,对于模拟则设置为“true”,对于使用真实机器人则设置为“false”。

2. 播放bag文件

rosbag play *.bag --clock --topic  /velodyne_points  /imu/data

P.S.①运行上述指令时注意cd到存放rosbag文件的路径下

      ②虽然 /imu/data 是可选的,但如果提供的话,它可以大大提高估计的准确性。如果你的 IMU 帧与 Velodyne 帧不对齐,使用 IMU 数据将导致明显漂移。

      ③如果bag文件的话题不匹配,需要remap话题,如下:LeGO-LOAM的节点话题是/kitti/velo/pointcloud,而rosbag的节点话题是velodyne_points。笔者选择更改rosbag的节点话题,运行remap命令:

格式 :

rosbag play *.bag 发布的话题 := 算法接收的话题 --clock

举例:

-r 参数用来指定播放速度,3代表以3倍速度播放

rosbag play *.bag velodyne_points:=/kitti/velo/pointcloud  --clock  -r 3

LeGO_LOAM

rosbag play kitti_2011_10_03_drive_0034_synced_10hz.bag --clock --topics /kitti/velo/pointcloud

LIO_SAM

rosbag play kitti_2011_10_03_drive_0027_synced_100hz.bag --clock --topics /points_raw /imu_raw -r 0.1

-s 参数用于指定从几秒开始:

//从第10秒开始播放xx.bag

rosbag play -s 10 xx.bag 

rosbag使用介绍_rosbag play 指定时间_春至冬去的博客-CSDN博客

ROS系统学习笔记:bag的录制和播放 - 知乎

使用LeGO-LOAM运行KITTI数据集_Ocean_Controller的博客-CSDN博客

④运动轨迹的记录、kitti的64线的配置 代码修改参考如下:

//保存轨迹,需要修改轨迹的保存路径,保存的txt文件要提前建好。

在终端窗口,键入ctrl+c退出程序后,在对应路径文件下会写入tum格式的路径信息(当你在运行(lego_loam算法run.launch)的终端里按一次Ctrl + C时才会执行。按两次会保存不全,因此按一次就可以了。生成完后会自动停止。),如果想保存在其他路径下,需要在mapOptmization.cpp中的visualizeGlobalMapThread函数中修改路径:

LeGO-LOAM跑KITTI数据集评估方法【代码改】_lego loam跑kitti需要改什么_学无止境的小龟的博客-CSDN博客

值得注意的是:

        我按照上面博客修改方法保存位姿,测试kitti_06序列。发现evo怎么都对不齐位姿,即使用了-as命令也不行。就是差一点点,让人感觉是不是lego算法有误差?后来发现,这个轨迹像反的,尝试着把xyz取反跑一下。结果就完美匹配。

修改前
修改后

不过呢还是存在一个问题,roll和真值相比差了一个常量,如下图:

分享一下我的代码:

  1. //保存轨迹信息(时间戳、xyz、旋转四元数q)
  2. void visualizeGlobalMapThread(){
  3. ros::Rate rate(0.2);
  4. while (ros::ok()){
  5. rate.sleep();
  6. publishGlobalMap();
  7. }
  8. // save final point cloud
  9. std::cout << "start save final point cloud" << std::endl;
  10. std::cout << "======================================================" << std::endl;
  11. ofstream f;
  12. f.open("/home/nb/traj_slam/06_own_tum.txt");
  13. f << fixed;
  14. //std::cout << "traj roll" << cloudKeyPoses6D->points[0].roll << std::endl;
  15. for(size_t i = 0;i < cloudKeyPoses3D->size();i++)
  16. {
  17. float cy = cos((cloudKeyPoses6D->points[i].yaw)*0.5);
  18. float sy = sin((cloudKeyPoses6D->points[i].yaw)*0.5);
  19. float cr = cos((cloudKeyPoses6D->points[i].roll)*0.5);
  20. float sr = sin((cloudKeyPoses6D->points[i].roll)*0.5);
  21. float cp = cos((cloudKeyPoses6D->points[i].pitch)*0.5);
  22. float sp = sin((cloudKeyPoses6D->points[i].pitch)*0.5);
  23. float w = cy * cp * cr + sy * sp * sr;
  24. float x = cy * cp * sr - sy * sp * cr;
  25. float y = sy * cp * sr + cy * sp * cr;
  26. float z = sy * cp * cr - cy * sp * sr;
  27. //save the traj 减掉第0个时间戳。cloudKeyPoses6D->points[0].time == 1317400962.039569
  28. f << setprecision(6) << (cloudKeyPoses6D->points[i].time-cloudKeyPoses6D->points[0].time) << " " << setprecision(9) << -1*(cloudKeyPoses6D->points[i].x) << " " << -1*(cloudKeyPoses6D->points[i].y) << " " << -1*(cloudKeyPoses6D->points[i].z) << " " << x << " " << y << " " << z << " " << w << endl;
  29. }
  30. f.close();
  31. }

  ⑤(选做)打印话题、测试信息

 "a_loam和lego_loam在节点中订阅话题/aft_mapped_to_init,lio-sam要订阅lio_sam/mapping/odometryh话题(或者lio_sam/mapping/odometry_incremental,这两个好像没区别),这些都可以在算法的Map节点里找到。我看很多博客上对lego/lio的欧拉角进行了转化再保存,实际上它们都有自己发布的odom信息,直接订阅就可以保存了。"

LOAM、Lego-liom、Lio-sam轨迹保存,与Kitti数据集真值进行评估_kitti数据集轨迹_李97的博客-CSDN博客

在终端执行以下命令,可以查看打印pose:

rostopic echo /aft_mapped_to_init(odom的话题)

在终端执行以下命令,可以将pose保存到pose.txt中:

rostopic echo /aft_mapped_to_init > pose.txt

但是,这种方式不方便数据分析,需要以一定格式存储。

参考链接:

LeGO-LOAM初探:原理,安装和测试_W_Tortoise的博客-CSDN博客

三、evo对比使用

evo开源代码:https://github.com/MichaelGrupp/evo
evo 对姿态的数据格式有三种,分别为:TUM/EuRoC/Kitti     

区别为:


        进行odom的轨迹保存后进行评估,常用的指令主要包括三个:

1 evo_traj

evo_traj tum --ref=tum_00_gt.txt re_00_TUM.txt  -p --plot_mode xz -as

其中,-p参数绘制轨迹,--ref参数指定参考轨迹, -as参数将两条轨迹进行对齐

2 evo_ape

evo_ape评价的是绝对误差随路程的累计,是一个累积量。

evo_ape tum groundtruth.txt our.txt -as

max:最大误差(m)                        mean:误差均值
median:误差中位数                          min:最小误差
rmse:均方根误差                              sse:方差
std:标准差 

3 evo_rpe

evo_rpe相对位姿误差可以给出局部精度,例如slam系统每米的平移或者旋转漂移量。

其中--delta 100表示的是每隔100米统计一次误差,这样统计的其实就是误差的百分比,和kitti的odometry榜单中的距离误差指标就可以直接对应了。

evo_rpe tum Ground_Truth.txt Marked-LIEOours.txt --delta 100

轨迹评估工具使用总结(一) evo从安装到入门_evo工具起点对齐_Techblog of HaoWANG的博客-CSDN博客

lego-loam 跑 kitti00包(kitti2bag+lego-loam+evo)详细版_kitti00数据集_Cloudy_to_sunny的博客-CSDN博客

用于evo评估的里程计轨迹保存方法_evo保存轨迹_dear小王子的博客-CSDN博客


evo的旋转对齐命令,如下:

–align/-a 采用SE(3) Umeyama对齐,只处理平移和旋转
–align --correct_scale/-as 采用Sim(3) Umeyama对齐,同时处理平移旋转和尺度
–correct_scale/-s 仅对齐尺度

SLAM精度评定工具——EVO使用方法详解_有了个相册的博客-CSDN博客


evo的tum转kitti命令,如下:

evo_traj tum KeyFrameTrajectory_TUM.txt --save_as_kitti

用evo工具分析ORB-SLAM2运行TUM,KITTI,EuRoC数据集轨迹_m0_60355964的博客-CSDN博客


四、kitti2bag转换

看了几个转换算法,最后还是选用了kitti2bag.

lidar2rosbag_kitti :

        这里阿里员工在github上开源的工具包,

        优点:据说可以解决各种类型数据集的转换问题。

        缺点:从名字可以看出来,转换后只有lidar话题数据,所以我也没试过。

kitti_lego_loam:

        代码:https://github.com/Mitchell-Lee-93/kitti-lego-loam

        该code里面是两个功能包,一个是将kitti数据集转换成bag,另一个就是修改了一点的lego-loam代码:1.修改雷达参数(kitti数据集的lidar是64线)2.关闭了运动补偿 3.为了评测,保存了位姿数据,也就是记录了轨迹。
        测试该代码出现错误:转换出来只有4.1kb。原博主给我指出了问题:“数据集文件夹的子文件夹名字是sequence,c程序里设定是sequences,有一个s的差别,可能导致读取不到数据集。另外查看路径是否正确,文件夹名字摆放位置是否正确。生成4.1kb一般是没有读取到源文件。”但是我检查发现,命名是正确的。找不到原因,因此也就放弃了这个方法。

kitti2bag:(推荐)

        (同步imu10hz)代码:GitHub - tomas789/kitti2bag: Convert KITTI dataset to ROS bag file the easy way!

        (原始imu100hz)代码:GitHub - TixiaoShan/LIO-SAM: LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping

        kitti2bag是kitti官网推荐的转换包,优点是既能够转换同步后的数据中,imu的频率在10Hz左右,也能转换原始的100Hz的IMU数据,该方法同时使用原始数据(extract)和同步数据(synced)制作数据包。

下面是kitti2bag的具体操作流程:        

1、该博客手把手图片教学,挺方便。

将KITTI数据集转化为ROS bag包——kitti2bag使用教程_kitti rosbag_Kadima°的博客-CSDN博客

不过我看好多博客下面评论,不知道在哪下载数据集,下载真值等等。

官网如下:The KITTI Vision Benchmark Suite

先科学上网,再注册,然后找raw data,进去找residential,再按下图找对应的序列。

数据区别:100Hz的IMU数据在 [unsynced+unrectified data] 中,而 [synced+rectified data] 保存的是修正后的10Hz数据。


我按照参考方法,产生了4.1kb的bag包,报错信息说找不到相关文件。所以我新建了一个2011_09_30的文件,这下就成功了。分享一下我的文件目录。

首先cd到2011_09_30文件夹所在的目录,然后执行如下命令: 

kitti2bag -t 2011_09_30 -r 0020 raw_synced 

该命令中,2011_09_30下载并解压后的数据集目录,0020是drive的地址序号,raw_synced表示加载的文件是raw_data。

转换后的bag文件太大了,因此使用命令进行了压缩。

rosbag compress --lz4 *.bag


2、该博客提供了序列对应表格,序列图,最重要的是,在文末她说:“同步后的数据中,imu的频率在10Hz左右,如果需要100Hz的IMU数据,可以参考LIO-SAM【项目地址】中作者改过的kitti2bag文件。该方法同时使用原始数据(extract)和同步数据(synced)制作数据包。”

  1. # kitti2bag(原始imu数据100hz)
  2. ## How to run it?
  3. ```bash
  4. wget https://s3.eu-central-1.amazonaws.com/avg-kitti/raw_data/2011_09_26_drive_0084/2011_09_26_drive_0084_sync.zip
  5. wget https://s3.eu-central-1.amazonaws.com/avg-kitti/raw_data/2011_09_26_drive_0084/2011_09_26_drive_0084_extract.zip
  6. wget https://s3.eu-central-1.amazonaws.com/avg-kitti/raw_data/2011_09_26_calib.zip
  7. unzip 2011_09_26_drive_0084_sync.zip
  8. unzip 2011_09_26_drive_0084_extract.zip
  9. unzip 2011_09_26_calib.zip
  10. python kitti2bag.py -t 2011_09_26 -r 0084 raw_synced .
  11. ```
  12. That's it. You have a bag that contains your data.
  13. Other source files can be found at [KITTI raw data](http://www.cvlibs.net/datasets/kitti/raw_data.php) page.

KITTI数据集_木子雨舟的博客-CSDN博客

执行命令后报错:

  1. Traceback (most recent call last):
  2. File "kitti2bag.py", line 17, in <module>
  3. from tqdm import tqdm
  4. ImportError: No module named tqdm

第一次跑,报错的原因是缺少tqdm包,我们只需要安装一下就行。

pip install tqdm

成功截图:

在kitti 06序列上,进行原始数据和优化后数据的evo_traj对比,如下图:

可以看出优化后的轨迹06_own_tum更优,与真值轨迹较近。

在kitti 06序列上,进行原始数据和优化后数据的ape、rpe对比,如下图:

              

SLAM精度评定工具——EVO使用方法详解_有了个相册的博客-CSDN博客


五、lio_sam

为了适配kitti数据集,我们需要修改参数和代码,来满足lio_sam对ring 和 time的要求。

5.1、修改参数

params.yaml文件修改:

  1. # Topics for processing kitti by author
  2. pointCloudTopic: "/points_raw" # Point cloud data/ for kitti "/kitti/velo/pointcloud"
  3. imuTopic: "/imu_raw" # IMU data/ for kitti "/kitti/oxts/imu"
  4. odomTopic: "odometry/imu" # IMU pre-preintegration odometry, same frequency as IMU
  5. gpsTopic: "odometry/gpsz" # GPS odometry topic from navsat, see module_navsat.launch file
  6. # Sensor Settings
  7. sensor: velodyne # lidar sensor type, 'velodyne' or 'ouster' or 'livox'
  8. N_SCAN: 64 # number of lidar channel (i.e., Velodyne/Ouster: 16, 32, 64, 128, Livox Horizon: 6)
  9. Horizon_SCAN: 1800 # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048, Livox Horizon: 4000)
  10. downsampleRate: 2 # default: 1. Downsample your data if too many points. i.e., 16 = 64 / 4, 16 = 16 / 1
  11. lidarMinRange: 1.0 # default: 1.0, minimum lidar range to be used
  12. lidarMaxRange: 1000.0 # default: 1000.0, maximum lidar range to be used
  13. # lidar到imu的平移
  14. extrinsicTrans: [8.086759e-01, -3.195559e-01, 7.997231e-01]
  15. # imu到lidar的旋转
  16. extrinsicRot: [9.999976e-01, 7.553071e-04, -2.035826e-03, -7.854027e-04, 9.998898e-01, -1.482298e-02, 2.024406e-03, 1.482454e-02, 9.998881e-01]
  17. extrinsicRPY: [9.999976e-01, 7.553071e-04, -2.035826e-03, -7.854027e-04, 9.998898e-01, -1.482298e-02, 2.024406e-03, 1.482454e-02, 9.998881e-01]

注意点:
1、配置文件(params.yaml)内的参数通过参数服务器传送进源程序,会覆盖掉头文件内(utility.h)的对应参数。
2、其中extrinsicRot表示imu -> lidar的坐标变换,用于旋转imu坐标系下的角速度和线加速度到lidar坐标系下,extrinsicRPY则用于旋转imu坐标系下的欧拉角(姿态信息)到lidar坐标下,由于lio-sam作者使用的imu的欧拉角旋转指向与lidar坐标系不一致,因此使用了两个不同的旋转矩阵,但是大部分的设备两个旋转应该是设置为相同的。 

在Ubuntu20.04系统上LIO-SAM跑KITTI数据集和自己数据集代码修改_学无止境的小龟的博客-CSDN博客

5.2、修改代码(产生ring、time、保存tum格式的轨迹)

utility.h文件修改:

  1. // add by sx
  2. bool has_ring;
  3. float ang_bottom;
  4. float ang_res_y;
  5. ParamServer()
  6. {
  7. nh.param<std::string>("/robot_id", robot_id, "roboat");
  8. nh.param<std::string>("lio_sam/pointCloudTopic", pointCloudTopic, "points_raw");
  9. nh.param<std::string>("lio_sam/imuTopic", imuTopic, "imu_correct");
  10. nh.param<std::string>("lio_sam/odomTopic", odomTopic, "odometry/imu");
  11. nh.param<std::string>("lio_sam/gpsTopic", gpsTopic, "odometry/gps");
  12. // add by sx
  13. nh.param<bool>("lio_sam/has_ring", has_ring, true);
  14. nh.param<float>("lio_sam/ang_bottom", ang_bottom, 15.0);
  15. nh.param<float>("lio_sam/ang_res_y", ang_res_y, 1.0);
  16. ..............
  17. }

 cloud_info.msg文件修改:

  1. # 用于改写ring和time相关
  2. float32 startOrientation
  3. float32 endOrientation
  4. float32 orientationDiff

imageProjection.cpp文件修改:

  1. void projectPointCloud()
  2. {
  3. int cloudSize = laserCloudIn->points.size();
  4. bool halfPassed = false;
  5. cloudInfo.startOrientation = -atan2(laserCloudIn->points[0].y, laserCloudIn->points[0].x);
  6. cloudInfo.endOrientation = -atan2(laserCloudIn->points[laserCloudIn->points.size() - 1].y,
  7. laserCloudIn->points[laserCloudIn->points.size() - 1].x) + 2 * M_PI;
  8. if (cloudInfo.endOrientation - cloudInfo.startOrientation > 3 * M_PI) {
  9. cloudInfo.endOrientation -= 2 * M_PI;
  10. } else if (cloudInfo.endOrientation - cloudInfo.startOrientation < M_PI)
  11. cloudInfo.endOrientation += 2 * M_PI;
  12. cloudInfo.orientationDiff = cloudInfo.endOrientation - cloudInfo.startOrientation;
  13. // range image projection
  14. for (int i = 0; i < cloudSize; ++i)
  15. {
  16. PointType thisPoint;
  17. thisPoint.x = laserCloudIn->points[i].x;
  18. thisPoint.y = laserCloudIn->points[i].y;
  19. thisPoint.z = laserCloudIn->points[i].z;
  20. thisPoint.intensity = laserCloudIn->points[i].intensity;
  21. float range = pointDistance(thisPoint);
  22. if (range < lidarMinRange || range > lidarMaxRange)
  23. continue;
  24. //从此处开始
  25. int rowIdn = -1;
  26. if (has_ring == true){
  27. rowIdn = laserCloudIn->points[i].ring;
  28. }
  29. else{
  30. float verticalAngle, horizonAngle;
  31. verticalAngle = atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI;
  32. rowIdn = (verticalAngle + ang_bottom) / ang_res_y;
  33. }
  34. if (rowIdn < 0 || rowIdn >= N_SCAN)
  35. continue;
  36. if (rowIdn % downsampleRate != 0)
  37. continue;
  38. int columnIdn = -1;
  39. if (sensor == SensorType::VELODYNE || sensor == SensorType::OUSTER)
  40. {
  41. float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
  42. static float ang_res_x = 360.0/float(Horizon_SCAN);
  43. columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2;
  44. if (columnIdn >= Horizon_SCAN)
  45. columnIdn -= Horizon_SCAN;
  46. }
  47. else if (sensor == SensorType::LIVOX)
  48. {
  49. columnIdn = columnIdnCountVec[rowIdn];
  50. columnIdnCountVec[rowIdn] += 1;
  51. }
  52. if (columnIdn < 0 || columnIdn >= Horizon_SCAN)
  53. continue;
  54. if (rangeMat.at<float>(rowIdn, columnIdn) != FLT_MAX)
  55. continue;
  56. if (has_ring == true)
  57. thisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].time);
  58. else {
  59. float ori = -atan2(thisPoint.y, thisPoint.x);
  60. if (!halfPassed) {
  61. if (ori < cloudInfo.startOrientation - M_PI / 2) {
  62. ori += 2 * M_PI;
  63. } else if (ori > cloudInfo.startOrientation + M_PI * 3 / 2) {
  64. ori -= 2 * M_PI;
  65. }
  66. if (ori - cloudInfo.startOrientation > M_PI) {
  67. halfPassed = true;
  68. }
  69. } else {
  70. ori += 2 * M_PI;
  71. if (ori < cloudInfo.endOrientation - M_PI * 3 / 2) {
  72. ori += 2 * M_PI;
  73. } else if (ori > cloudInfo.endOrientation + M_PI / 2) {
  74. ori -= 2 * M_PI;
  75. }
  76. }
  77. float relTime = (ori - cloudInfo.startOrientation) / cloudInfo.orientationDiff;
  78. // 激光雷达10Hz,一帧0.1秒
  79. laserCloudIn->points[i].time = 0.1 * relTime;
  80. thisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].time);
  81. }
  82. rangeMat.at<float>(rowIdn, columnIdn) = range;
  83. int index = columnIdn + rowIdn * Horizon_SCAN;
  84. fullCloud->points[index] = thisPoint;
  85. }
  86. }

保存tum格式的轨迹

在mapOptmization -> void publishOdometry()增加下面代码:

  1. //保存轨迹,path_save是文件目录,txt文件提前建好,/home/xxx/xxx.txt,
  2. std::ofstream pose1("/home/nb/traj_slam/lio_sam/100hz_own_tum.txt", std::ios::app);
  3. pose1.setf(std::ios::scientific, std::ios::floatfield);
  4. pose1.precision(9);
  5. // if(flag_tum=1){
  6. static double timeStart = laserOdometryROS.header.stamp.toSec();
  7. // flag_tum=0;
  8. // }
  9. auto T1 =ros::Time().fromSec(timeStart) ;
  10. // tf::Quaternion quat;
  11. // tf::createQuaternionMsgFromRollPitchYaw(double r, double p, double y);//返回四元数
  12. pose1<< laserOdometryROS.header.stamp -T1<< " "
  13. << -laserOdometryROS.pose.pose.position.x << " "
  14. <<- laserOdometryROS.pose.pose.position.z << " "
  15. << -laserOdometryROS.pose.pose.position.y<< " "
  16. << laserOdometryROS.pose.pose.orientation.x << " "
  17. << laserOdometryROS.pose.pose.orientation.y << " "
  18. << laserOdometryROS.pose.pose.orientation.z << " "
  19. << laserOdometryROS.pose.pose.orientation.w << std::endl;
  20. pose1.close();

遇到的问题:lio_sam跑kitti总跑飞,Large velocity, reset IMU-preintegration!

解决方法:在参数配置正确的情况下,降低bag播放速度。(找这个问题花了2天时间!)

rosbag play your_bag.bag -r 0.1   //以0.1倍速播放bag包


使用KITTI跑LIOSAM并完成EVO评价_小霍金的博客-CSDN博客

使用开源激光SLAM方案LIO-SAM运行KITTI数据集,如有用,请评论雷锋_我兔会飞的博客-CSDN博客

LIO-SAM:配置环境、安装测试、适配自己采集数据集_有待成长的小学生的博客-CSDN博客


声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/Monodyee/article/detail/116653
推荐阅读
相关标签
  

闽ICP备14008679号