| 187 | } |
| 188 | |
| 189 | void Publisher::saveTrajectory(const std::string& filename) const { |
| 190 | |
| 191 | // Save the trajectory to a text file |
| 192 | std::string txt_file = filename + ".txt"; |
| 193 | // Save the trajectory to a .ply file |
| 194 | std::string ply_file = filename + ".ply"; |
| 195 | |
| 196 | // Save .txt file |
| 197 | std::ofstream loop_path_file(txt_file, std::ios::out); |
| 198 | loop_path_file.setf(std::ios::fixed, std::ios::floatfield); |
| 199 | loop_path_file.precision(9); |
| 200 | loop_path_file << "#timestamp tx ty tz qx qy qz qw" << std::endl; |
| 201 | for (geometry_msgs::msg::PoseStamped keyframe_pose : loop_closure_traj_.poses) { |
| 202 | geometry_msgs::msg::Quaternion quat = keyframe_pose.pose.orientation; |
| 203 | geometry_msgs::msg::Point pos = keyframe_pose.pose.position; |
| 204 | loop_path_file << keyframe_pose.header.stamp.sec << "." << keyframe_pose.header.stamp.nanosec << " " << pos.x << " " |
| 205 | << pos.y << " " << pos.z << " " << quat.x << " " << quat.y << " " << quat.z << " " << quat.w |
| 206 | << std::endl; |
| 207 | } |
| 208 | loop_path_file.close(); |
| 209 | RCLCPP_INFO(node_->get_logger(), "Trajectory saved to: %s", txt_file.c_str()); |
| 210 | |
| 211 | // Save .ply file with positions |
| 212 | std::ofstream ply_ofs(ply_file, std::ios::out); |
| 213 | if (!ply_ofs.is_open()) { |
| 214 | RCLCPP_ERROR(node_->get_logger(), "Failed to open PLY file: %s", ply_file.c_str()); |
| 215 | return; |
| 216 | } |
| 217 | |
| 218 | // Write ASCII PLY header |
| 219 | ply_ofs << "ply\n"; |
| 220 | ply_ofs << "format ascii 1.0\n"; |
| 221 | ply_ofs << "element vertex " << loop_closure_traj_.poses.size() << "\n"; |
| 222 | ply_ofs << "property float x\n"; |
| 223 | ply_ofs << "property float y\n"; |
| 224 | ply_ofs << "property float z\n"; |
| 225 | ply_ofs << "end_header\n"; |
| 226 | |
| 227 | // Write vertex data (positions) |
| 228 | ply_ofs.setf(std::ios::fixed, std::ios::floatfield); |
| 229 | ply_ofs.precision(9); |
| 230 | for (const auto& keyframe_pose : loop_closure_traj_.poses) { |
| 231 | geometry_msgs::msg::Point pos = keyframe_pose.pose.position; |
| 232 | ply_ofs << pos.x << " " << pos.y << " " << pos.z << "\n"; |
| 233 | } |
| 234 | ply_ofs.close(); |
| 235 | RCLCPP_INFO(node_->get_logger(), "Trajectory PLY saved to: %s", ply_file.c_str()); |
| 236 | } |
| 237 | |
| 238 | bool Publisher::saveKeyframes(const std::string& filename, const std::vector<KeyframeDump>& keyframes) const { |
| 239 | |