LeGO-LOAM跑KITTI数据集评估方法【代码改】

评估

只需要轨迹信息，也就是时间戳，xyz和旋转四元数。可以只保存这部分的轨迹就可以。在LeGO-LOAM项目中mapOptmization.cpp文件，cloudKeyPoses6D中保存了一个点对应的位置信息和旋转信息。只需要将其中旋转的欧拉角转换为旋转四元数。

代码修改

1、添加轨迹函数

将原有函数改为（参考ORBSLAM3轨迹的保存）：

void visualizeGlobalMapThread(){
        ros::Rate rate(0.2);
        while (ros::ok()){
            rate.sleep();
            publishGlobalMap();
        }
        // save final point cloud
        std::cout << "start save final point cloud" << std::endl;
        std::cout << "======================================================" << std::endl;

        ofstream f;
        f.open("/home/l/data/lego_traj.txt");
        f << fixed;
        //std::cout << "traj roll" << cloudKeyPoses6D->points[0].roll << std::endl;
        for(size_t i = 0;i < cloudKeyPoses3D->size();i++)
        {
            float cy = cos((cloudKeyPoses6D->points[i].yaw)*0.5);
            float sy = sin((cloudKeyPoses6D->points[i].yaw)*0.5);
            float cr = cos((cloudKeyPoses6D->points[i].roll)*0.5);
            float sr = sin((cloudKeyPoses6D->points[i].roll)*0.5);
            float cp = cos((cloudKeyPoses6D->points[i].pitch)*0.5);
            float sp = sin((cloudKeyPoses6D->points[i].pitch)*0.5);

            float w = cy * cp * cr + sy * sp * sr;
            float x = cy * cp * sr - sy * sp * cr;
            float y = sy * cp * sr + cy * sp * cr;
            float z = sy * cp * cr - cy * sp * sr;
            //save the traj
            f << setprecision(6) << cloudKeyPoses6D->points[i].time << " " << setprecision(9) << cloudKeyPoses6D->points[i].x << " " << cloudKeyPoses6D->points[i].y << " " << cloudKeyPoses6D->points[i].z << " " << x << " " << y << " " << z << " " << w << endl;
        }

        f.close();
}

参考代码：
1、LeGo-LOAM运行KITTI数据集
2、LeGO-LOAM初探：原理，安装和测试
3、LeGO-LOAM初探：原理，安装和测试

2、参数修改

因为KITTI的激光雷达是64线所以为了充分保证64线的充分利用，需要要修改参数，位于utility.h中

修改代码如下：

// 注释掉原来的
// // 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;

// 添加新的
// KITTI的64线
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;

参照大佬：LegoLOAM在KITTI数据集中的参数配置

【注】：欧拉角到四元数的转换参照四元数与欧拉角（Yaw、Pitch、Roll）的转换。