使用evo测评SLAM算法可以是多角度的,例如轨迹评估,绝对位姿误差评估、相对位姿误差评估等,这些就交给读者自行研究,写这篇文章主要是帮助大家梳理一下A-LOAM算法评估流程。
1. KITTI数据集转bag
因为A-LOAM算法需要播放数据包,所以我们在运行A-LOAM算法时首先需要将KITTI数据集转成.bag文件。
关于KITTI数据中点云bin文件转成rosbag包的方法,大家用的最多的应该是被传得最多的阿里员工开发的转化工具lidar2rosbag_KITTI,github链接如下:
https://github.com/AbnerCSZ/lidar2rosbag_KITTI
安装就不讲了,直接创建工作空间和src文件夹,在src文件中git clone,然后在工作空间目录下catkin_make,使用前先source一下即可。
这里主要讲一下该功能包的使用,打开程序可以看到,读取KITTI数据集文件目录是写好了的:
所以按照这个目录去存储KITTI数据集即可,例如笔者在测评A-LOAM时用了KITTI05序列,相应的存储目录为:
其中,velodyne文件夹中存放点云.bin文件。
在进行使用时,需要先开启roscore。
然后,打开终端:
- source ./devel/setup.bash
- rosrun lidar2rosbag lidar2rosbag /home/xxx/data/KITTI/dataset/sequences/05/ 05
补充说明:第2行语句中,第一个05对应的是KITTI数据集序列号和存放目录号,第二个05是生成的.bag文件名,可以更改。
2. 保存A-LOAM发布的里程计话题数据(tum格式)
可以在A-LOAM算法中laserMapping文件中直接进行数据读取和写入操作,然后将当前帧添加到map坐标系后,发布的当前帧点云到map坐标下的里程计信息保存到.txt。
但笔者更倾向于订阅话题节点进行数据存储,今天在这里主要也是介绍这种方法。A-LOAM中需要订阅的里程计话题为“/aft_mapped_to_init”,所以我们需要订阅该话题,保存相应的时间戳和数据到.txt文件中去,因此保存的格式为tum(包含了时间戳)。
在这里,尊重原创,附上笔者当时借鉴的链接:
-
#include <iostream> #include <string> #include <fstream> #include <ros/ros.h> #include <nav_msgs/Odometry.h> using namespace std; //结合自己目录进行修改 string ALOAM_file = "/home/wangchao/odom_evo/ALOAM.txt"; fstream aloam_file; void aloam_handler(const nav_msgs::Odometry::ConstPtr& msgIn) { nav_msgs::Odometry data = *msgIn; static int flag=1; static double stamp_init; if(flag==1){ stamp_init = data.header.stamp.toSec(); flag=0; } aloam_file << fixed << data.header.stamp.toSec()-stamp_init << " " << data.pose.pose.position.x << " " << data.pose.pose.position.y << " " << data.pose.pose.position.z << " " << data.pose.pose.orientation.x << " " << data.pose.pose.orientation.y << " " << data.pose.pose.orientation.z << " " << data.pose.pose.orientation.w << std::endl; } int main(int argc, char** argv) { ros::init(argc, argv, "odom2txt2"); ros::NodeHandle nh; aloam_file.open(ALOAM_file, ios::out); ros::Subscriber sub_aloam = nh.subscribe<nav_msgs::Odometry>("/aft_mapped_to_init", 1000, &aloam_handler, ros::TransportHints().tcpNoDelay()); ROS_INFO("\033[1;32m----> odom2txt start.\033[0m"); ros::spin(); return 0; }
注意:请根据自己目录修改代码中写入文件的目录和文件名。
相应的CMakeLists.txt也比较简单,读者只需要像往常一样创建功能包,添加.cpp即可,创建功能包时依赖项为roscpp rospy std_msgs即可,然后没有的在CMakeLists.txt中添加即可,笔者的CMakeLists.txt文件直接参照A-LOAM的依赖项进行的:
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
geometry_msgs
nav_msgs
sensor_msgs
rosbag
image_transport
cv_bridge
tf
)
其他没有什么需要注意的,正常修改add_executable和target_link_libraries就行了。
3. evo评估A-LOAM
直接放上使用的语句:
语句1:
evo_traj tum --ref=gt05_tum.txt ALOAM.txt --plot --plot_mode=xyz --align --correct_scale
gt05_tum.txt为kitti真值tum格式文件。
参数说明:
- 自动对齐 --align 或者 -a
- 尺度校准 --correct_scale 或者 -s
- 自动对齐且尺度校准 --align --correct_scale 或者 -as
语句2:
evo_rpe tum ALOAM.txt gt05_tum.txt -r trans_part -va --plot --plot_mode xyz --save_plot /home/wangchao/kitti_result/05.pdf --save_results /home/wangchao/kitti_result/05.zip
语句2中指定了目录对运行结果进行保存,方便后续操作。
如果需要KITTI真值tum格式文件,可以使用这个链接:
链接: https://pan.baidu.com/s/17hvZyQ3yAw74H1JtJ3OvsQ?pwd=tsnm 提取码: tsnm