kitti数据包的使用

kitti开源数据集的官网：http://www.cvlibs.net/datasets/kitti/eval_odometry.php

我使用的就是官网封面的包，下载下来后，先查看每个包里面都有什么，因为笔者本次只跑了00以作示例，将下载后的包凑凑拼拼，组成一个文件夹，里面包含这些文件（本次示例不使用IMU）：

dataset_folder|____results|____00.txt|____sequences|_____00|___image_0|___image_1|___velodyne|___times.txt|___calib.txt

打开文件可以查看到kitti的bag保存形式为.bin形式，这对于ros环境不是特别友好，因此我们就得将kitti的.bin转换为.bag，就可以直接使用rosbag的命令直接跑了，比较方便。因此需要写一个代码来进行转换。当然此类代码就比较多了，在这里笔者介绍笔者接触到的两个代码：

lidar2rosbag_KITTI

开源代码：https://github.com/AbnerCSZ/lidar2rosbag_KITTI
阅读README，进行下载编译，按照要求将所需要的文件放置在对应的文件夹即可，然后运行指定的命令：

└── dataset└── sequences└── 04├── calib.txt├── image_0 [271 entries exceeds filelimit, not opening dir]├── image_1 [271 entries exceeds filelimit, not opening dir]├── times.txt└── velodyne [271 entries exceeds filelimit, not opening dir]

运行格式：
rosrun lidar2rosbag lidar2rosbag KITTI_input_dir output_name
示例：
rosrun lidar2rosbag lidar2rosbag /data/KITTI/dataset/sequences/04/ 04
或者
rosrun lidar2rosbag lidar2rosbag /data/KITTI/dataset/sequences/01/ bag01

生成的bag信息为：

这里突出介绍一下这个代码的优点和缺点：
优点：这个代码将点云数据的时间戳直接跟kitti数据集的时间戳对应，即从0s开始，一直到470s
缺点：这个代码生成的bag里面的话题只有点云：/velodyne_points，没有其他的话题了。

另外：有些人觉得生成bag文件会占用一些空间，因此就不生成bag了，直接在读取.bin文件时转换成PointCloud2进行发布话题，具体的可以参考关于KITTI数据中点云bin文件转成rosbag包的方法

kitti-lego-loam

代码：https://github.com/Mitchell-Lee-93/kitti-lego-loam

这个code里面其实是两个包，一个是将kitti数据集转换成bag，另一个就是修改了一点的lego-loam代码：1.修改雷达参数（kitti数据集的lidar是64线）2.关闭了运动补偿 3.为了评测，保存了位姿数据。

这里在转换成bag时，需要注意一下文件的存放顺序：

dataset_folder|____results|____00.txt|____sequences|_____00|___image_0|___image_1|___velodyne|___times.txt|___calib.txt

在运行kittibag时，记得修改kittibag.launch里的内容：

然后运行

roslaunch kittibag kittibag.launch

有可能会出现下面这个错误：

经过我查寻解决方案后，得到的解决方案：
第一次解决：将roscore关掉，重新再打开。
第二次解决：将times.txt文件的开始时间比0大一点点（不能为0）

生成bag的信息为：

这里可能是因为使用了ROS::Time()的原因导致这个包有626s跟470s不一样。

然后介绍一下这种方式的优点和缺点：
1.优点：话题里除了lidar的点云较上一种方法，还包含了相机和真值话题，我很喜欢！
2.缺点：生成bag文件的时间戳是当时转换bag时的时间戳ROS::Time()，因此在rosbag play --clock 00.bag时，lego-loam的rviz不会显示当前正在构建的地图（时间不一致）
因此，笔者结合这两种方法，修改了第二种方法，解决了第二种方法的缺点，得到了我想要的bag。具体的修改方式为：只修改第164行代码为如下形式：

        if (to_bag){bag_out.write("/image_left", ros::Time().fromSec(timestamp), image_left_msg);bag_out.write("/image_right", ros::Time().fromSec(timestamp), image_right_msg);bag_out.write("/kitti/velo/pointcloud", ros::Time().fromSec(timestamp), laser_cloud_msg);bag_out.write("/path_gt", ros::Time().fromSec(timestamp), pathGT);bag_out.write("/odometry_gt", ros::Time().fromSec(timestamp), odomGT);// bag_out.write("/image_left", ros::Time::now(), image_left_msg);// bag_out.write("/image_right", ros::Time::now(), image_right_msg);// bag_out.write("/kitti/velo/pointcloud", ros::Time::now(), laser_cloud_msg);// bag_out.write("/path_gt", ros::Time::now(), pathGT);// bag_out.write("/odometry_gt", ros::Time::now(), odomGT);}

修改过后再次生成的bag信息为：

这次时间就是从0开始了，然后直接跑lego-loam就可以在rviz中看到在线建立的图像了。

但是当我跑lego-loam时，几乎很大的概率会炸，就是漂的离谱的那种，乱七八糟的：

先不说回环不回环的问题，我看的别人跑的不使用回环效果都比这个好，比如这里，但是还有这里他也漂的离谱，我觉得不是加没加回环的问题，因为，我测试过，加回环也是这样。哈哈哈哈哈。啊呸，怎么笑的出来的。。。

经过我一番检测问题，找到了问题所在，就是我rosbag play时，中间会卡几下，严重的中间直接给我卡没了40s，40s的数据没了，再TM牛逼的代码估计也不好使吧。于是我心里想着，是不是电脑不行了（得找老板要台新电脑了）。。。 但是，这电脑也不能下一秒就到吧，但是我这科研绝不能因为一台电脑耽误，因此只能另寻方法（实则是比较卑微，不敢要电脑）。

于是，参考了大文件rosbag播放太慢问题解决，虽然题主说的不是我这个问题，但是题主说的问题，我也遇到了，同时bag文件太大了不得不压缩压缩，因此使用下面这条命令将bag进行了压缩。

rosbag compress --lz4 *.bag

压缩后的bag信息如下图所示：确实小了很多

我使用压缩后的bag跑lego-loam，确实好了些，但是不稳定，几乎每次跑都不一样，有时候偏的多一点，有时候就挺好的，不知道大家是不是这种情况，以至于，我觉得读取数据速度的原因，因为我是将bag文件放在机械硬盘里面的，后来我将bag放在了固态里面，稳定性又提升了一个台阶。后来经过与网友@琦琦酱_ 讨论得出内存不够，他用的是虚拟机，分配内存多一点就好了很多。

lego-loam运行

上面的跑包问题解决了，就直接上实验结果了。

跑包之前时，先对lego-loam进行一个小改动，
1，首先kitti-lego-loam已经对lego-loam修改了，即生成姿态信息，代码在transformFusion.cpp里面的line223。生成结果的路径需要在run.launch里修改【已修改，自己不用修改】

笔者想法：经过笔者实验这个生成的姿态不是优化后的姿态，优化后的姿态不能实时保存，而应该最后全部跑完时保存（因为最后可能有回环，会调整前面的位姿），因此这个位姿是实时的位姿，与GT对比时误差会比较大。

/added, cout results///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();//

2，修改：增加实时地图中路径的显示【未修改，需要自行添加】

在utility.h

#include <nav_msgs/Path.h>

在transformfusion.cpp

//在line48前后增加
nav_msgs::Path laserAfterMappedPath;//在line48前后增加
ros::Publisher pubLaserAfterMappedPath;//在line79前后增加
pubLaserAfterMappedPath = nh.advertise<nav_msgs::Path>("/aft_mapped_path", 100);//在line230前后增加
geometry_msgs::PoseStamped laserAfterMappedPose;
laserAfterMappedPose.header = laserOdometry2.header;
laserAfterMappedPose.pose = laserOdometry2.pose.pose;
laserAfterMappedPath.header.stamp = laserOdometry2.header.stamp;
laserAfterMappedPath.header.frame_id = "/camera_init";       
laserAfterMappedPath.poses.push_back(laserAfterMappedPose);        
pubLaserAfterMappedPath.publish(laserAfterMappedPath);

笔者想法：这一步修改是参考了LeGO-LOAM运行kitti数据集，作者说是优化后的路径显示，其实经过我自己的实验，以及代码查看，认为这不是优化后的路径（我指的优化是回环检测），而是目前实时的路径。

实验结果如下：

lego-loam是否使用回环就在utility.h里面的line104里修改 loopClosureEnableFlag就可以。

无回环：

从这两张图片可以看到，当没有回环时，代码自己的轨迹与自行添加的实时轨迹一摸一样，而当加上回环时，就不一样了。

有回环：

这三张图中最左边的是实时的路径信息，最右边的是回环优化后的路径信息。中间是两者的结合。从最左边可以看到一些细节，就是路径突然变正，没错，那就是回环检测矫正。

rviz中显示gt轨迹和实时轨迹和优化后的轨迹

因为kittibag里面把真值路径和真值里程计也转化成了话题，因此在lego-loam运行时不显示它，就挺对不起这两个话题的。。。哈哈哈。
因此，在运行时就将gt、实时、优化三种轨迹全都画出来，进行实时对比。
首先，需要对代码进行更改，

更改1： 因为kittibag里面将真值输出时，frame_id为“/camera_init”，如果直接显示，会因为run.launch里面对camera_init到map进行了旋转，因此真值并不能与另外两个轨迹显示在一个平面，绕了某两个轴旋转了90°，因此，为了解决这个问题，就将bag里面gt的frame_id变成其他的，然后独自运行TF发布与map的坐标转换关系。首先更改bag里面frame_id有两种方法：
方法1： 直接在kittibag.cpp里面修改：

	    geometry_msgs::PoseStamped poseGT;poseGT.header = odomGT.header;pathGT.header.frame_id = "/path";   //添加这一行poseGT.pose = odomGT.pose.pose;pathGT.header.stamp = odomGT.header.stamp;pathGT.poses.push_back(poseGT);pubPathGT.publish(pathGT);

然后再次使用命令

roslaunch kittibag kittibag.launch

生成的bag里面pathGT的frame_id就是/path了（odomGT我没改，但修改方式类似）

方法2： 参考改变ros bag 中消息的frame_id 和话题名
运用下面这条命令：

rosrun bag_tools change_frame_id.py -t /path_gt -f /path -i 00.bag -o 00_path.bag

运行之前需要安装srv_tools，参考：http://wiki.ros.org/srv_tools

cd catkin_ws/src
mkdir srv_tools
cd srv_tools
git clone https://github.com/srv/srv_tools.git .
cd ../..
rosdep install --from-paths src --ignore-src --rosdistro kinetic # install dependencies
catkin_make

就可以更改/path_gt的frame_id了，我在这个过程中遇到了个小问题，就是我用的方法1更改的，第一次更改后frame_id在rviz中并没有变还是/camera_init，但是当我使用命令： rostopic echo /path_gt | grep frame_id 输出的frame_id是修改后的/path，我觉得是rviz没有反应过来，当我多试了几次，就是多生成几次包，然后就好了，rviz更新过来了。方法2我没有试过，应该可以。

更改2： 这里需要更改lego-loam里面的run.launch内容,

<launch><!--- Result dir -->  <param name="RESULT_PATH" type="string" value="/home/seu/wsh/study/study/re_00_loop.txt" /><!--- Sim Time --><param name="/use_sim_time" value="true" /><!--- Run Rviz--><node pkg="rviz" type="rviz" name="rviz" args="-d $(find lego_loam)/launch/test.rviz" /><!--- 下面这句话是添加的 --><node pkg="tf" type="static_transform_publisher" name="path_to_map"  args="0 0 0 0   0.04   0 /map    /path 10" /><!--- TF --><node pkg="tf" type="static_transform_publisher" name="camera_init_to_map"  args="0 0 0 1.570795   0        1.570795 /map    /camera_init 10" /><node pkg="tf" type="static_transform_publisher" name="base_link_to_camera" args="0 0 0 -1.570795 -1.570795 0        /camera /base_link   10" /><!--- LeGO-LOAM -->    <node pkg="lego_loam" type="imageProjection"    name="imageProjection"    output="screen"/><node pkg="lego_loam" type="featureAssociation" name="featureAssociation" output="screen"/><node pkg="lego_loam" type="mapOptmization"     name="mapOptmization"     output="screen"/><node pkg="lego_loam" type="transformFusion"    name="transformFusion"    output="screen"/></launch>

其实后面的参数6个0就可以，我觉得z稍微有一点点偏，所以加了个0.04弧度，看着差不多在一个平面就行了。

然后运行结果：

这个图中，绿色的线是实时轨迹，红色的线是真值，五彩的虚线是优化轨迹。

evo 评估

evo开源代码：https://github.com/MichaelGrupp/evo
evo 对姿态的数据格式有三种，分别为：TUM/EuRoC/Kitti
区别为：

也可以看官方的数据格式说明：https://github.com/MichaelGrupp/evo/wiki/Formats

在上边的修改1那里，增加的输出位姿信息，其输出的格式为kitti格式即SE(3)，而真值的格式也是kitti。当我使用输出的这个re_00.txt或者re_00_loop.txt与00.txt(真值)做对比时，问题出来了，能够使用evo_traj命令画图，但是当要计算绝对位姿误差时，就出现了问题：（evo命令的使用可以参考如何使用evo工具评估LeGO-LOAM跑KITTI数据集的结果、一种SLAM精度评定工具——EVO使用方法详解、用evo工具评估SLAM轨迹、SLAM精度测评——EVO等）

意思是位姿数量不对应，即真值里面的位姿为4541个，而生成的只有4524个。所以报错了。本来我想的是 直接在re_00_loop.txt里面把真值的最后17行添加进去，这样确实能运行了，不报错了。当我想把真值的最后17个数据删掉来跟我的输出位姿对应时，发现这样做会报错，难道它发现我动真值的数据了？哈哈。
这样做的话我意识到一个问题，他们这些位姿是怎么对齐的，并且kitti里面又没有时间戳。因此参考网上的这里，于是乎我想到了解决方法，就是将真值的kitti格式转换成TUM格式，这个格式是带时间戳的。然后自行修改kitti-lego-loam里面的代码生成TUM格式的位姿。这样子 即使数量不对应依旧可以进行计算绝对位姿误差(因为带有时间戳了)

将kitti真值转换成TUM格式

准备文件：序列真值轨迹+序列times文件，如00.txt和00序列的times.txt（位于sequences/00/文件夹中）

采用工具：evo中自带的转换程序，evo中contrib文件夹下的kitti_poses_and_timestamps_to_trajectory.py(源码里面才会有，源码在这里)

终端命令：

python kitti_poses_and_timestamps_to_trajectory.py results/00.txt sequences/00/times.txt results/tum_00_gt.txt

转换后的格式打开看一眼是8列，顺序为timestamp x y z q_x q_y q_z q_w

kitti-lego-loam输出TUM格式位姿

最开始，我是在修改1那里直接进行的修改，输出位姿信息，修改如下：

//在transformFusion.cpp里面的laserOdometryHandler函数里面line241左右
/added, cout results///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;/生成TUM格式位姿///需要增加的部分std::ofstream foutTum("/home/seu/wsh/study/study/re_00_tum.txt", std::ios::app);foutTum.setf(std::ios::scientific, std::ios::floatfield);foutTum.precision(6);//foutTum << fixed;foutTum  << laserOdometry2.header.stamp << " " << laserOdometry2.pose.pose.position.x << " " << laserOdometry2.pose.pose.position.y << " " << laserOdometry2.pose.pose.position.z << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl;foutTum.close();
/生成TUM格式位姿结束/需要增加的部分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;}

这个之间的部分为增加的部分

但是注意到这个输出的是实时位姿，并不是优化后的位姿，因为这个输出是一直在输出，而优化后的输出应该跑完整个过程再输出（因为最后可能有回环，会调整之前的位姿），因此这里只是简单的增加一个输出，下面增加一个优化后的位姿输出，然后一会我们把这两种位姿都计算一下绝对位姿误差就看出来了。参考LeGo-LOAM运行KITTI数据集

增加优化后的位姿输出TUM格式：

//在mapOptmization.cpp文件里面的visualizeGlobalMapThread函数里面进行增加void visualizeGlobalMapThread(){ros::Rate rate(0.2);while (ros::ok()){rate.sleep();publishGlobalMap();}// save final point cloud
//生成优化后的TUM位姿///std::cout << "start save final point cloud" << std::endl;std::cout << "======================================================" << std::endl;ofstream foutTUM;foutTUM.open("/home/seu/wsh/study/study/re_00_TUM.txt");foutTUM << fixed;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 trajfoutTUM << 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;}foutTUM.close();生成优化后的TUM位姿pcl::io::savePCDFileASCII(+"finalCloud.pcd", *globalMapKeyFramesDS);string cornerMapString = "/tmp/cornerMap.pcd";string surfaceMapString = "/tmp/surfaceMap.pcd";string trajectoryString = "/tmp/trajectory.pcd";

这个代码不会一直运行，只会最后，当你在kitti-lego-loam运行终端里按一次Ctrl + C时才会执行。按两次会保存不全，因此按一次就可以了。生成完后会自动停止。

re_00.txt为kitti格式位姿
re_00_tum.txt为tum格式实时位姿
re_00_TUM.txt为tum格式优化位姿

re_00_tum.txt轨迹图：

evo_traj tum --ref=tum_00_gt.txt re_00_tum.txt -va --plot --plot_mode xz

re_00_TUM.txt轨迹图：

evo_traj tum --ref=tum_00_gt.txt re_00_TUM.txt -va --plot --plot_mode xz

我们计算一下re_00_tum.txt绝对位姿误差：
(另外说一下，我安装完evo后需要在python3.6的环境下运行，不然会报错，计算完绝对误差之后，在换回2.7，因为其他的比如catkin_make需要2.7环境)


re_00_TUM.txt绝对位姿误差：

可以看到基本上一样，这是为什么呢，因为这是没有添加回环的位姿，所以两条曲线基本一样，然后我们测一下加回环的绝对位姿误差：

re_00_tum_loop.txt轨迹图：

evo_traj tum --ref=tum_00_gt.txt re_00_tum_loop.txt -va --plot --plot_mode xz

re_00_TUM_loop.txt轨迹图：

evo_traj tum --ref=tum_00_gt.txt re_00_TUM_loop.txt -va --plot --plot_mode xz

re_00_tum_loop.txt绝对位姿误差：

evo_ape tum tum_00_gt.txt re_00_tum_loop.txt -r full -va --plot --plot_mode xz

从最后的图看到每到回环的时候，误差就会大，因为优化位姿回环了，实时位姿直接靠了上去，产生了较大误差。

re_00_TUM_loop.txt绝对位姿误差：

evo_ape tum tum_00_gt.txt re_00_TUM_loop.txt -r full -va --plot --plot_mode xz

综合考虑，优化位姿加回环是误差最小的情况，不得不说lego-loam是🐂🍺的。

好了，这次学习就到这里！希望能对大家有所帮助。

参考

1.如何使用evo工具评估LeGO-LOAM跑KITTI数据集的结果
2.使用LeGO-LOAM运行KITTI数据集
3.LeGo-LOAM运行KITTI数据集
4.kitti-lego-loam
5.LeGO-LOAM运行kitti数据集
6.evo
7.使用evo及kitti_odometry_evaluation测评slam轨迹
8.一种SLAM精度评定工具——EVO使用方法详解
9.用evo工具评估SLAM轨迹
10.SLAM精度测评——EVO
11.大文件rosbag播放太慢问题解决
12.kitti真值轨迹由kitti格式转为tum格式
13.KITTI数据集基准、转换成tum以及十个groundtruth对应图
14.rosbag中–clock的使用
15.使用自己的激光雷达/数据集运行lego_loam,修改代码教程