hdl-graph-slam怎么保存tum格式轨迹

引自免费微信小程序：皆我百晓生

在使用HDL Graph-SLAM进行SLAM后，要将路径数据保存为TUM格式，并使用EVO进行评估，你需要按照以下步骤操作：

1. 安装依赖库：

   • 首先确保已经安装了Eigen、g2o、Ceres Solver等必要的库。
   • 为了使用EVO，你需要安装EVO工具包。可以通过以下命令在GitHub上克隆仓库并构建：

     git clone https://github.com/paolomonti/Evo.git
     cd Evo
     mkdir build
     cd build
     cmake ..
     make
     

2. 修改HDL Graph-SLAM源码：

   • 打开HDL Graph-SLAM的源码，找到负责输出轨迹的部分（通常是 slam_system.cpp 或者 slam_system.h）。你需要添加一个函数来导出轨迹到TUM格式。

3. 编写TUM格式输出函数： 在slam_system.h中添加一个新的函数声明，例如：

   void saveTrajectoryToTUM(const std::string& filename, const Eigen::Matrix4d& pose);
   

   然后在slam_system.cpp文件中实现这个函数，内容如下：

   void slam_system::saveTrajectoryToTUM(const std::string& filename, const Eigen::Matrix4d& pose) {
       std::ofstream output(filename);
       if (!output.is_open()) {
           throw std::runtime_error("Failed to open file for writing.");
       }
   
       // 假设你有一个包含所有关键帧姿态的向量 poses_
       int num_keyframes = poses_.size();
       for (int i = 0; i < num_keyframes; ++i) {
           Eigen::Matrix4d current_pose = poses_[i];
           Eigen::Vector3d position(current_pose(0, 3), current_pose(1, 3), current_pose(2, 3));
           Eigen::Quaterniond orientation(current_pose.block<3, 3>(0, 0));
   
           // 写入TUM格式
           output << "timestamp," << i << "," << position.x() << "," << position.y() << "," << position.z() << "," << orientation.w() << "," << orientation.x() << "," << orientation.y() << "," << orientation.z() << "\n";
       }
       output.close();
   }
   

4. 调用新函数保存轨迹： 在你的主程序中，在完成SLAM后调用这个新函数来保存轨迹，例如：

   slam_system slam;
   // ... 运行SLAM算法 ...
   slam.saveTrajectoryToTUM("trajectory.txt", slam.getLatestPose());
   

5. 使用EVO进行评估：

   • 将刚刚保存的"trajectory.txt"文件复制到EVO的源码目录下的data文件夹中。
   • 执行以下命令运行EVO，评估SLAM结果：

     ./evo data/trajectory.txt --odom odom.txt --gt gt.txt --plot
     

   • odom.txt是你的odom数据（如果有的话），gt.txt是地面真实轨迹（如果有参考的话）。

这样就完成了HDL Graph-SLAM轨迹的TUM格式保存以及EVO评估的过程。注意，如果你的系统有其他特定的数据结构或接口，你可能需要根据实际情况调整上述代码。此外，确保你理解了每个步骤，并且对你的代码进行了适当的测试。

让阿豪来帮你解答，本回答参考chatgpt3.5编写提供，如果还有疑问可以评论或留言

解答： hdl-graph-slam是一个基于Lidar的SLAM算法，可以用于在室内或室外环境中进行地图构建和定位。如果要将hdl-graph-slam的轨迹保存为tum格式并使用EVO进行评估，则需要遵循以下步骤： 步骤一：修改hdl_localization.cpp文件，加入保存轨迹代码： 在hdl_localization.cpp文件中，找到LocalizationNode::spinOnce()函数，加入以下代码： ofstream f; f.open("/path/to/save/tum/trajectory.txt",std::ios::app); f << fixed << setprecision(6) << t << " " << p(0) << " " << p(1) << " " << p(2) << " " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << endl; f.close(); 其中，/path/to/save/tum/trajectory.txt是你要保存轨迹的文件路径。 步骤二：编译并运行hdl-graph-slam 使用以下命令编译hdl-graph-slam： catkin_make 然后运行hdl_localization节点： rosrun hdl_localization hdl_localization_node 步骤三：使用EVO进行评估 EVO是一个用于评估SLAM算法性能的工具，可以用来比较不同算法的性能。要使用EVO评估hdl-graph-slam生成的轨迹，需要先将轨迹转换为TUM格式。可以使用以下Python脚本将hdl-graph-slam生成的轨迹转换为TUM格式： import numpy as np f = open('/path/to/hdl/graph/slam/trajectory', 'r') lines = f.readlines() f.close() f = open('/path/to/hdl/graph/slam/trajectory_tum.txt', 'w') for i in range(len(lines)):

if i == 0:
    continue
data = lines[i].strip().split()
t = float(data[0])
x = float(data[1])
y = float(data[2])
z = float(data[3])
qw = float(data[4])
qx = float(data[5])
qy = float(data[6])
qz = float(data[7])
pos = [x, y, z]
quat = [qw, qx, qy, qz]
T = np.eye(4)
T[0:3,0:3] = quat_to_rotmat(quat)
T[0:3,3] = pos
line = "{} {:.9f} {:.9f} {:.9f} {:.9f} {:.9f} {:.9f} {:.9f}\n".format(i, t, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3])
f.write(line)

f.close() 其中，/path/to/hdl/graph/slam/trajectory是hdl-graph-slam生成的轨迹文件路径，/path/to/hdl/graph/slam/trajectory_tum.txt是转换后的TUM格式轨迹文件路径。 然后使用EVO工具评估轨迹，可以使用以下命令： evo_traj tum /path/to/hdl/graph/slam/trajectory_tum.txt --ref=groundtruth.tum --plot --save_plot plot.png 其中，/path/to/hdl/graph/slam/trajectory_tum.txt是上一步生成的TUM格式轨迹文件，groundtruth.tum是参考轨迹文件，--plot参数表示绘制轨迹图，--save_plot plot.png表示将轨迹图保存为plot.png文件。