diff --git a/apps/multi_session_registration/multi_session_registration.cpp b/apps/multi_session_registration/multi_session_registration.cpp index 56493b6f..3fde9b35 100644 --- a/apps/multi_session_registration/multi_session_registration.cpp +++ b/apps/multi_session_registration/multi_session_registration.cpp @@ -3755,6 +3755,109 @@ pose_tait_bryan_from_affine_matrix(m_src.inverse() * m_g); } std::cout << "Finished saving all trajectories to TUM TXT files." << std::endl; } + if (ImGui::MenuItem("Save all as TUM TXT (single folder)")) + { + for (size_t i = 0; i < project_settings.session_file_names.size(); ++i) + { + const auto& session_path = project_settings.session_file_names[i]; + + if (i >= sessions.size()) + { + std::cerr << "No loaded session for: " + << session_path << std::endl; + continue; + } + + Session& session = sessions[i]; + + std::filesystem::path session_dir = + std::filesystem::path(session_path).parent_path(); + + std::filesystem::path base_dir = + session_dir.parent_path(); + + std::filesystem::path output_dir = + base_dir / "all_tum_files"; + + try + { + std::filesystem::create_directories(output_dir); + } + catch (const std::exception& e) + { + std::cerr << "Failed to create export directory: " + << e.what() << std::endl; + continue; + } + + std::string folder_name = + session_dir.filename().string(); + + std::filesystem::path txt_path = + output_dir / (folder_name + "_trajectory_tum.txt"); + + std::cout << "Saving trajectory to TUM TXT: " + << txt_path << std::endl; + + try + { + std::ofstream outfile(txt_path); + + if (!outfile.is_open()) + { + std::cerr << "Failed to create file: " + << txt_path << std::endl; + continue; + } + + for (const auto& pc : session.point_clouds_container.point_clouds) + { + if (!pc.visible) + continue; + + for (const auto& traj : pc.local_trajectory) + { + Eigen::Affine3d pose = pc.m_pose * traj.m_pose; + + Eigen::Vector3d pos = pose.translation(); + + Eigen::Quaterniond q(pose.rotation()); + + double t_s = + static_cast(traj.timestamps.first) / 1e9; + + outfile << std::fixed + << std::setprecision(9) + << t_s << " " + << std::setprecision(10) + << pos.x() << " " + << pos.y() << " " + << pos.z() << " " + << q.x() << " " + << q.y() << " " + << q.z() << " " + << q.w() + << "\n"; + } + } + + outfile.close(); + + std::cout << "Saved: " + << txt_path << std::endl; + } + catch (const std::exception& e) + { + std::cerr << "Error creating " + << txt_path + << ": " + << e.what() + << std::endl; + } + } + std::cout << "Finished saving all trajectories to single folder." + << std::endl; + } ImGui::EndMenu(); }