如何使用evo工具评估LeGO-LOAM跑KITTI数据集的结果

2023-10-08 09:40

本文主要是介绍如何使用evo工具评估LeGO-LOAM跑KITTI数据集的结果,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

如何使用evo工具评估LeGO-LOAM跑KITTI数据集的结果

  • 下载KITTI数据集
  • 安装kitti2bag
  • 修改LeGO-LOAM代码
    • utility.h
    • imageProjection.cpp
    • transformFusion.cpp
  • 安装evo
  • 最终结果

下载KITTI数据集

官方链接:KITTI官网
我们只用得到点云数据集和groundtruth,也就是odometry data set (velodyne laser data, 80 GB)和odometry ground truth poses (4 MB),不过我自己也没下成功那个80G的包,而是在raw data选项卡里找了00-10对应的包来下,要下的是包的scnced+rectified data与calibration两项,如图:
在这里插入图片描述这里给出groundtruth的00-10对应的数据集的名字(图来自CSDN博主学无止境的小龟):
在这里插入图片描述

安装kitti2bag

这个工具是用来把我们上一步中下的KITTI数据集转化成bag文件形式,直接pip安装就行。

pip install kitti2bag

把上一步中的数据集解压并如下合并:
在这里插入图片描述
(那个bag文件是我生成的,不用管)
然后在2011_09_30所在文件夹(即当前文件夹下)运行:

kitti2bag -t 2011_09_30 -r 0018 raw_synced

注意标号一定是要和你下的数据集的号对应,比如我这里就是0018(之前没注意这个就总运行错误,排查半天发现命令没打对)。
成功之后就会把文件夹转化成kitti_2011_09_30_drive_0018_synced.bag。

修改LeGO-LOAM代码

utility.h

原代码中是设置为16线的,不适配KITTI数据集的64线,以及其他一些参数也需要修改,所以在原代码中修改如下:

// VLP-16
//extern const int N_SCAN = 16;
//extern const int Horizon_SCAN = 1800;
//extern const float ang_res_x = 0.2;
//extern const float ang_res_y = 2.0;
//extern const float ang_bottom = 15.0+0.1;
//extern const int groundScanInd = 7;extern const int N_SCAN = 64;
extern const int Horizon_SCAN = 2083;
extern const float ang_res_x = 360.0/float(Horizon_SCAN);
extern const float ang_res_y = 26.8/float(N_SCAN-1);
extern const float ang_bottom = 24.8;
extern const int groundScanInd = 55;

注释掉的是原来的代码,便于找到并对应。
此外,还修改imuTopic:

extern const string imuTopic = "/kitti/oxts/imu";

不过我跑的过程中发现没有imu反而效果会更好,就没有用其实。(看到别的博文说是因为KITTI数据集imu本身的问题,会飘)

imageProjection.cpp

需要在imageProjection.cpp中修改雷达topic的名字:

        subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/kitti/velo/pointcloud", 1, &ImageProjection::cloudHandler, this);

或者直接remap改也是一样的。

transformFusion.cpp

其实这里主要是需要生成轨迹记录文件以用于后续evo的评估,我的做法是直接用了kitti-lego-loam(链接:kitti-lego-loam)代码中保存轨迹的做法,之前在网上找的别的方法都出现了奇奇怪怪的偏差……比如:
在这里插入图片描述(挠头.gif)于是自己找了kitti-lego-loam代码里的生成轨迹部分加上去。

在transformFusion.cpp中的TransformFusion类的private变量中加上:

    int init_flag=true;Eigen::Matrix4f H;Eigen::Matrix4f H_init;Eigen::Matrix4f H_rot;const string RESULT_PATH = "/media/cairui/Backup Plus/ubuntu20/KITTI/myRes.txt";

这个RESULT_PATH就是你要生成的轨迹文件,记得自行创建并修改这个变量哦。

在laserOdometryHandler函数修改如下:

    void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry){currentHeader = laserOdometry->header;double roll, pitch, yaw;geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation;tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);transformSum[0] = -pitch;transformSum[1] = -yaw;transformSum[2] = roll;transformSum[3] = laserOdometry->pose.pose.position.x;transformSum[4] = laserOdometry->pose.pose.position.y;transformSum[5] = laserOdometry->pose.pose.position.z;transformAssociateToMap();geoQuat = tf::createQuaternionMsgFromRollPitchYaw(transformMapped[2], -transformMapped[0], -transformMapped[1]);laserOdometry2.header.stamp = laserOdometry->header.stamp;laserOdometry2.pose.pose.orientation.x = -geoQuat.y;laserOdometry2.pose.pose.orientation.y = -geoQuat.z;laserOdometry2.pose.pose.orientation.z = geoQuat.x;laserOdometry2.pose.pose.orientation.w = geoQuat.w;laserOdometry2.pose.pose.position.x = transformMapped[3];laserOdometry2.pose.pose.position.y = transformMapped[4];laserOdometry2.pose.pose.position.z = transformMapped[5];pubLaserOdometry2.publish(laserOdometry2);Eigen::Quaterniond q;q.w()=laserOdometry2.pose.pose.orientation.w;q.x()=laserOdometry2.pose.pose.orientation.x;q.y()=laserOdometry2.pose.pose.orientation.y;q.z()=laserOdometry2.pose.pose.orientation.z;Eigen::Matrix3d R = q.toRotationMatrix();if (init_flag==true){H_init<< R.row(0)[0],R.row(0)[1],R.row(0)[2],transformMapped[3],R.row(1)[0],R.row(1)[1],R.row(1)[2],transformMapped[4],R.row(2)[0],R.row(2)[1],R.row(2)[2],transformMapped[5],0,0,0,1;init_flag=false;std::cout<<"surf_th : "<<surfThreshold<<endl;}H_rot<<	-1,0,0,0,0,-1,0,0,0,0,1,0,0,0,0,1;H<<  R.row(0)[0],R.row(0)[1],R.row(0)[2],transformMapped[3],R.row(1)[0],R.row(1)[1],R.row(1)[2],transformMapped[4],R.row(2)[0],R.row(2)[1],R.row(2)[2],transformMapped[5],0,0,0,1;H = H_rot*H_init.inverse()*H; //to get H12 = H10*H02 , 180 rot according to z axisstd::ofstream foutC(RESULT_PATH, std::ios::app);foutC.setf(std::ios::scientific, std::ios::floatfield);foutC.precision(6);//foutC << R[0] << " "<<transformMapped[3]<<" "<< R.row(1) <<" "<<transformMapped[4] <<" "<<  R.row(2) <<" "<< transformMapped[5] << endl;for (int i = 0; i < 3; ++i){for (int j = 0; j < 4; ++j){if(i==2 && j==3){foutC <<H.row(i)[j]<< endl ;}else{foutC <<H.row(i)[j]<< " " ;}}}foutC.close();laserOdometryTrans2.stamp_ = laserOdometry->header.stamp;laserOdometryTrans2.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));laserOdometryTrans2.setOrigin(tf::Vector3(transformMapped[3], transformMapped[4], transformMapped[5]));tfBroadcaster2.sendTransform(laserOdometryTrans2);}

直接把整个函数复制上来了,直接替换就不会找不到要改的啦(懒鬼发言)

安装evo

先安装依赖:

pip3 install --user numpy 
pip3 install --user scipy 
pip3 install --user matplotlib

git源代码并安装:

git clone https://github.com/MichaelGrupp/evo
cd evo 
pip3 install --user --editable . --upgrade --no-binary evo

安装之后可以先测试一下自带的test:

cd test/data 
evo_traj kitti KITTI_00_ORB.txt KITTI_00_SPTAM.txt --ref=KITTI_00_gt.txt -p --plot_mode=xz

出现下图就说明安装成功了。
在这里插入图片描述

这里顺便记录一些常用的evo命令,来自知乎大佬SLAM评估工具-EVO使用(二):
①evo_traj指令可以将各个算法估计出的路径和真实路径画在同一幅图中。
例:evo_traj kitti tra1.txt tra2.txt tra3.txt --ref=ground_truth.txt -va --plot --plot_mode xy
kitti表明处理的是kitti数据集的相关结果,这里也可以替换为tum和euroc;
tra1.txt tra2.txt tra3.txt表示的是不同算法所估计出的轨迹,这里可以列举多个文件每个文件名之间用一个空格隔开;
–ref=ground_truth.txt指明参考轨迹即真实轨迹;
-va包含两部分;1.-v或–verbose指明输出文件数据的相关信息;2.-a或–align指明对轨迹进行配准;
–plot表示画图;
–plot_mode xy表示图像投影在xoy平面上,其余可选参数为:xz,yx,yz,zx,zy,xyz;

②evo_ape计算轨迹的绝对位姿误差
绝对位姿误差,用于比较估计轨迹和参考轨迹并计算整个轨迹的统计数据,常用于评估测试轨迹的全局一致性。这里还是以kitti为例,tum和euroc格式相同。
evo_ape kitti ground_truth.txt tra1.txt -r full -va --plot --plot_mode xyz --save_plot ./tra1plot --save_results ./tra1.zip
kitti表明处理的是kitti数据集的相关结果,这里也可以替换为tum和euroc;
ground_truth.txt代表真实轨迹的数据;
tra1.txt代表估计轨迹的数据;
-r full表示同时考虑旋转和平移误差得到的ape,无单位(unit-less);
另外:
-r trans_part表示考虑平移部分得到的ape,单位为m;
-r rot_part表示考虑旋转部分得到的ape,无单位(unit-less);
-r angle_deg表示考虑旋转角得到的ape,单位°(deg);
-r angle_rad表示考虑旋转角得到的ape,单位弧度(rad);
-va包含两部分;1.-v或–verbose指明输出文件数据的相关信息;2.-a或–align指明对轨迹进行配准;
–plot表示画图;
–plot_mode xy表示图像投影在xoy平面上,其余可选参数为:xz,yx,yz,zx,zy,xyz;
–save_plot ./tra1plot表示保存生成的图片,./tra1plot这里写自己保存的地址;
–save_results ./tra1.zip表示保存计算结果,./tra1.zip这里写自己保存的地址;

③evo_rpe 计算相对位姿误差
相对位姿误差不进行绝对位姿的比较,相对位姿误差比较运动(姿态增量)。相对位姿误差可以给出局部精度,例如SLAM系统每米的平移或者旋转漂移量。这里还是以kitti为例,tum和euroc格式相同。
evo_rpe kitti ground_truth.txt tra1.txt -r full -va --plot --plot_mode xyz --save_plot ./tra1plot --save_results ./tra1.zip
kitti表明处理的是kitti数据集的相关结果,这里也可以替换为tum和euroc;
ground_truth.txt代表真实轨迹的数据;
tra1.txt代表估计轨迹的数据;
-r full表示同时考虑旋转和平移误差得到的ape,无单位(unit-less);
另外:
-r trans_part表示考虑平移部分得到的ape,单位为m;
-r rot_part表示考虑旋转部分得到的ape,无单位(unit-less);
-r angle_deg表示考虑旋转角得到的ape,单位°(deg);
-r angle_rad表示考虑旋转角得到的ape,单位弧度(rad);
-va包含两部分;
1.-v或–verbose指明输出文件数据的相关信息;2.-a或–align指明对轨迹进行配准;
–plot表示画图;
–plot_mode xy表示图像投影在xoy平面上,其余可选参数为:xz,yx,yz,zx,zy,xyz;
–save_plot ./tra1plot表示保存生成的图片,./tra1plot这里写自己保存的地址;
–save_results ./tra1.zip表示保存计算结果,./tra1.zip这里写自己保存的地址;

最终结果

我用的是09数据集,也就是kitti_2011_09_30_drive_0033_synced.bag,跑完之后把生成的myRes.txt与之前我们下的odometry ground truth poses 解压出来的09.txt拷贝到同个文件夹下,然后运行:

evo_traj kitti myRes.txt --ref=09.txt -p --plot_mode=xz

得到:
在这里插入图片描述就成功啦,吃饭去了:D

这篇关于如何使用evo工具评估LeGO-LOAM跑KITTI数据集的结果的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/164374

相关文章

Python获取中国节假日数据记录入JSON文件

《Python获取中国节假日数据记录入JSON文件》项目系统内置的日历应用为了提升用户体验,特别设置了在调休日期显示“休”的UI图标功能,那么问题是这些调休数据从哪里来呢?我尝试一种更为智能的方法:P... 目录节假日数据获取存入jsON文件节假日数据读取封装完整代码项目系统内置的日历应用为了提升用户体验,

vue使用docxtemplater导出word

《vue使用docxtemplater导出word》docxtemplater是一种邮件合并工具,以编程方式使用并处理条件、循环,并且可以扩展以插入任何内容,下面我们来看看如何使用docxtempl... 目录docxtemplatervue使用docxtemplater导出word安装常用语法 封装导出方

Linux换行符的使用方法详解

《Linux换行符的使用方法详解》本文介绍了Linux中常用的换行符LF及其在文件中的表示,展示了如何使用sed命令替换换行符,并列举了与换行符处理相关的Linux命令,通过代码讲解的非常详细,需要的... 目录简介检测文件中的换行符使用 cat -A 查看换行符使用 od -c 检查字符换行符格式转换将

使用Jackson进行JSON生成与解析的新手指南

《使用Jackson进行JSON生成与解析的新手指南》这篇文章主要为大家详细介绍了如何使用Jackson进行JSON生成与解析处理,文中的示例代码讲解详细,感兴趣的小伙伴可以跟随小编一起学习一下... 目录1. 核心依赖2. 基础用法2.1 对象转 jsON(序列化)2.2 JSON 转对象(反序列化)3.

使用Python实现快速搭建本地HTTP服务器

《使用Python实现快速搭建本地HTTP服务器》:本文主要介绍如何使用Python快速搭建本地HTTP服务器,轻松实现一键HTTP文件共享,同时结合二维码技术,让访问更简单,感兴趣的小伙伴可以了... 目录1. 概述2. 快速搭建 HTTP 文件共享服务2.1 核心思路2.2 代码实现2.3 代码解读3.

Elasticsearch 在 Java 中的使用教程

《Elasticsearch在Java中的使用教程》Elasticsearch是一个分布式搜索和分析引擎,基于ApacheLucene构建,能够实现实时数据的存储、搜索、和分析,它广泛应用于全文... 目录1. Elasticsearch 简介2. 环境准备2.1 安装 Elasticsearch2.2 J

使用C#代码在PDF文档中添加、删除和替换图片

《使用C#代码在PDF文档中添加、删除和替换图片》在当今数字化文档处理场景中,动态操作PDF文档中的图像已成为企业级应用开发的核心需求之一,本文将介绍如何在.NET平台使用C#代码在PDF文档中添加、... 目录引言用C#添加图片到PDF文档用C#删除PDF文档中的图片用C#替换PDF文档中的图片引言在当

Java中List的contains()方法的使用小结

《Java中List的contains()方法的使用小结》List的contains()方法用于检查列表中是否包含指定的元素,借助equals()方法进行判断,下面就来介绍Java中List的c... 目录详细展开1. 方法签名2. 工作原理3. 使用示例4. 注意事项总结结论:List 的 contain

C#使用SQLite进行大数据量高效处理的代码示例

《C#使用SQLite进行大数据量高效处理的代码示例》在软件开发中,高效处理大数据量是一个常见且具有挑战性的任务,SQLite因其零配置、嵌入式、跨平台的特性,成为许多开发者的首选数据库,本文将深入探... 目录前言准备工作数据实体核心技术批量插入:从乌龟到猎豹的蜕变分页查询:加载百万数据异步处理:拒绝界面

Android中Dialog的使用详解

《Android中Dialog的使用详解》Dialog(对话框)是Android中常用的UI组件,用于临时显示重要信息或获取用户输入,本文给大家介绍Android中Dialog的使用,感兴趣的朋友一起... 目录android中Dialog的使用详解1. 基本Dialog类型1.1 AlertDialog(