roslaunch .launch
rosbag play your_dataset.bag -r 2.0

✅ 第二步：录制估计轨迹

bash

复制编辑

rosbag record -O traj_only.bag /aft_mapped_to_init 

运行一段时间后 Ctrl+C 停止，生成 traj_only.bag

第三步：提取估计轨迹和真值轨迹为 CSV

bash

复制编辑

rostopic echo -b traj_only.bag -p /aft_mapped_to_init > traj.txt rostopic echo -b your_dataset.bag -p /leica/pose/relative > gt_raw.txt

✅ 第四步：转换为 TUM 格式（8列，空格分隔）

python3 convert_traj_raw_to_tum.py traj.txt traj_tum.txt

python3 convert_gt_raw_to_tum.py gt_raw.txt gt_tum.txt

✅ 第五步：对齐估计轨迹的时间戳（手动）

head -n 1 gt_tum.txt
head -n 1 traj_tum.txt

offset = 1746711524.090586901 - 1609059060.110306978 ≈ 137652463.98028

python3 align_traj_time.py traj_tum.txt traj_tum_aligned.txt 137652463.98028

第六步：使用 evo 评估误差（最终步骤）

bash

复制编辑

python3 -m evo.main_ape tum gt_tum.txt traj_tum_aligned.txt --align --plot