From 3b9ad885674f23ec79a845048b26d6438bc97d07 Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Sun, 13 Sep 2026 09:24:32 +0200 Subject: [PATCH] Add scale to imu error visualization Signed-off-by: Michal Pelka --- .../multi_view_tls_registration_gui.cpp | 11 ++++++++++- .../multi_view_tls_registration_gui.cpp | 8 ++++++++ core/include/Core/point_cloud.h | 3 ++- core/include/Core/point_clouds.h | 1 + core/include/Core/raylib_render.hpp | 3 ++- core/src/point_cloud.cpp | 9 +++++---- core/src/point_clouds.cpp | 3 ++- core/src/raylib_render.cpp | 9 +++++---- 8 files changed, 35 insertions(+), 12 deletions(-) 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 72e48ebd..6000c596 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 @@ -4631,7 +4631,8 @@ void display() session.point_clouds_container.xz_intersection, session.point_clouds_container.yz_intersection, session.point_clouds_container.xy_intersection, - session.point_clouds_container.show_with_initial_pose); + session.point_clouds_container.show_with_initial_pose, + session.point_clouds_container.imu_to_lio_diff_scale); observationPickingRender(observation_picking); @@ -5883,6 +5884,14 @@ void display() } ImGui::MenuItem("Show IMU to LIO difference", nullptr, &session.point_clouds_container.show_imu_to_lio_diff); + if (session.point_clouds_container.show_imu_to_lio_diff) + { + ImGui::SetNextItemWidth(ImGuiNumberWidth); + ImGui::InputFloat( + "IMU to LIO diff scale", &session.point_clouds_container.imu_to_lio_diff_scale, 1.0f, 10.0f, "%.1f"); + if (session.point_clouds_container.imu_to_lio_diff_scale < 0.0f) + session.point_clouds_container.imu_to_lio_diff_scale = 0.0f; + } ImGui::Separator(); 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 84332f31..4dee9eda 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 @@ -3582,6 +3582,14 @@ void display() } ImGui::MenuItem("Show IMU to LIO difference", nullptr, &session.point_clouds_container.show_imu_to_lio_diff); + if (session.point_clouds_container.show_imu_to_lio_diff) + { + ImGui::SetNextItemWidth(ImGuiNumberWidth); + ImGui::InputFloat( + "IMU to LIO diff scale", &session.point_clouds_container.imu_to_lio_diff_scale, 1.0f, 10.0f, "%.1f"); + if (session.point_clouds_container.imu_to_lio_diff_scale < 0.0f) + session.point_clouds_container.imu_to_lio_diff_scale = 0.0f; + } ImGui::Separator(); diff --git a/core/include/Core/point_cloud.h b/core/include/Core/point_cloud.h index 047ef811..70d1e4d4 100644 --- a/core/include/Core/point_cloud.h +++ b/core/include/Core/point_cloud.h @@ -161,7 +161,8 @@ class PointCloud bool yz_intersection, bool xy_intersection, double intersection_width, - bool visible_imu_diff); + bool visible_imu_diff, + float imu_diff_scale = 10.0f); void render(Eigen::Affine3d pose, int viewer_decmiate_point_cloud, int viewer_reduce_rendered_trajectory, float _render_color[3]); #endif }; diff --git a/core/include/Core/point_clouds.h b/core/include/Core/point_clouds.h index eeb3188d..048ebec2 100644 --- a/core/include/Core/point_clouds.h +++ b/core/include/Core/point_clouds.h @@ -39,6 +39,7 @@ class PointClouds std::string initial_poses_file_name; std::string out_poses_file_name; bool show_imu_to_lio_diff = false; + float imu_to_lio_diff_scale = 10.0f; struct PointCloudDimensions { diff --git a/core/include/Core/raylib_render.hpp b/core/include/Core/raylib_render.hpp index 09fe75fc..b0d0c3f3 100644 --- a/core/include/Core/raylib_render.hpp +++ b/core/include/Core/raylib_render.hpp @@ -184,7 +184,8 @@ class ScanRenderer bool xzIntersection = false, bool yzIntersection = false, bool xyIntersection = false, - bool useInitialPose = false) const; + bool useInitialPose = false, + float imuDiffScale = 10.0f) const; // Draws a single already-cached scan (see rebuild()) straight from its // persistent, full-resolution GPU buffer -- no CPU re-transform or diff --git a/core/src/point_cloud.cpp b/core/src/point_cloud.cpp index 9339f7f3..ecd8572e 100644 --- a/core/src/point_cloud.cpp +++ b/core/src/point_cloud.cpp @@ -1533,7 +1533,8 @@ void PointCloud::render( bool yz_intersection, bool xy_intersection, double intersection_width, - bool visible_imu_diff) + bool visible_imu_diff, + float imu_diff_scale) { if (!this->visible) return; @@ -1547,15 +1548,15 @@ void PointCloud::render( glColor3f(1, 0, 0); glVertex3f(m(0, 3), m(1, 3), m(2, 3)); - glVertex3f(m(0, 3) + this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.x() * 10, m(1, 3), m(2, 3)); + glVertex3f(m(0, 3) + this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.x() * imu_diff_scale, m(1, 3), m(2, 3)); glColor3f(0, 1, 0); glVertex3f(m(0, 3), m(1, 3), m(2, 3)); - glVertex3f(m(0, 3), m(1, 3) + this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.y() * 10, m(2, 3)); + glVertex3f(m(0, 3), m(1, 3) + this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.y() * imu_diff_scale, m(2, 3)); glColor3f(0, 0, 1); glVertex3f(m(0, 3), m(1, 3), m(2, 3)); - glVertex3f(m(0, 3), m(1, 3), m(2, 3) + this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.z() * 10); + glVertex3f(m(0, 3), m(1, 3), m(2, 3) + this->local_trajectory[i].imu_diff_angle_om_fi_ka_deg.z() * imu_diff_scale); } glEnd(); } diff --git a/core/src/point_clouds.cpp b/core/src/point_clouds.cpp index 5be50e11..33ca9853 100644 --- a/core/src/point_clouds.cpp +++ b/core/src/point_clouds.cpp @@ -795,7 +795,8 @@ void PointClouds::render( yz_intersection, xy_intersection, intersection_width, - show_imu_to_lio_diff); + show_imu_to_lio_diff, + imu_to_lio_diff_scale); } } #endif diff --git a/core/src/raylib_render.cpp b/core/src/raylib_render.cpp index 7d3bc493..010c18e6 100644 --- a/core/src/raylib_render.cpp +++ b/core/src/raylib_render.cpp @@ -649,7 +649,8 @@ void ScanRenderer::drawTrajectories( bool xzIntersection, bool yzIntersection, bool xyIntersection, - bool useInitialPose) const + bool useInitialPose, + float imuDiffScale) const { int stride = reduceRenderedTrajectory < 1 ? 1 : reduceRenderedTrajectory; @@ -714,9 +715,9 @@ void ScanRenderer::drawTrajectories( Eigen::Affine3d m = shownPose * pc.local_trajectory[i].m_pose; Vector3 origin = toVec3(m.translation()); const auto& diff = pc.local_trajectory[i].imu_diff_angle_om_fi_ka_deg; - DrawLine3D(origin, toVec3(m.translation() + Eigen::Vector3d(diff.x() * 10, 0, 0)), RED); - DrawLine3D(origin, toVec3(m.translation() + Eigen::Vector3d(0, diff.y() * 10, 0)), GREEN); - DrawLine3D(origin, toVec3(m.translation() + Eigen::Vector3d(0, 0, diff.z() * 10)), BLUE); + DrawLine3D(origin, toVec3(m.translation() + Eigen::Vector3d(diff.x() * imuDiffScale, 0, 0)), RED); + DrawLine3D(origin, toVec3(m.translation() + Eigen::Vector3d(0, diff.y() * imuDiffScale, 0)), GREEN); + DrawLine3D(origin, toVec3(m.translation() + Eigen::Vector3d(0, 0, diff.z() * imuDiffScale)), BLUE); } }