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

下载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
1

把上一步中的数据集解压并如下合并：

（那个bag文件是我生成的，不用管）
然后在2011_09_30所在文件夹（即当前文件夹下）运行：

kitti2bag -t 2011_09_30 -r 0018 raw_synced
1

注意标号一定是要和你下的数据集的号对应，比如我这里就是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;
1
2
3
4
5
6
7
8
9
10
11
12
13
14

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

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

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

imageProjection.cpp

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

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

或者直接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";
1
2
3
4
5
6
7
8

这个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 axis

        std::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);
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99

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

安装evo

先安装依赖：

pip3 install --user numpy 
pip3 install --user scipy 
pip3 install --user matplotlib
1
2
3

git源代码并安装：

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

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

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

出现下图就说明安装成功了。

这里顺便记录一些常用的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
1

得到：
就成功啦，吃饭去了：D