LeGo-LOAM运行KITTI数据集

在上一篇博客：LeGO-LOAM编译和运行的最后，自己给自己挖了个坑。KITTI数据集的运行结果很明显有问题，接下来就要解决这个问题然后完美运行LeGO-LOAM。

一.KITTI数据集的转换

官方的KITTI数据集是连续的数据图片的形式，为了之后的各种测试的方便，先使用工具将KITTI数据集转换成rosbag的形式。

1. 安装kitti2bag
   kitti2bag项目地址

   pip install kitti2bag
   

2. 数据集的转换
   提前下载需要的kitti数据集，在项目中我使用的是2011-10-03-0027序列。

   //解压
   unzip 2011_10_03_drive_0027_sync.zip
   unzip 2011_10_03_calib.zip
   //转换
   kitti2bag -t 2011_10_03 -r 0027 raw_synced
   

转换之后会在对应的文件夹下生成kitti_2011_10_03_drive_0027_synced.bag
之后会用到的topic：

/kitti/oxts/imu
/kitti/velo/pointcloud

二、运行

这就是一个非常漫长的过程了。。。

---

最开始我一直认为是配置文件的问题，所以参照Velodyne官网的信息更改了新的配置文件。

// HDL-64E
extern const int N_SCAN = 64;
extern const int Horizon_SCAN = 1800;
extern const float ang_res_x = 360.0/float(Horizon_SCAN);
extern const float ang_res_y = 0.427;
extern const float ang_bottom = 24.9;
extern const int groundScanInd = 60;//lidar光束在地面下的数量

跑出来的结果还是一团糟。。。

---

然后去问了问周围的师兄师弟，KITTI数据集最开始的一段时间内IMU的数据是缺失的。
在配置文件里去掉IMU的topic信息

得到这个结果就花了我一天的时间。。。。

三、评估

跑这些经典项目的原因是想要对这些项目使用KITTI数据集时候生成的轨迹进行一个评估，所以最后要保存轨迹信息。
但是！！！
我保存轨迹又出错了QAQ

---

报错信息：

terminate called after throwing an instance of 'pcl::IOException'
  what():  : [pcl::PCDWriter::writeASCII] Input point cloud has no data!

度娘说是因为pcl点云保存的时候路径错误造成的问题，问题出现在所在函数的最开始部分

pcl::io::savePCDFileASCII(fileDirectory + "finalCloud.pcd", *globalMapKeyFramesDS);

我是实在搞不懂到底哪里出了问题，如果有人解决了请告诉我！

---

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

轨迹保存

欧拉角到四元数的转换参照博客，将原有函数改为（参考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();
}

评估结果

没有回环检测好像结果有点飘？？