生成自己的rosbag运行轨迹文件用EVO绘制
一、EVO安装
pip install evo --upgrade --no-binary evo
依赖问题自行添加解决
二、rosbag轨迹生成
2.1 修改laserMapping.cpp代码
找到文件改代码
(我自己的)代码路径:/home/flycar/aloam_ws/src/A-LOAM/src/laserMapping.cpp
-----------源代码------------
nav_msgs::Odometry odomAftMapped;
odomAftMapped.header.frame_id = "/camera_init";
odomAftMapped.child_frame_id = "/aft_mapped";
odomAftMapped.header.stamp = ros::Time().fromSec(timeLaserOdometry);
odomAftMapped.pose.pose.orientation.x = q_w_curr.x();
odomAftMapped.pose.pose.orientation.y = q_w_curr.y();
odomAftMapped.pose.pose.orientation.z = q_w_curr.z();
odomAftMapped.pose.pose.orientation.w = q_w_curr.w();
odomAftMapped.pose.pose.position.x = t_w_curr.x();
odomAftMapped.pose.pose.position.y = t_w_curr.y();
odomAftMapped.pose.pose.position.z = t_w_curr.z();
pubOdomAftMapped.publish(odomAftMapped);
-----------添加代码------------
//***add evo***//
//根据自己的文件存放位置修改路径和文件名
std::ofstream pose1("/home/flycar/aloam_ws/src/A-LOAM/data1/college.txt", std::ios::app);
pose1.setf(std::ios::scientific, std::ios::floatfield);
//保存结果的精度,可调
pose1.precision(6);
//static double timeStart = odometryBuf.front()-> header.stamp.toSec();
//auto T1 = ros::Time().fromSec(timeStart);
pose1 << odomAftMapped.header.stamp << " "
<< odomAftMapped.pose.pose.position.x << " "
<< odomAftMapped.pose.pose.position.y << " "
<< odomAftMapped.pose.pose.position.z << " "
<< odomAftMapped.pose.pose.orientation.x << " "
<< odomAftMapped.pose.pose.orientation.y << " "
<< odomAftMapped.pose.pose.orientation.z << " "
<< odomAftMapped.pose.pose.orientation.w << std::endl;
pose1.close();
//***add evo***//
2.2 运行roslaunch+rosbag
编译后运行
catkin_make
source devel/setup.bash
roslaunch aloam_velodyne aloam_velodyne_VLP_16.launch
新建终端
rosbag play college.bag
就可以在路径下看到txt文件的生成,存放了tum格式的轨迹,包括时间戳,位置,姿态四元数。这里用的tum格式。
2.3 EVO绘制
cd /home/flycar/aloam_ws/src/A-LOAM/data1
evo_traj tum college.txt -p
其他命令
指标:
evo_ape- 绝对姿势错误
evo_rpe- 相对位姿误差
工具:
evo_traj- 用于分析、绘制或导出一条或多条轨迹的工具
evo_res- 用于比较一个或多个结果文件的工具 evo_ape或者 evo_rpe
evo_fig-(实验性)用于重新打开序列化图的工具(保存为 --serialize_plot)
evo_config- 全局设置和配置文件操作工具