devbox
不正经
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

使用EVO评估工具评价LeGo-LOAM算法运行Kitti数据集_kitti数据集优化前后对比

作者：不正经 | 2024-05-01 15:29:13

赞

踩

kitti数据集优化前后对比

1.KITTI数据集使用

1.1kitti数据集下载

        官网链接：KITTI官网

        我们需要用到点云数据集和groundtruth，这里给出ground truth的00-10对应数据的名字（图片来自其他博主）

在这里插入图片描述

 

         由于点云数据集80G有点大，我们下载raw data中对应00-10对应包，这里我们选择06数据对于数据包，要下的是包的scnced+rectified data与calibration两项

 

        从官网下载时，有可能下载失败，这里建议使用命令行下载

wget https://s3.eu-central-1.amazonaws.com/avg-kitti/raw_data/2011_09_30_drive_0027/2011_09_30_drive_0027_sync.zip

1.2安装kitti2bag

pip install kitti2bag

        安装后的kitti2bag工具其实是.py文件，存在一定问题，会导致生成的.bag文件在运行时报错 Failed to find match for field  intensity 接下来，解决这个问题

whereis kitti2bag#找到kitti2bag.py文件的位置

打开文件，找到如下字段（197行）将"i"改为"intensity" ，保存并关闭文件

1.3 转换文件

        将2011_09_28_drive_0149_sync.zip解压在kitti文件夹，2011_09_28_calib.zip中为数据集的配置文件，文件目录应该以下图方式放置

在这里插入图片描述

        在2011_10_03文件夹的上一级目录，打开终端输入

kitti2bag -t 2011_10_03 -r 0027 raw_synced

        执行结束之后，会生成一个文件“kitti_2011_10_03_drive_0027_synced.bag”，这个就是使用KITTI数据集生成的bag文件了。 

2.修改LeGo-Loam文件

2.1utility.h

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

  1. // VLP-16
  2. //extern const int N_SCAN = 16;
  3. //extern const int Horizon_SCAN = 1800;
  4. //extern const float ang_res_x = 0.2;
  5. //extern const float ang_res_y = 2.0;
  6. //extern const float ang_bottom = 15.0+0.1;
  7. //extern const int groundScanInd = 7;
  8.  
  9. extern const int N_SCAN = 64;
  10. extern const int Horizon_SCAN = 2083;
  11. extern const float ang_res_x = 360.0/float(Horizon_SCAN);
  12. extern const float ang_res_y = 26.8/float(N_SCAN-1);
  13. extern const float ang_bottom = 24.8;
  14. extern const int groundScanInd = 55;

         修改imu Topic

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

        不过许多博主说，在运行过程中发现没有imu效果反而会更好，因为本身早期数据集imu数据的原因，会飘。。。。 

         除此之外，还需修改第60行的useCloudRing参数为false，否则你就会遇到这样的报错

Point cloud is not in dense format, please remove NaN points first!

2.2 imageProjection.cpp

我们需要在该cpp文件中修改雷达topic的名字

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

 2.3transformFusion.cpp

        该部分修改是为了在建图过程中生成轨迹

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

  1.     int init_flag=true;
  2.  
  3.     Eigen::Matrix4f H;
  4.     Eigen::Matrix4f H_init;
  5.     Eigen::Matrix4f H_rot;
  6.  
  7. 		
  8.     const string RESULT_PATH = "/media/cairui/Backup Plus/ubuntu20/KITTI/myRes.txt";//这里要写自己保存轨迹txt文件的路径

        在laserOdometryHandler函数修改如下：

  1.     void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry)
  2.     {
  3.         currentHeader = laserOdometry->header;
  4.  
  5.         double roll, pitch, yaw;
  6.         geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation;
  7.         tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);
  8.  
  9.         transformSum[0] = -pitch;
  10.         transformSum[1] = -yaw;
  11.         transformSum[2] = roll;
  12.  
  13.         transformSum[3] = laserOdometry->pose.pose.position.x;
  14.         transformSum[4] = laserOdometry->pose.pose.position.y;
  15.         transformSum[5] = laserOdometry->pose.pose.position.z;
  16.  
  17.         transformAssociateToMap();
  18.  
  19.         geoQuat = tf::createQuaternionMsgFromRollPitchYaw
  20.                   (transformMapped[2], -transformMapped[0], -transformMapped[1]);
  21.  
  22.         laserOdometry2.header.stamp = laserOdometry->header.stamp;
  23.         laserOdometry2.pose.pose.orientation.x = -geoQuat.y;
  24.         laserOdometry2.pose.pose.orientation.y = -geoQuat.z;
  25.         laserOdometry2.pose.pose.orientation.z = geoQuat.x;
  26.         laserOdometry2.pose.pose.orientation.w = geoQuat.w;
  27.         laserOdometry2.pose.pose.position.x = transformMapped[3];
  28.         laserOdometry2.pose.pose.position.y = transformMapped[4];
  29.         laserOdometry2.pose.pose.position.z = transformMapped[5];
  30.         pubLaserOdometry2.publish(laserOdometry2);
  31.  
  32.  
  33.         Eigen::Quaterniond q;
  34.  
  35.         q.w()=laserOdometry2.pose.pose.orientation.w;
  36.         q.x()=laserOdometry2.pose.pose.orientation.x;
  37.         q.y()=laserOdometry2.pose.pose.orientation.y;
  38.         q.z()=laserOdometry2.pose.pose.orientation.z;
  39.  
  40.         Eigen::Matrix3d R = q.toRotationMatrix();
  41.  
  42.         if (init_flag==true)
  43.         {
  44.  
  45.             H_init<< R.row(0)[0],R.row(0)[1],R.row(0)[2],transformMapped[3],
  46.                     R.row(1)[0],R.row(1)[1],R.row(1)[2],transformMapped[4],
  47.                     R.row(2)[0],R.row(2)[1],R.row(2)[2],transformMapped[5],
  48.                     0,0,0,1;
  49.  
  50.             init_flag=false;
  51.  
  52.             std::cout<<"surf_th : "<<surfThreshold<<endl;
  53.  
  54.         }
  55.  
  56.         H_rot<<	-1,0,0,0,
  57.                 0,-1,0,0,
  58.                 0,0,1,0,
  59.                 0,0,0,1;
  60.  
  61.         H<<  R.row(0)[0],R.row(0)[1],R.row(0)[2],transformMapped[3],
  62.                 R.row(1)[0],R.row(1)[1],R.row(1)[2],transformMapped[4],
  63.                 R.row(2)[0],R.row(2)[1],R.row(2)[2],transformMapped[5],
  64.                 0,0,0,1;
  65.  
  66.  
  67.  
  68.         H = H_rot*H_init.inverse()*H; //to get H12 = H10*H02 , 180 rot according to z axis
  69.  
  70.         std::ofstream foutC(RESULT_PATH, std::ios::app);
  71.  
  72.         foutC.setf(std::ios::scientific, std::ios::floatfield);
  73.         foutC.precision(6);
  74.  
  75.         //foutC << R[0] << " "<<transformMapped[3]<<" "<< R.row(1) <<" "<<transformMapped[4] <<" "<<  R.row(2) <<" "<< transformMapped[5] << endl;
  76.         for (int i = 0; i < 3; ++i)
  77.         {
  78.             for (int j = 0; j < 4; ++j)
  79.             {
  80.                 if(i==2 && j==3)
  81.                 {
  82.                     foutC <<H.row(i)[j]<< endl ;
  83.                 }
  84.                 else
  85.                 {
  86.                     foutC <<H.row(i)[j]<< " " ;
  87.                 }
  88.  
  89.             }
  90.         }
  91.  
  92.         foutC.close();
  93.  
  94.  
  95.         laserOdometryTrans2.stamp_ = laserOdometry->header.stamp;
  96.         laserOdometryTrans2.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));
  97.         laserOdometryTrans2.setOrigin(tf::Vector3(transformMapped[3], transformMapped[4], transformMapped[5]));
  98.         tfBroadcaster2.sendTransform(laserOdometryTrans2);
  99.     }

        要注意，这里可以运行的话题名称为/kitti/oxts/imu与/kitti/velo/pointcloud 。

3.安装EVO工具 

pip install evo --upgrade --no-binary evo

        我用的是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

还有更多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这里写自己保存的地址；
————————————————
版权声明：本文为CSDN博主「国内知名女性谐星蔡某」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/qq_40297109/article/details/124961265

声明：本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：【wpsshop博客】
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号