生成自己的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- 全局设置和配置文件操作工具