ORB-SLAM2描述轨迹误差（ROS）

一、轨迹输出函数

SaveTrajectoryTUM：时间戳-位移-四元数（每一帧）

SaveTrajectoryKITTI：时间戳-位移-变换矩阵（每一帧）

SaveKeyFrameTrajectoryTUM/SaveTrajectoryKITTI：关键帧的相机轨迹

二、KeyFrameTrajectory_TUM_Format.txt关键帧轨迹内容分析

  该文件记录了相机从开始到结束的每一帧轨迹，第一帧是世界坐标系的坐标原点。

时间戳 tx ty tz qx qy qz qw

tx、ty、tz为平移矩阵；

qx、qy、qz、qw为四元数记录的旋转矩阵。

三、安装EVO软件

由于系统的python版本为2.7，所以安装evo的版本为1.12.0。

查看evo版本：

evo pkg --version

四、使用轨迹评估软件EVO计算slam精度

使用EUROC官方数据集V1_01_easy.bag，groundtruth格式为：

timestamp tx ty tz qw qx qy qz

而slam输出的数据文件FrameTrajectory_TUM_Format.txt为TUM格式（timestamp tx ty tz qx qy qz qw），在/home/ybl/ORB_SLAM2/V1_01_easy/mav0/state_groundtruth_estimate0路径下打开终端使用如下命令：

evo_traj euroc data.csv --save_as_tum

可将 EUROC数据集真值的格式转换为tum数据集格式，自动保存为data.tum，要将该文件重新命名为data_tum.txt，否则后续进行画图时会报错！！！。

然后将data_tum.txt与，计算值与真实值进行比较，得到精度等指标

evo_ape tum  data_tum.txt FrameTrajectory_TUM_Format.txt -va --plot

 

若画图命令执行后终端出现该问题：

[ERROR] found no matching timestamps between reference and ORBSLAM3.txt with max. time diff 0.01 (s) and time offset 0.0 (s)

意思是计算得出的轨迹与真值轨迹的时间戳对不上，我自己的生成轨迹时间戳保留了小数点后6位，而rgbd_dataset_freiburg1_rpy和rgbd_dataset_freiburg1_desk数据集的时间戳则保留小数点后四位。

解决办法：将源程序中的System.cc中的System::SaveTrajectoryTUM函数的语句进行修改，保留四位有效数字。如下：

//f << setprecision(6) << *lT << " " <<  setprecision(9) << twc.at<float>(0) << " " << twc.at<float>(1) << " " << twc.at<float>(2) << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl;
f << setprecision(4) << *lT << " " <<  setprecision(9) << twc.at<float>(0) << " " << twc.at<float>(1) << " " << twc.at<float>(2) << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl;

slam精度评估标准：

绝对轨迹误差ATE：估计位姿与真实位姿的直接差值。

相对位姿误差RPE：相隔固定时间差，两帧位姿差的精度（相比真实位姿)，相当于直接测量里程计的误差。

五、获得gazebo仿真环境中机器人运行轨迹的真值

首先在机器人的模型配置文件中加入获得位姿的插件libgazebo_ros_p3d，我自己的配置文件的路径为/home/ybl/.gazebo/models/stewart_tui/model.sdf，插入如下代码

<plugin name="p3d_base_controller" filename="libgazebo_ros_p3d.so">
		<alwaysOn>true</alwaysOn>
		<updateRate>50.0</updateRate>
		<bodyName>laser</bodyName>
		<topicName>base_pose_ground_truth</topicName>
		<gaussianNoise>0.0</gaussianNoise>
		<frameName>map</frameName>
		<xyzOffsets>0 0 0</xyzOffsets>
		<rpyOffsets>0 0 0</rpyOffsets>
	</plugin>

并保存该文件。

然后启动仿真环境并运行机器人

cd Robot_ws/
source ./devel/setup.bash
roslaunch test_model stewart_new_shinei.launch

cd demo06_ws/
source ./devel/setup.bash 
rosrun plumbing_pub_sub main

此时，使用rostopic list可查看所有话题

输入

rostopic echo /base_pose_ground_truth

可查看话题的具体内容

现在的问题是如何把真实轨迹的有用的数据保存下来，存成tum数据集的格式

网上方法，先录制仿真过程的话题，形成bag包（该bag包里只有一个话题），然后从包里提取位姿等有用信息，

rostopic info /base_pose_ground_truth //查看topic的相关信息

/base_pose_ground_truth该话题的消息类型为nav_msgs/Odometry

（1）将topic话题录制为bag包并保存

//将/base_pose_ground_truth话题以groundtruth+时间的名字保存为bag包
rosbag recor-o groundtruth.bag /base_pose_ground_truth 

（2）将groundtruth_2024-02-29-16-04-42.bag里的base_pose_ground_truth话题的需要用到的数据提取出来并保存为example.txt文件，用Python脚本来实现

#!/usr/bin/python2
import rospy
import rosbag
#for sensor_msgs.msg import NavSatFix

def read_bag_gps(bag_file,output_file):
    with rosbag.Bag(bag_file,'r')as bag:
        with open(output_file,'w')as f:
            for topic,msg,t in bag.read_messages(topics=['/base_pose_ground_truth']):
                timestr="%.6f"% msg.header.stamp.to_sec()
                tx=msg.pose.pose.position.x
                ty=msg.pose.pose.position.y
                tz=msg.pose.pose.position.z
                qw=msg.pose.pose.orientation.w
                qx=msg.pose.pose.orientation.x
                qy=msg.pose.pose.orientation.y
                qz=msg.pose.pose.orientation.z
                #lat=msg.latitude
                #lon=msg.longitude

               
                f.write("{} {} {} {} {} {} {} {}\n".format(timestr,tx,ty,tz,qw,qx,qy,qz))

if __name__=='__main__':
    bag_file='groundtruth_2024-02-29-16-04-42.bag'
    output_file='example.txt'
    read_bag_gps(bag_file,output_file)