From b8bea2171d1dfdfff8fc609fb3702aeebd821894 Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Fri, 14 Aug 2026 23:12:20 +0200 Subject: [PATCH 1/2] Add TUM trajectory support to Step 2, treated like GNSS Loads TUM-format trajectories (Core/tum.h/.cpp), renders them as GPU point dots via a new ScanRenderer::PointsGPU cache (upload once, draw many, avoiding per-frame VAO/VBO churn), and draws correspondence lines to each scan's nearest local_trajectory sample by timestamp (matching timestamps.first in nanoseconds against TUM's Unix-epoch-seconds timestamp, scaled by 1e9). Also adds a manual-loop-closure "Fuse trajectory with TUM (trajectory is rigid)" action mirroring the existing GNSS one, via a new PoseGraphLoopClosure::FuseTrajectoryWithTUM(). Co-Authored-By: Claude Sonnet 5 --- .../multi_view_tls_registration.h | 4 + .../multi_view_tls_registration_gui.cpp | 163 ++++++++++++++++++ .../multi_view_tls_registration.h | 4 + .../multi_view_tls_registration_gui.cpp | 1 + core/CMakeLists.txt | 1 + .../Core/manual_pose_graph_loop_closure.h | 2 + core/include/Core/pfd_wrapper.hpp | 1 + core/include/Core/pose_graph_loop_closure.h | 2 + core/include/Core/raylib_render.hpp | 31 ++++ core/include/Core/tum.h | 60 +++++++ core/src/manual_pose_graph_loop_closure.cpp | 6 + core/src/pose_graph_loop_closure.cpp | 130 ++++++++++++++ core/src/raylib_render.cpp | 68 ++++++++ core/src/tum.cpp | 138 +++++++++++++++ 14 files changed, 611 insertions(+) create mode 100644 core/include/Core/tum.h create mode 100644 core/src/tum.cpp diff --git a/apps/multi_view_tls_registration/multi_view_tls_registration.h b/apps/multi_view_tls_registration/multi_view_tls_registration.h index 0553abb5..b7a75c90 100644 --- a/apps/multi_view_tls_registration/multi_view_tls_registration.h +++ b/apps/multi_view_tls_registration/multi_view_tls_registration.h @@ -11,6 +11,7 @@ #include #include #include +#include namespace fs = std::filesystem; @@ -104,6 +105,9 @@ struct TLSRegistration // GNSS GNSS gnss; + // TUM trajectory, treated like a second GNSS-style external track + TUM tum; + // Loading bool calculate_offset; // Whether to calculate offset to point cloud on loading bool is_decimate = true; // Whether to decimate point clouds on loading diff --git a/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp b/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp index a02b01be..911b9bce 100644 --- a/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp +++ b/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp @@ -49,6 +49,7 @@ #include #include #include +#include #include #ifdef _WIN32 @@ -145,6 +146,7 @@ void renderLoopClosureLabels(PointClouds& point_clouds_container); void renderGroundControlPoints(const GroundControlPoints& ground_control_points, const PointClouds& point_clouds_container); void renderGroundControlPointsLabels(const GroundControlPoints& ground_control_points, const PointClouds& point_clouds_container); void renderGNSS(const GNSS& gnss, const PointClouds& point_clouds_container); +void renderTUM(const TUM& tum, const PointClouds& point_clouds_container); void renderControlPoints(const ControlPoints& control_points, PointClouds& point_clouds_container); void renderControlPointsLabels(const ControlPoints& control_points, const PointClouds& point_clouds_container); void display(); @@ -263,6 +265,7 @@ static bool show_demo_window = true; static bool show_another_window = false; bool gnssWithOffset = false; +bool tumSubtractFirstPose = false; // radio button selectors static int NDTnomSelection = 0; @@ -1202,6 +1205,7 @@ void loop_closure_gui() index_loop_closure_target, m_gizmo, tls_registration.gnss, + tls_registration.tum, session.ground_control_points, session.control_points, num_edge_extended_before, @@ -2739,6 +2743,94 @@ void renderGNSS(const GNSS& gnss, const PointClouds& point_clouds_container) } } +// TUM trajectories are treated like a second GNSS-style external track (see +// renderGNSS() above, whose structure this mirrors): a polyline through +// tum_poses plus, when show_correspondences is set, lines to the nearest +// local_trajectory sample of every loaded scan by timestamp. Unlike GNSS, +// TumPose::x/y/z are already Cartesian in the trajectory's own frame, so +// only the point_clouds_container offset is subtracted, with no ENU/PROJ +// conversion. +void renderTUM(const TUM& tum, const PointClouds& point_clouds_container) +{ + // Cached across frames -- re-uploaded only when tum_poses actually + // changed (tum.version) or the point cloud recentering offset shifted, + // not every frame (see ScanRenderer::PointsGPU's own comment for why + // that matters). + static ScanRenderer::PointsGPU tumPointsGPU; + static size_t tumPointsGPUVersion = SIZE_MAX; + static Eigen::Vector3d tumPointsGPUOffset = Eigen::Vector3d::Zero(); + + if (!tum.tum_poses.empty()) + { + bool stale = tumPointsGPUVersion != tum.version || !tumPointsGPUOffset.isApprox(point_clouds_container.offset, 1e-9); + if (stale) + { + std::vector positions; + positions.reserve(tum.tum_poses.size()); + for (const auto& p : tum.tum_poses) + { + positions.emplace_back( + p.x - point_clouds_container.offset.x(), + p.y - point_clouds_container.offset.y(), + p.z - point_clouds_container.offset.z()); + } + scan_renderer.uploadPoints(tumPointsGPU, positions); + tumPointsGPUVersion = tum.version; + tumPointsGPUOffset = point_clouds_container.offset; + } + scan_renderer.drawPoints(tumPointsGPU, YELLOW, tum.point_size); + } + + if (tum.show_correspondences) + { + rlBegin(RL_LINES); + rlColor3f(1.0f, 0.0f, 0.0f); + for (const auto& pc : point_clouds_container.point_clouds) + { + for (size_t i = 0; i < tum.tum_poses.size(); ++i) + { + // TUM timestamps are Unix-epoch seconds; local_trajectory's + // timestamps.first is the LIO trajectory CSV's + // "timestamp_nanoseconds" column -- Unix-epoch nanoseconds, + // same epoch, 1e9x the scale. timestamps.second + // ("timestampUnix_nanoseconds") looks like the more obvious + // match by name, but is 0 for every node unless that column + // was actually captured during LIO (commonly isn't), so + // matching against it silently finds nothing -- .first with + // the unit conversion below is the field that's actually + // populated. + double time_stamp_ns = tum.tum_poses[i].timestamp * 1.0e9; + + auto it = std::lower_bound( + pc.local_trajectory.begin(), + pc.local_trajectory.end(), + time_stamp_ns, + [](const PointCloud::LocalTrajectoryNode& lhs, const double& time) -> bool + { + return lhs.timestamps.first < time; + }); + + size_t index = static_cast(it - pc.local_trajectory.begin()); + + if (index > 0 && index < pc.local_trajectory.size()) + { + if (fabs(time_stamp_ns - pc.local_trajectory[index].timestamps.first) < 5.0e8) // 0.5s, in ns + { + auto m = pc.m_pose * pc.local_trajectory[index].m_pose; + rlVertex3f(static_cast(m(0, 3)), static_cast(m(1, 3)), static_cast(m(2, 3))); + + rlVertex3f( + static_cast(tum.tum_poses[i].x - point_clouds_container.offset.x()), + static_cast(tum.tum_poses[i].y - point_clouds_container.offset.y()), + static_cast(tum.tum_poses[i].z - point_clouds_container.offset.z())); + } + } + } + } + rlEnd(); + } +} + // Was ControlPoints::render() (core/src/control_points.cpp) -- legacy-GL, // compiled once into `core` and shared with the remaining GLUT apps, so it // can't be touched; reimplemented here. Two parts, like the original's @@ -3173,6 +3265,7 @@ void display() { renderGroundControlPoints(session.ground_control_points, session.point_clouds_container); renderGNSS(tls_registration.gnss, session.point_clouds_container); + renderTUM(tls_registration.tum, session.point_clouds_container); if (is_loop_closure_gui) renderLoopClosure( @@ -4168,6 +4261,64 @@ void display() if (ImGui::IsItemHovered()) ImGui::SetTooltip("GNSS (GPS, etc.) related open/save commands"); + if (ImGui::BeginMenu("TUM")) + { + ImGui::MenuItem("Subtract 1st pose transform -> move to (0,0,0)", nullptr, &tumSubtractFirstPose); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Re-express every pose relative to the first one, so the trajectory starts at " + "identity (0,0,0, no rotation) instead of the file's raw coordinates"); + + if (ImGui::MenuItem("Load TUM trajectory")) + { + std::vector input_file_names; + input_file_names = mandeye::fd::OpenFileDialog("Load TUM trajectory files", mandeye::fd::Tum_filter, true); + + if (input_file_names.size() > 0) + { + if (!tls_registration.tum.load_data_from_tum(input_file_names, tumSubtractFirstPose)) + { + spdlog::error("Error loading TUM trajectory files!"); + } + else + { + spdlog::info( + "point_clouds_container.offset = ({}, {}, {})", + session.point_clouds_container.offset.x(), + session.point_clouds_container.offset.y(), + session.point_clouds_container.offset.z()); + } + } + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Load a trajectory in the TUM RGB-D format (timestamp tx ty tz qx qy qz qw), treated like a GNSS track"); + + ImGui::BeginDisabled(tls_registration.tum.tum_poses.size() == 0); + if (ImGui::MenuItem("Center camera on TUM trajectory")) + { + Eigen::Vector3d centroid(0, 0, 0); + for (const auto& p : tls_registration.tum.tum_poses) + { + centroid += Eigen::Vector3d(p.x, p.y, p.z); + } + centroid /= static_cast(tls_registration.tum.tum_poses.size()); + centroid -= session.point_clouds_container.offset; + + app_state.new_rotation_center = centroid.cast(); + app_state.camera_transition_active = true; + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Jump the camera to the loaded TUM trajectory -- use this if the trajectory doesn't " + "appear where you expect it (e.g. it's far from the loaded point clouds)"); + ImGui::EndDisabled(); + + ImGui::EndMenu(); + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("TUM-format external trajectory open commands"); + ImGui::EndMenu(); } if (ImGui::IsItemHovered()) @@ -4549,6 +4700,18 @@ void display() } ImGui::EndDisabled(); + ImGui::BeginDisabled(tls_registration.tum.tum_poses.size() <= 0); + { + if (ImGui::BeginMenu("TUM GT Trajectory")) + { + ImGui::MenuItem("Show TUM correspondences", nullptr, &tls_registration.tum.show_correspondences); + ImGui::SetNextItemWidth(ImGuiNumberWidth); + ImGui::SliderFloat("TUM point size", &tls_registration.tum.point_size, 1.0f, 20.0f); + ImGui::EndMenu(); + } + } + ImGui::EndDisabled(); + ImGui::Separator(); } ImGui::EndDisabled(); diff --git a/apps/multi_view_tls_registration_legacy/multi_view_tls_registration.h b/apps/multi_view_tls_registration_legacy/multi_view_tls_registration.h index 0553abb5..d9645053 100644 --- a/apps/multi_view_tls_registration_legacy/multi_view_tls_registration.h +++ b/apps/multi_view_tls_registration_legacy/multi_view_tls_registration.h @@ -11,6 +11,7 @@ #include #include #include +#include namespace fs = std::filesystem; @@ -104,6 +105,9 @@ struct TLSRegistration // GNSS GNSS gnss; + // TUM trajectory + TUM tum; + // Loading bool calculate_offset; // Whether to calculate offset to point cloud on loading bool is_decimate = true; // Whether to decimate point clouds on loading diff --git a/apps/multi_view_tls_registration_legacy/multi_view_tls_registration_gui.cpp b/apps/multi_view_tls_registration_legacy/multi_view_tls_registration_gui.cpp index c976aa18..84332f31 100644 --- a/apps/multi_view_tls_registration_legacy/multi_view_tls_registration_gui.cpp +++ b/apps/multi_view_tls_registration_legacy/multi_view_tls_registration_gui.cpp @@ -1078,6 +1078,7 @@ void loop_closure_gui() index_loop_closure_target, m_gizmo, tls_registration.gnss, + tls_registration.tum, session.ground_control_points, session.control_points, num_edge_extended_before, diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 64bc3b1c..0140c773 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -12,6 +12,7 @@ set(CORE_BASE_SOURCES src/point_cloud.cpp src/point_clouds.cpp src/session.cpp + src/tum.cpp # # src/utils.cpp # TODO(mwlasiuk) : broken AF ... ) diff --git a/core/include/Core/manual_pose_graph_loop_closure.h b/core/include/Core/manual_pose_graph_loop_closure.h index 000b93a8..d43bc92a 100644 --- a/core/include/Core/manual_pose_graph_loop_closure.h +++ b/core/include/Core/manual_pose_graph_loop_closure.h @@ -5,6 +5,7 @@ #include #include #include +#include class ManualPoseGraphLoopClosure : public PoseGraphLoopClosure { @@ -23,6 +24,7 @@ class ManualPoseGraphLoopClosure : public PoseGraphLoopClosure int& index_loop_closure_target, float* m_gizmo, GNSS& gnss, + TUM& tum, GroundControlPoints& gcps, ControlPoints& cps, int num_edge_extended_before, diff --git a/core/include/Core/pfd_wrapper.hpp b/core/include/Core/pfd_wrapper.hpp index 87977bfd..3665d317 100644 --- a/core/include/Core/pfd_wrapper.hpp +++ b/core/include/Core/pfd_wrapper.hpp @@ -50,6 +50,7 @@ namespace mandeye::fd const std::vector Nmea_filter = { "NMEA file (*.nmea)", "*.nmea", "All files", "*" }; const std::vector Gnss_filter = { "GNSS file (*.gnss)", "*.gnss", "All files", "*" }; + const std::vector Tum_filter = { "TUM trajectory file (*.tum, *.txt)", "*.tum *.txt", "All files", "*" }; std::string OpenFileDialogOneFile(const std::string& title, const std::vector& filter); std::vector OpenFileDialog(const std::string& title, const std::vector& filter, bool multiselect); diff --git a/core/include/Core/pose_graph_loop_closure.h b/core/include/Core/pose_graph_loop_closure.h index 2d60089a..d8a65f7d 100644 --- a/core/include/Core/pose_graph_loop_closure.h +++ b/core/include/Core/pose_graph_loop_closure.h @@ -4,6 +4,7 @@ #include #include #include +#include class PoseGraphLoopClosure { @@ -57,6 +58,7 @@ class PoseGraphLoopClosure void set_current_poses_as_motion_model(PointClouds& point_clouds_container); void graph_slam(PointClouds& point_clouds_container, GNSS& gnss, GroundControlPoints& gcps, ControlPoints& cps); void FuseTrajectoryWithGNSS(PointClouds& point_clouds_container, GNSS& gnss); + void FuseTrajectoryWithTUM(PointClouds& point_clouds_container, TUM& tum); void run_icp( PointClouds& point_clouds_container, int index_active_edge, diff --git a/core/include/Core/raylib_render.hpp b/core/include/Core/raylib_render.hpp index dd9d997b..ed8d72e0 100644 --- a/core/include/Core/raylib_render.hpp +++ b/core/include/Core/raylib_render.hpp @@ -170,6 +170,37 @@ class ScanRenderer void drawCachedWithTransform( size_t index, const Eigen::Affine3d& extraTransform, Color color, float pointSize, bool useIntensityColor) const; + // A caller-owned GPU buffer for an arbitrary world-space point set drawn + // via drawPoints() below -- for overlays that aren't part of any + // PointCloud (e.g. an externally loaded TUM trajectory) but still want + // trajectory-style point dots (GL_POINTS, sized via the same pointSize + // shader uniform drawTrajectories() uses) instead of a thin rlgl line. + // Default-constructed as empty/unallocated; the caller uploadPoints()s + // into it once (or whenever its source data actually changes) and + // drawPoints()s it every frame, rather than rebuilding a VAO/VBO every + // frame. Caller must unloadPoints() it before destruction (e.g. in its + // owner's own shutdown/destructor) to avoid leaking the VAO/VBO. + struct PointsGPU + { + unsigned int vao = 0; + unsigned int vbo = 0; + int vertexCount = 0; + }; + + // (Re)uploads positions into gpu, replacing whatever it held before. + // Call only when positions actually changed -- e.g. once after loading a + // new TUM trajectory, not unconditionally every frame. + void uploadPoints(PointsGPU& gpu, const std::vector& positions) const; + + // Draws a previously uploaded PointsGPU as GL_POINTS, flat-colored. Safe + // to call every frame -- issues one glDrawArrays against the existing + // buffer, no allocation. Does nothing if gpu is empty or the shader + // failed to load. + void drawPoints(const PointsGPU& gpu, Color color, float pointSize) const; + + // Releases gpu's VAO/VBO, resetting it back to empty/unallocated. + void unloadPoints(PointsGPU& gpu) const; + private: struct CloudGPU { diff --git a/core/include/Core/tum.h b/core/include/Core/tum.h new file mode 100644 index 00000000..8056e787 --- /dev/null +++ b/core/include/Core/tum.h @@ -0,0 +1,60 @@ +#pragma once + +#include +#include + +// Loads trajectories in the TUM RGB-D dataset format (one pose per line: +// "timestamp tx ty tz qx qy qz qw", space separated, '#'-prefixed lines +// treated as comments). Data-only -- unlike GNSS, this has no render()/GL +// dependency of its own; drawing it is the caller's job (see renderTUM() +// in multi_view_tls_registration_gui.cpp). tum_poses are drawn as GL_POINTS +// via ScanRenderer::uploadPoints()/drawPoints(ScanRenderer::PointsGPU&, ...) +// -- the same point-based approach and pointSize-uniform shader used for a +// scan's own local_trajectory (see PointCloud::line_width / +// ScanRenderer::drawTrajectories()) -- rather than a thin polyline, so +// point_size mirrors line_width's role: it's the drawn dot size, not a line +// width. version lets the caller cache its uploaded GPU buffer instead of +// re-uploading every frame -- see version's own comment below. +// show_correspondences still draws thin lines +// to each scan's nearest local_trajectory sample by timestamp -- matched +// against timestamps.first (the LIO trajectory CSV's "timestamp_nanoseconds" +// column, Unix-epoch nanoseconds), scaling TUM's own Unix-epoch-seconds +// timestamp up by 1e9 first. timestamps.second ("timestampUnix_nanoseconds") +// is the more obviously-named match but is commonly left at 0 (not every +// LIO run captures it), so it isn't used. +class TUM +{ +public: + struct TumPose + { + double timestamp; + double x; // already Cartesian, in the trajectory's local/global frame + double y; + double z; + double qx; // orientation quaternion + double qy; + double qz; + double qw; + }; + + TUM() = default; + ~TUM() = default; + + //! \brief Load trajectory data from TUM-format files + //! \param input_file_names - vector of file names + //! \param subtract_first_pose - if true, every pose is re-expressed relative to the + //! first (chronologically earliest) pose, so that pose becomes the identity + //! transform (0,0,0, no rotation) and the rest follow relative to it -- mirrors + //! GNSS's "load with offset -> move to (0,0,0)" localize option + //! \return true if the data was loaded successfully, false otherwise + bool load_data_from_tum(const std::vector& input_file_names, bool subtract_first_pose = false); + + std::vector tum_poses; + bool show_correspondences = false; + float point_size = 4.0f; // GL_POINTS dot size (pixels) used to draw tum_poses + + // Bumped every time load_data_from_tum() succeeds. A GL-side cache keyed + // on this (e.g. a renderer's own "last uploaded version") only needs to + // re-upload tum_poses when this changes, rather than every frame. + size_t version = 0; +}; diff --git a/core/src/manual_pose_graph_loop_closure.cpp b/core/src/manual_pose_graph_loop_closure.cpp index 84585047..9aa9b6bc 100644 --- a/core/src/manual_pose_graph_loop_closure.cpp +++ b/core/src/manual_pose_graph_loop_closure.cpp @@ -31,6 +31,7 @@ void ManualPoseGraphLoopClosure::Gui( int& index_loop_closure_target, float* m_gizmo, GNSS& gnss, + TUM& tum, GroundControlPoints& gcps, ControlPoints& cps, int num_edge_extended_before, @@ -151,6 +152,11 @@ void ManualPoseGraphLoopClosure::Gui( { FuseTrajectoryWithGNSS(point_clouds_container, gnss); } + + if (ImGui::Button("Fuse trajectory with TUM (trajectory is rigid)")) + { + FuseTrajectoryWithTUM(point_clouds_container, tum); + } } } diff --git a/core/src/pose_graph_loop_closure.cpp b/core/src/pose_graph_loop_closure.cpp index ce902dd3..a6c8d9dd 100644 --- a/core/src/pose_graph_loop_closure.cpp +++ b/core/src/pose_graph_loop_closure.cpp @@ -1129,6 +1129,136 @@ void PoseGraphLoopClosure::FuseTrajectoryWithGNSS(PointClouds& point_clouds_cont } } +// Mirrors FuseTrajectoryWithGNSS() above -- same single-rigid-6DOF-transform +// least-squares fit, applied to every point cloud pose in one shot, just +// matched against a TUM trajectory instead of a GNSS one. The one real +// difference is the timestamp comparison: pc.timestamps[0] is Unix-epoch +// nanoseconds (as GNSS's own .timestamp happens to share, hence +// FuseTrajectoryWithGNSS's direct comparison), while TUM's timestamp field +// is Unix-epoch seconds -- scaled up by 1e9 here before comparing, same fix +// as renderTUM()'s correspondence lines in multi_view_tls_registration_gui.cpp. +// +// Unlike FuseTrajectoryWithGNSS, the normal equations here are accumulated +// as dense 6x6/6x1 blocks (one rank-3 Ai^T*Ai / Ai^T*bi update per +// observation) rather than via triplet lists into a sparse matA/matP/matB -- +// there are always exactly 6 unknowns (one shared rigid delta-pose) and the +// observation weight P is always identity, so the sparse machinery buys +// nothing here. +void PoseGraphLoopClosure::FuseTrajectoryWithTUM(PointClouds& point_clouds_container, TUM& tum) +{ + for (int iter = 0; iter < 30; iter++) + { + Eigen::Matrix AtPA = Eigen::Matrix::Zero(); + Eigen::Matrix AtPB = Eigen::Matrix::Zero(); + Eigen::Affine3d m_pose = Eigen::Affine3d::Identity(); + int num_observations = 0; + + for (int index_pose = 0; index_pose < point_clouds_container.point_clouds.size(); index_pose++) + { + const auto& pc = point_clouds_container.point_clouds[index_pose]; + + double time_stamp_ns = pc.timestamps[0]; + + auto it = std::lower_bound( + tum.tum_poses.begin(), + tum.tum_poses.end(), + time_stamp_ns, + [](const TUM::TumPose& lhs, const double& time) -> bool + { + return lhs.timestamp * 1.0e9 < time; + }); + + int index = it - tum.tum_poses.begin() - 1; + + if (index > 0 && index < tum.tum_poses.size()) + { + if (fabs(time_stamp_ns - tum.tum_poses[index].timestamp * 1.0e9) < 5.0e8) // 0.5s, in ns + { + Eigen::Matrix jacobian; + TaitBryanPose pose_s; + pose_s = pose_tait_bryan_from_affine_matrix(m_pose); + Eigen::Vector3d p_s = pc.m_pose.translation(); + point_to_point_source_to_target_tait_bryan_wc_jacobian( + jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z()); + + double delta_x; + double delta_y; + double delta_z; + Eigen::Vector3d p_t( + tum.tum_poses[index].x - point_clouds_container.offset.x(), + tum.tum_poses[index].y - point_clouds_container.offset.y(), + tum.tum_poses[index].z - point_clouds_container.offset.z()); + + point_to_point_source_to_target_tait_bryan_wc( + delta_x, + delta_y, + delta_z, + pose_s.px, + pose_s.py, + pose_s.pz, + pose_s.om, + pose_s.fi, + pose_s.ka, + p_s.x(), + p_s.y(), + p_s.z(), + p_t.x(), + p_t.y(), + p_t.z()); + + Eigen::Matrix Ai = -jacobian; + Eigen::Vector3d bi(delta_x, delta_y, delta_z); + + AtPA += Ai.transpose() * Ai; + AtPB += Ai.transpose() * bi; + ++num_observations; + } + } + } + + bool is_ok = false; + TaitBryanPose pose; + + if (num_observations > 0) + { + Eigen::LDLT> solver(AtPA); + if (solver.info() == Eigen::Success) + { + Eigen::Matrix x = solver.solve(AtPB); + pose.px = x(0); + pose.py = x(1); + pose.pz = x(2); + pose.om = x(3); + pose.fi = x(4); + pose.ka = x(5); + is_ok = true; + } + else + { + std::cout << "FuseTrajectoryWithTUM: solving AtPA=AtPB FAILED" << std::endl; + } + } + + if (is_ok) + { + m_pose = affine_matrix_from_pose_tait_bryan(pose); + + for (size_t i = 0; i < point_clouds_container.point_clouds.size(); i++) + { + point_clouds_container.point_clouds[i].m_pose = m_pose * point_clouds_container.point_clouds[i].m_pose; + point_clouds_container.point_clouds[i].pose = + pose_tait_bryan_from_affine_matrix(point_clouds_container.point_clouds[i].m_pose); + point_clouds_container.point_clouds[i].gui_translation[0] = point_clouds_container.point_clouds[i].pose.px; + point_clouds_container.point_clouds[i].gui_translation[1] = point_clouds_container.point_clouds[i].pose.py; + point_clouds_container.point_clouds[i].gui_translation[2] = point_clouds_container.point_clouds[i].pose.pz; + point_clouds_container.point_clouds[i].gui_rotation[0] = rad2deg(point_clouds_container.point_clouds[i].pose.om); + point_clouds_container.point_clouds[i].gui_rotation[1] = rad2deg(point_clouds_container.point_clouds[i].pose.fi); + point_clouds_container.point_clouds[i].gui_rotation[2] = rad2deg(point_clouds_container.point_clouds[i].pose.ka); + } + } + } +} + void PoseGraphLoopClosure::run_icp( PointClouds& point_clouds_container, int index_active_edge, diff --git a/core/src/raylib_render.cpp b/core/src/raylib_render.cpp index 7374630a..0c3d7595 100644 --- a/core/src/raylib_render.cpp +++ b/core/src/raylib_render.cpp @@ -408,6 +408,74 @@ void ScanRenderer::drawCachedWithTransform( rlDisableShader(); } +void ScanRenderer::uploadPoints(PointsGPU& gpu, const std::vector& positions) const +{ + unloadPoints(gpu); + + if (positions.empty()) + { + return; + } + + std::vector data; + data.reserve(positions.size() * 3); + for (const auto& p : positions) + { + data.push_back(static_cast(p.x())); + data.push_back(static_cast(p.y())); + data.push_back(static_cast(p.z())); + } + + gpu.vao = rlLoadVertexArray(); + rlEnableVertexArray(gpu.vao); + gpu.vbo = rlLoadVertexBuffer(data.data(), static_cast(data.size() * sizeof(float)), false); + rlSetVertexAttribute(0, 3, RL_FLOAT, false, 3 * sizeof(float), 0); + rlEnableVertexAttribute(0); + rlDisableVertexArray(); + + gpu.vertexCount = static_cast(positions.size()); +} + +void ScanRenderer::drawPoints(const PointsGPU& gpu, Color color, float pointSize) const +{ + if (!shaderValid_ || gpu.vertexCount == 0) + { + return; + } + + rlDrawRenderBatchActive(); + + Matrix mvp = MatrixMultiply(rlGetMatrixModelview(), rlGetMatrixProjection()); + float colorF[4] = { color.r / 255.f, color.g / 255.f, color.b / 255.f, color.a / 255.f }; + int colorModeFlat = 0; + + rlEnableShader(shader_.id); + rlSetUniformMatrix(locMVP_, mvp); + rlSetUniform(locColorMode_, &colorModeFlat, RL_SHADER_UNIFORM_INT, 1); + rlSetUniform(locColor_, colorF, RL_SHADER_UNIFORM_VEC4, 1); + rlSetUniform(locPointSize_, &pointSize, RL_SHADER_UNIFORM_FLOAT, 1); + + rlEnableVertexArray(gpu.vao); + glDrawArrays(GL_POINTS, 0, gpu.vertexCount); + rlDisableVertexArray(); + rlDisableShader(); +} + +void ScanRenderer::unloadPoints(PointsGPU& gpu) const +{ + if (gpu.vao) + { + rlUnloadVertexArray(gpu.vao); + gpu.vao = 0; + } + if (gpu.vbo) + { + rlUnloadVertexBuffer(gpu.vbo); + gpu.vbo = 0; + } + gpu.vertexCount = 0; +} + namespace { constexpr double RAD_TO_DEG = 180.0 / M_PI; diff --git a/core/src/tum.cpp b/core/src/tum.cpp new file mode 100644 index 00000000..764f2b65 --- /dev/null +++ b/core/src/tum.cpp @@ -0,0 +1,138 @@ +#include + +#include + +#include +#include + +namespace +{ + void split(const std::string& str, char delim, std::vector& out) + { + size_t start; + size_t end = 0; + + while ((start = str.find_first_not_of(delim, end)) != std::string::npos) + { + end = str.find(delim, start); + out.push_back(str.substr(start, end - start)); + } + } +} // namespace + +bool TUM::load_data_from_tum(const std::vector& input_file_names, bool subtract_first_pose) +{ + tum_poses.clear(); + + std::cout << "loading TUM trajectory data from following files:" << std::endl; + for (const auto& fn : input_file_names) + { + std::cout << fn << std::endl; + } + + for (const auto& fn : input_file_names) + { + std::ifstream infile(fn); + if (!infile.good()) + { + std::cout << "problem with file: '" << fn << "'" << std::endl; + return false; + } + std::string s; + while (!infile.eof()) + { + getline(infile, s); + + if (s.empty() || s[0] == '#') + { + continue; + } + + std::vector strs; + split(s, ' ', strs); + + if (strs.size() >= 8) + { + TUM::TumPose tp; + std::istringstream(strs[0]) >> tp.timestamp; + std::istringstream(strs[1]) >> tp.x; + std::istringstream(strs[2]) >> tp.y; + std::istringstream(strs[3]) >> tp.z; + std::istringstream(strs[4]) >> tp.qx; + std::istringstream(strs[5]) >> tp.qy; + std::istringstream(strs[6]) >> tp.qz; + std::istringstream(strs[7]) >> tp.qw; + + if (std::isfinite(tp.x) && std::isfinite(tp.y) && std::isfinite(tp.z) && std::isfinite(tp.qx) && + std::isfinite(tp.qy) && std::isfinite(tp.qz) && std::isfinite(tp.qw)) + { + tum_poses.push_back(tp); + } + } + } + infile.close(); + } + + std::sort( + tum_poses.begin(), + tum_poses.end(), + [](const TUM::TumPose& a, const TUM::TumPose& b) + { + return (a.timestamp < b.timestamp); + }); + + std::cout << "loaded " << tum_poses.size() << " TUM poses" << std::endl; + + if (subtract_first_pose && !tum_poses.empty()) + { + const TumPose& first = tum_poses.front(); + Eigen::Affine3d T0 = Eigen::Affine3d::Identity(); + T0.translate(Eigen::Vector3d(first.x, first.y, first.z)); + T0.rotate(Eigen::Quaterniond(first.qw, first.qx, first.qy, first.qz).normalized()); + Eigen::Affine3d T0_inv = T0.inverse(); + + for (auto& p : tum_poses) + { + Eigen::Affine3d T = Eigen::Affine3d::Identity(); + T.translate(Eigen::Vector3d(p.x, p.y, p.z)); + T.rotate(Eigen::Quaterniond(p.qw, p.qx, p.qy, p.qz).normalized()); + + Eigen::Affine3d rel = T0_inv * T; + const Eigen::Vector3d t = rel.translation(); + const Eigen::Quaterniond q(rel.rotation()); + + p.x = t.x(); + p.y = t.y(); + p.z = t.z(); + p.qx = q.x(); + p.qy = q.y(); + p.qz = q.z(); + p.qw = q.w(); + } + } + + if (!tum_poses.empty()) + { + Eigen::Vector3d min_p( + std::numeric_limits::max(), std::numeric_limits::max(), std::numeric_limits::max()); + Eigen::Vector3d max_p( + std::numeric_limits::lowest(), std::numeric_limits::lowest(), std::numeric_limits::lowest()); + for (const auto& p : tum_poses) + { + min_p = min_p.cwiseMin(Eigen::Vector3d(p.x, p.y, p.z)); + max_p = max_p.cwiseMax(Eigen::Vector3d(p.x, p.y, p.z)); + } + spdlog::info( + "TUM bounding box: min=({}, {}, {}) max=({}, {}, {})", + min_p.x(), + min_p.y(), + min_p.z(), + max_p.x(), + max_p.y(), + max_p.z()); + } + + ++version; + + return true; +} From d27c00e56ad3d4cc3299a92354c7dc0759be695f Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Fri, 14 Aug 2026 23:50:12 +0200 Subject: [PATCH 2/2] clang format Signed-off-by: Michal Pelka --- core/src/tum.cpp | 15 ++++----------- 1 file changed, 4 insertions(+), 11 deletions(-) diff --git a/core/src/tum.cpp b/core/src/tum.cpp index 764f2b65..abbe3f5b 100644 --- a/core/src/tum.cpp +++ b/core/src/tum.cpp @@ -63,8 +63,8 @@ bool TUM::load_data_from_tum(const std::vector& input_file_names, b std::istringstream(strs[6]) >> tp.qz; std::istringstream(strs[7]) >> tp.qw; - if (std::isfinite(tp.x) && std::isfinite(tp.y) && std::isfinite(tp.z) && std::isfinite(tp.qx) && - std::isfinite(tp.qy) && std::isfinite(tp.qz) && std::isfinite(tp.qw)) + if (std::isfinite(tp.x) && std::isfinite(tp.y) && std::isfinite(tp.z) && std::isfinite(tp.qx) && std::isfinite(tp.qy) && + std::isfinite(tp.qz) && std::isfinite(tp.qw)) { tum_poses.push_back(tp); } @@ -113,8 +113,7 @@ bool TUM::load_data_from_tum(const std::vector& input_file_names, b if (!tum_poses.empty()) { - Eigen::Vector3d min_p( - std::numeric_limits::max(), std::numeric_limits::max(), std::numeric_limits::max()); + Eigen::Vector3d min_p(std::numeric_limits::max(), std::numeric_limits::max(), std::numeric_limits::max()); Eigen::Vector3d max_p( std::numeric_limits::lowest(), std::numeric_limits::lowest(), std::numeric_limits::lowest()); for (const auto& p : tum_poses) @@ -123,13 +122,7 @@ bool TUM::load_data_from_tum(const std::vector& input_file_names, b max_p = max_p.cwiseMax(Eigen::Vector3d(p.x, p.y, p.z)); } spdlog::info( - "TUM bounding box: min=({}, {}, {}) max=({}, {}, {})", - min_p.x(), - min_p.y(), - min_p.z(), - max_p.x(), - max_p.y(), - max_p.z()); + "TUM bounding box: min=({}, {}, {}) max=({}, {}, {})", min_p.x(), min_p.y(), min_p.z(), max_p.x(), max_p.y(), max_p.z()); } ++version;