diff --git a/apps/multi_session_registration/multi_session_registration.cpp b/apps/multi_session_registration/multi_session_registration.cpp index f613f3fe..b19e8f63 100644 --- a/apps/multi_session_registration/multi_session_registration.cpp +++ b/apps/multi_session_registration/multi_session_registration.cpp @@ -70,7 +70,7 @@ static const std::vector appShortcuts = { { "Normal keys", "A", " { "", "F", "" }, { "", "Ctrl+F", "" }, { "", "G", "" }, - { "", "Ctrl+G", "" }, + { "", "Ctrl+G", "Move center of rotation to gizmo" }, { "", "H", "" }, { "", "Ctrl+H", "" }, { "", "I", "" }, @@ -135,8 +135,8 @@ static const std::vector appShortcuts = { { "Normal keys", "A", " { "", "Shift + scroll", "" }, { "", "Shift + drag", "" }, { "", "Ctrl + left click", "" }, - { "", "Ctrl + right click", "" }, - { "", "Ctrl + middle click", "" } }; + { "", "Ctrl + right click", "change center of rotation" }, + { "", "Ctrl + middle click", "change center of rotation" } }; float m_gizmo[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; @@ -2179,8 +2179,8 @@ void settings_gui() } // set rotation center to gizmo - const auto& session = sessions[index_gizmo]; - if (session.point_clouds_container.point_clouds.size() > 0) + if (index_gizmo >= 0 && index_gizmo < sessions.size() && + sessions[index_gizmo].point_clouds_container.point_clouds.size() > 0) { setNewRotationCenter(sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose.translation()); } @@ -2987,6 +2987,20 @@ void display() if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_M, false)) is_loop_closure_gui = !is_loop_closure_gui; + // Ctrl+G: bring the rotation center to the gizmo (session or edge gizmo, both kept in m_gizmo). + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_G, false)) + { + const bool session_gizmo_visible = std::any_of( + sessions.begin(), + sessions.end(), + [](const Session& s) + { + return s.is_gizmo && !s.is_ground_truth && !s.point_clouds_container.point_clouds.empty(); + }); + if (session_gizmo_visible || (edge_gizmo && !edges.empty())) + setNewRotationCenter(Eigen::Map(m_gizmo).block<3, 1>(0, 3).cast()); + } + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_O, false)) openProject(); @@ -3904,12 +3918,9 @@ void mouse(int glut_button, int state, int x, int y) io.KeyShift, time_stamp_offset); } - else + else if (io.KeyCtrl) { - if (update_rotation_center) - { - setNewRotationCenter(x, y); - } + setNewRotationCenterToClosestTrajectoryPoint(sessions, x, y); } } diff --git a/apps/multi_session_registration/raylib_utils.cpp b/apps/multi_session_registration/raylib_utils.cpp index 7d8a0618..7d6a0eae 100644 --- a/apps/multi_session_registration/raylib_utils.cpp +++ b/apps/multi_session_registration/raylib_utils.cpp @@ -542,6 +542,35 @@ void getClosestTrajectoriesPoint( camera.moveEulerRotationCenterTo(center); } +void setNewRotationCenterToClosestTrajectoryPoint(const std::vector& sessions, int x, int y) +{ + const auto laser_beam = GetLaserBeam(x, y); + double min_distance = std::numeric_limits::max(); + Eigen::Vector3d center; + + for (const auto& session : sessions) + { + if (!session.visible) + continue; + for (const auto& pc : session.point_clouds_container.point_clouds) + for (const auto& node : pc.local_trajectory) + { + Eigen::Vector3d vp = pc.m_pose * node.m_pose.translation(); + double dist = distance_point_to_line(vp, laser_beam); + if (dist < min_distance) + { + min_distance = dist; + center = vp; + } + } + } + + if (min_distance == std::numeric_limits::max()) + setNewRotationCenter(x, y); + else + setNewRotationCenter(center); +} + void setNewRotationCenter(int x, int y) { RegistrationPlaneFeature::Plane pl; diff --git a/apps/multi_session_registration/raylib_utils.h b/apps/multi_session_registration/raylib_utils.h index aa12d9c9..964e4b05 100644 --- a/apps/multi_session_registration/raylib_utils.h +++ b/apps/multi_session_registration/raylib_utils.h @@ -101,6 +101,9 @@ void getClosestTrajectoriesPoint( int& index_loop_closure_target, bool KeyShift, double& time_stamp_offset); +//! Ctrl + middle/right click outside loop closure picking (like Step 2): moves the rotation center to the +//! trajectory point of a visible session closest to the mouse ray, or to the z = 0 plane if there is none. +void setNewRotationCenterToClosestTrajectoryPoint(const std::vector& sessions, int x, int y); void setNewRotationCenter(int x, int y); //! Moves the rotation center directly to a known world point (e.g. the gizmo's position), //! skipping the screen-pick raycast the (x, y) overload does.