diff --git a/CMakeLists.txt b/CMakeLists.txt index 464a1f9f..54b30b2d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -35,6 +35,7 @@ option(BUILD_WITH_BUNDLED_GLEW "Build with bundled GLEW" ON) option(BUILD_WITH_BUNDLED_LIBLASZIP "Build with bundled Lib LASZIP" ON) option(BUILD_WITH_BUNDLED_ONETBB "Build with bundled oneTBB" ON) option(BUILD_WITH_PYBIND "Enable pybind11 bindings" OFF) +option(HDMAPPING_ASSERT_ALWAYS_ABORT "Make HDMAPPING_ASSERT() abort on failure even in release (NDEBUG) builds" OFF) # Add definitions for all projects add_definitions(-DHDMAPPING_VERSION_MAJOR=${HDMAPPING_VERSION_MAJOR}) @@ -46,6 +47,8 @@ add_definitions(-D_USE_MATH_DEFINES) if (MSVC) add_definitions(-DNOMINMAX) add_definitions(-DWIN32_LEAN_AND_MEAN) + # HDMAPPING_ASSERT's __VA_OPT__ needs the conforming preprocessor; + add_compile_options(/Zc:preprocessor) endif() if(APPLE AND CMAKE_CXX_COMPILER_ID STREQUAL "AppleClang") @@ -54,6 +57,10 @@ else() add_compile_definitions(USE_EXECUTION_PAR_UNSEQ=1) endif() +if(HDMAPPING_ASSERT_ALWAYS_ABORT) + add_compile_definitions(HDMAPPING_ASSERT_ALWAYS_ABORT) +endif() + # Expand module seatch path list(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake/") diff --git a/apps/multi_session_registration/CMakeLists.txt b/apps/multi_session_registration/CMakeLists.txt index 89be3fdf..dce3214f 100644 --- a/apps/multi_session_registration/CMakeLists.txt +++ b/apps/multi_session_registration/CMakeLists.txt @@ -48,7 +48,8 @@ target_link_libraries( rlimgui imguizmo_raylib ${PLATFORM_LASZIP_LIB} - ${PLATFORM_MISCELLANEOUS_LIBS}) + ${PLATFORM_MISCELLANEOUS_LIBS} + spdlog::spdlog) if(WIN32) add_custom_command( diff --git a/apps/multi_session_registration/multi_session_registration.cpp b/apps/multi_session_registration/multi_session_registration.cpp index 21fae477..f613f3fe 100644 --- a/apps/multi_session_registration/multi_session_registration.cpp +++ b/apps/multi_session_registration/multi_session_registration.cpp @@ -1,5 +1,6 @@ #include #include +#include #include #include @@ -10,6 +11,7 @@ #include #include #include +#include #include @@ -33,6 +35,7 @@ #undef ShowCursor #endif +#include #include #ifdef _WIN32 @@ -151,8 +154,11 @@ double search_radius = 0.3; bool loaded_sessions = false; bool optimized = false; bool gizmo_all_sessions = false; +//!< Stashed Gizmo update when ImGuizmo::IsUsing(). index is sessionId +std::map> session_drag_preview_poses; bool is_ndt_gui = false; bool is_loop_closure_gui = false; +bool prev_is_loop_closure_gui = false; bool remove_gui = false; NDT ndt; @@ -161,6 +167,7 @@ bool update_rotation_center = false; bool is_settings_gui = true; int number_visible_sessions = 0; +std::set visible_sessions; int index_gt = -1; int old_index_gt = -1; int index_gizmo = -1; @@ -190,7 +197,22 @@ namespace fs = std::filesystem; int num_edge_extended_before = 0; int num_edge_extended_after = 0; -int gui_point_size = 2; +// Cross-section slicing, ported from step 2's per-session "Intersections" menu. Step 3 has no +// single active session, so -- same pattern as View > Points size above -- these are pushed to +// every session's point_clouds_container when changed, instead of being one session's fields. +bool gui_xz_intersection = false; +bool gui_yz_intersection = false; +bool gui_xy_intersection = false; +double gui_intersection_width = 0.1; +bool gui_xz_grid_10x10 = false; +bool gui_xz_grid_1x1 = false; +bool gui_xz_grid_01x01 = false; +bool gui_yz_grid_10x10 = false; +bool gui_yz_grid_1x1 = false; +bool gui_yz_grid_01x01 = false; +bool gui_xy_grid_10x10 = false; +bool gui_xy_grid_1x1 = false; +bool gui_xy_grid_01x01 = false; TaitBryanPose motion_model_weights = { 0.01, 0.01, 0.01, 0.1, 0.1, 0.1 }; /////////////////////////////////////////////////////////////////////////////////// @@ -376,33 +398,179 @@ void ndt_gui() // Places the edge gizmo at the active edge's target pose. void setGizmoFromActiveEdge() { + HDMAPPING_ASSERT(index_active_edge < edges.size(), "requested edge outside edges"); const Edge& e = edges[index_active_edge]; + + HDMAPPING_ASSERT(e.index_session_from < sessions.size(), "requested session from outside session vector"); + HDMAPPING_ASSERT( + e.index_from < sessions[e.index_session_from].point_clouds_container.point_clouds.size(), "scan index outside session"); const Eigen::Affine3d m_to = sessions[e.index_session_from].point_clouds_container.point_clouds[e.index_from].m_pose * affine_matrix_from_pose_tait_bryan(e.relative_pose_tb); Eigen::Map gizmo(m_gizmo); gizmo = m_to.matrix().cast(); } +bool canEdgeBeActivated(const int i) +{ + if (i < 0 || i >= (int)edges.size()) + { + return false; + } + const auto& e = edges[i]; + return visible_sessions.contains(e.index_session_from) && visible_sessions.contains(e.index_session_to); +} + // Makes edge `i` active. The manipulate-edge view draws the scans picked by first/second_session_index and // index_loop_closure_source/target, so these follow the edge; a gizmo that is on is moved to the new edge, // otherwise it would write the previous edge's pose into this one. -void setActiveEdge(int i) +void setActiveEdge(const int i) { + HDMAPPING_ASSERT(i >= 0 && i < (int)edges.size(), "Edge outside bounds"); + if (i < 0 || i >= (int)edges.size()) return; - index_active_edge = i; const Edge& e = edges[i]; + + HDMAPPING_ASSERT(e.index_session_from < (int)sessions.size(), "First session outside"); + HDMAPPING_ASSERT(e.index_session_to < (int)sessions.size(), "Second session outside"); + + HDMAPPING_ASSERT(visible_sessions.contains(e.index_session_from), "Cannot edit edge in hidden session"); + HDMAPPING_ASSERT(visible_sessions.contains(e.index_session_to), "Cannot edit edge in hidden session"); + + index_active_edge = i; first_session_index = e.index_session_from; second_session_index = e.index_session_to; index_loop_closure_source = e.index_from; index_loop_closure_target = e.index_to; + HDMAPPING_ASSERT( + e.index_from < (int)sessions[e.index_session_from].point_clouds_container.point_clouds.size(), + "Edge index_from outside its session's point clouds"); + HDMAPPING_ASSERT( + e.index_to < (int)sessions[e.index_session_to].point_clouds_container.point_clouds.size(), + "Edge index_to outside its session's point clouds"); + if (edge_gizmo) setGizmoFromActiveEdge(); } -void loop_closure_gui() +// Shared by icp_active_edge() and loop_closure_gui()'s plain "ICP" button: when either +// endpoint session of the active edge is ground truth, fills source_out/target_out with the +// other scan's points and the ground-truth scan's points trimmed to that scan's bounding box +// (so a large ground-truth cloud doesn't drag in unrelated overlapping geometry), and returns +// true. Returns false if neither/both sessions are ground truth -- caller handles that case. +bool ground_truth_icp_source_target(std::vector& source_out, std::vector& target_out) +{ + bool is_with_ground_truth = sessions[edges[index_active_edge].index_session_from].is_ground_truth || + sessions[edges[index_active_edge].index_session_to].is_ground_truth; + + if (!is_with_ground_truth) + return false; + + int index_session_from = -1; + int index_session_to = -1; + int index_from = -1; + int index_to = -1; + + if (sessions[edges[index_active_edge].index_session_from].is_ground_truth) + { + index_session_from = edges[index_active_edge].index_session_from; + index_session_to = edges[index_active_edge].index_session_to; + index_from = edges[index_active_edge].index_from; + index_to = edges[index_active_edge].index_to; + } + else + { + index_session_from = edges[index_active_edge].index_session_to; + index_session_to = edges[index_active_edge].index_session_from; + index_from = edges[index_active_edge].index_to; + index_to = edges[index_active_edge].index_from; + } + + double x_min = 1000000000000.0; + double y_min = 1000000000000.0; + double z_min = 1000000000000.0; + double x_max = -1000000000000.0; + double y_max = -1000000000000.0; + double z_max = -1000000000000.0; + + auto& points_to = sessions[index_session_to].point_clouds_container.point_clouds[index_to]; + + for (const auto& p : points_to.points_local) + { + auto pg = points_to.m_pose * p; + if (pg.x() < x_min) + x_min = pg.x(); + if (pg.y() < y_min) + y_min = pg.y(); + if (pg.z() < z_min) + z_min = pg.z(); + + if (pg.x() > x_max) + x_max = pg.x(); + if (pg.y() > y_max) + y_max = pg.y(); + if (pg.z() > z_max) + z_max = pg.z(); + } + auto& points_from = sessions[index_session_from].point_clouds_container.point_clouds[index_from]; + std::vector ground_truth; + for (const auto& p : points_from.points_local) + { + auto pg = points_from.m_pose * p; + if (pg.x() > x_min && pg.x() < x_max) + { + if (pg.y() > y_min && pg.y() < y_max) + { + if (pg.z() > z_min && pg.z() < z_max) + ground_truth.push_back(p); + } + } + } + + // index_session_to/index_to here (not edges[index_active_edge]'s raw fields): those + // get swapped above when the *target* (not source) side of the edge is the ground + // truth session, and source must track that swap too, or it silently ends up + // aligning the ground-truth scan against a filtered copy of itself. + source_out = sessions[index_session_to].point_clouds_container.point_clouds[index_to].points_local; + target_out = std::move(ground_truth); + return true; +} + +// Shared body of the fixed-search-radius ICP buttons in loop_closure_gui(): registers the +// active edge's two scans, via ground_truth_icp_source_target() when applicable. +void icp_active_edge(float search_radius, int number_of_iterations = 30) +{ + std::cout << "Iterative Closest Point" << std::endl; + if (sessions[edges[index_active_edge].index_session_from].is_ground_truth && + sessions[edges[index_active_edge].index_session_to].is_ground_truth) + { + std::cout << "Two sessions are ground truth!!! ICP is disabled" << std::endl; + return; + } + + PairWiseICP icp; + auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); + + std::vector source; + std::vector target; + + if (!ground_truth_icp_source_target(source, target)) + { + source = sessions[edges[index_active_edge].index_session_to] + .point_clouds_container.point_clouds[edges[index_active_edge].index_to] + .points_local; + target = sessions[edges[index_active_edge].index_session_from] + .point_clouds_container.point_clouds[edges[index_active_edge].index_from] + .points_local; + } + + if (icp.compute(source, target, search_radius, number_of_iterations, m_pose)) + edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose); +} + +void on_loop_closure_gui_open() { // Session gizmos are not drawn in this mode and would block the edge gizmo, so drop the selection. if (index_gizmo != -1) @@ -412,9 +580,27 @@ void loop_closure_gui() for (auto& s : sessions) s.is_gizmo = false; } + manipulate_active_edge = false; +} +void on_loop_closure_gui_close() +{ + // reset + manipulate_active_edge = false; + index_loop_closure_target = 0; + index_loop_closure_source = 0; +} +void loop_closure_gui() +{ if (ImGui::Begin("Manual Pose Graph Loop Closure Mode", &is_loop_closure_gui, ImGuiWindowFlags_AlwaysAutoResize)) { + std::stringstream ss; + for (auto& s : visible_sessions) + { + ss << s << " "; + } + ImGui::Text("Visible sessions %s", ss.str().c_str()); + if (ImGui::Button("Optimize GRAPH")) { for (int i = 0; i < 100; i++) @@ -427,56 +613,83 @@ void loop_closure_gui() ImGui::Checkbox("update_rotation_center", &update_rotation_center); - // - auto point_cloud_upper = sessions[first_session_index].point_clouds_container.point_clouds.size() - 1; + // visible session + const auto& firstSessionRef = sessions[first_session_index]; + const auto& secondSessionRef = sessions[second_session_index]; + HDMAPPING_ASSERT(!firstSessionRef.point_clouds_container.point_clouds.empty(), "First session should have at least one cloud"); + HDMAPPING_ASSERT(!secondSessionRef.point_clouds_container.point_clouds.empty(), "Second session should have at least one cloud"); - ImGui::InputInt("gui_point_size", &gui_point_size); - if (gui_point_size < 1) - gui_point_size = 1; + const int firstSessionUpper = firstSessionRef.point_clouds_container.point_clouds.size(); + const int secondSessionUpper = secondSessionRef.point_clouds_container.point_clouds.size(); + { + // Same control and the same push-to-every-session behavior as View > Points + // size -- was a separate "gui_point_size" that unconditionally overwrote every + // session's point_size every frame, permanently fighting View > Points size. + int tmp_point_size = point_size; + ImGui::InputInt("Points size", &point_size); + if (point_size < 1) + point_size = 1; + if (tmp_point_size != point_size) + for (auto& session : sessions) + for (auto& point_cloud : session.point_clouds_container.point_clouds) + point_cloud.point_size = point_size; + } + + ImGui::Text("Manualy adding edges from session %d to %d", first_session_index, second_session_index); ImGui::Text("Num edge extended:"); ImGui::Text("before: "); ImGui::SameLine(); ImGui::PushItemWidth(ImGuiNumberWidth); - ImGui::SliderInt("##fs", &num_edge_extended_before, 0, point_cloud_upper); + ImGui::SliderInt("##fs", &num_edge_extended_before, 0, firstSessionUpper); if (ImGui::IsItemHovered()) - ImGui::SetTooltip("min 0; max %zu", point_cloud_upper); + ImGui::SetTooltip("min 0; max %zu", firstSessionUpper); ImGui::SameLine(); ImGui::InputInt("##fi", &num_edge_extended_before, 1, 5); if (ImGui::IsItemHovered()) - ImGui::SetTooltip("min 0; max %zu", point_cloud_upper); + ImGui::SetTooltip("min 0; max %zu", firstSessionUpper); if (num_edge_extended_before < 0) num_edge_extended_before = 0; - if (num_edge_extended_before >= point_cloud_upper) - num_edge_extended_before = point_cloud_upper; - - point_cloud_upper = sessions[second_session_index].point_clouds_container.point_clouds.size() - 1; + if (num_edge_extended_before >= firstSessionUpper) + num_edge_extended_before = firstSessionUpper; ImGui::Text(" after: "); ImGui::SameLine(); - ImGui::SliderInt("##ts", &num_edge_extended_after, index_loop_closure_target, static_cast(point_cloud_upper)); + ImGui::SliderInt("##ts", &num_edge_extended_after, index_loop_closure_target, static_cast(secondSessionUpper)); if (ImGui::IsItemHovered()) - ImGui::SetTooltip("min 0; max %zu", point_cloud_upper); + ImGui::SetTooltip("min 0; max %zu", secondSessionUpper); ImGui::SameLine(); ImGui::InputInt("##ti", &num_edge_extended_after, 1, 5); if (ImGui::IsItemHovered()) - ImGui::SetTooltip("min 0; max %zu", point_cloud_upper); + ImGui::SetTooltip("min 0; max %zu", secondSessionUpper); if (num_edge_extended_after < 0) num_edge_extended_after = 0; - if (num_edge_extended_after >= point_cloud_upper) - num_edge_extended_after = point_cloud_upper; + if (num_edge_extended_after >= secondSessionUpper) + num_edge_extended_after = secondSessionUpper; ImGui::PopItemWidth(); // if (!manipulate_active_edge) { + ImGui::SliderInt( + "##index_loop_closure_source_slider", + &index_loop_closure_source, + 0, + static_cast(sessions[first_session_index].point_clouds_container.point_clouds.size() - 1)); + ImGui::SameLine(); ImGui::InputInt("index_loop_closure_source", &index_loop_closure_source); if (index_loop_closure_source < 0) index_loop_closure_source = 0; if (index_loop_closure_source >= sessions[first_session_index].point_clouds_container.point_clouds.size() - 1) index_loop_closure_source = sessions[first_session_index].point_clouds_container.point_clouds.size() - 1; + ImGui::SliderInt( + "##index_loop_closure_target_slider", + &index_loop_closure_target, + 0, + static_cast(sessions[second_session_index].point_clouds_container.point_clouds.size() - 1)); + ImGui::SameLine(); ImGui::InputInt("index_loop_closure_target", &index_loop_closure_target); if (index_loop_closure_target < 0) index_loop_closure_target = 0; @@ -532,11 +745,14 @@ void loop_closure_gui() ImGui::SameLine(); char buttonLabel[128]; snprintf(buttonLabel, sizeof(buttonLabel), "Set Active##%d", i); + + ImGui::BeginDisabled(!canEdgeBeActivated(i)); if (ImGui::Button(buttonLabel)) { setActiveEdge(i); manipulate_active_edge = true; } + ImGui::EndDisabled(); ImGui::SameLine(); snprintf(buttonLabel, sizeof(buttonLabel), "Delete##%d", i); if (ImGui::Button(buttonLabel)) @@ -563,6 +779,8 @@ void loop_closure_gui() } if (edges.size() > 0) { + const bool isEdgeOk = canEdgeBeActivated(index_active_edge); + ImGui::BeginDisabled(!isEdgeOk); if (ImGui::Checkbox("manipulate_active_edge", &manipulate_active_edge) && manipulate_active_edge) setActiveEdge(std::clamp(index_active_edge, 0, (int)edges.size() - 1)); if (manipulate_active_edge) @@ -581,6 +799,8 @@ void loop_closure_gui() if (!is_gizmo) { + ImGui::SliderInt("##index_active_edge_slider", &index_active_edge, 0, (int)edges.size() - 1); + ImGui::SameLine(); ImGui::InputInt("index_active_edge", &index_active_edge); if (index_active_edge < 0) @@ -618,87 +838,15 @@ void loop_closure_gui() } else { - bool is_with_ground_truth = false; - if (sessions[edges[index_active_edge].index_session_from].is_ground_truth || - sessions[edges[index_active_edge].index_session_to].is_ground_truth) - { - is_with_ground_truth = true; - } + std::vector source; + std::vector target; - if (is_with_ground_truth) + if (ground_truth_icp_source_target(source, target)) { - int index_session_from = -1; - int index_session_to = -1; - int index_from = -1; - int index_to = -1; - - if (sessions[edges[index_active_edge].index_session_from].is_ground_truth) - { - index_session_from = edges[index_active_edge].index_session_from; - index_session_to = edges[index_active_edge].index_session_to; - index_from = edges[index_active_edge].index_from; - index_to = edges[index_active_edge].index_to; - } - else - { - index_session_from = edges[index_active_edge].index_session_to; - index_session_to = edges[index_active_edge].index_session_from; - index_from = edges[index_active_edge].index_to; - index_to = edges[index_active_edge].index_from; - } - - double x_min = 1000000000000.0; - double y_min = 1000000000000.0; - double z_min = 1000000000000.0; - double x_max = -1000000000000.0; - double y_max = -1000000000000.0; - double z_max = -1000000000000.0; - - auto& points_to = sessions[index_session_to].point_clouds_container.point_clouds[index_to]; - - for (const auto& p : points_to.points_local) - { - auto pg = points_to.m_pose * p; - if (pg.x() < x_min) - x_min = pg.x(); - if (pg.y() < y_min) - y_min = pg.y(); - if (pg.z() < z_min) - z_min = pg.z(); - - if (pg.x() > x_max) - x_max = pg.x(); - if (pg.y() > y_max) - y_max = pg.y(); - if (pg.z() > z_max) - z_max = pg.z(); - } - auto& points_from = sessions[index_session_from].point_clouds_container.point_clouds[index_from]; - std::vector ground_truth; - for (const auto& p : points_from.points_local) - { - auto pg = points_from.m_pose * p; - if (pg.x() > x_min && pg.x() < x_max) - { - if (pg.y() > y_min && pg.y() < y_max) - { - if (pg.z() > z_min && pg.z() < z_max) - ground_truth.push_back(p); - } - } - } - int number_of_iterations = 10; PairWiseICP icp; auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - std::vector source = - sessions[edges[index_active_edge].index_session_to] - .point_clouds_container.point_clouds[edges[index_active_edge].index_to] - .points_local; - std::vector target = - ground_truth; // sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; - if (icp.compute(source, target, search_radius, number_of_iterations, m_pose)) edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose); } @@ -791,597 +939,20 @@ void loop_closure_gui() ///////////////////////////////// if (ImGui::Button("ICP [search radius 2m]")) - { - float sr = 2.0; - std::cout << "Iterative Closest Point" << std::endl; - if (sessions[edges[index_active_edge].index_session_from].is_ground_truth && - sessions[edges[index_active_edge].index_session_to].is_ground_truth) - { - std::cout << "Two sessions are ground truth!!! ICP is disabled" << std::endl; - } - else - { - bool is_with_ground_truth = false; - if (sessions[edges[index_active_edge].index_session_from].is_ground_truth || - sessions[edges[index_active_edge].index_session_to].is_ground_truth) - { - is_with_ground_truth = true; - } - - if (is_with_ground_truth) - { - int index_session_from = -1; - int index_session_to = -1; - int index_from = -1; - int index_to = -1; - - if (sessions[edges[index_active_edge].index_session_from].is_ground_truth) - { - index_session_from = edges[index_active_edge].index_session_from; - index_session_to = edges[index_active_edge].index_session_to; - index_from = edges[index_active_edge].index_from; - index_to = edges[index_active_edge].index_to; - } - else - { - index_session_from = edges[index_active_edge].index_session_to; - index_session_to = edges[index_active_edge].index_session_from; - index_from = edges[index_active_edge].index_to; - index_to = edges[index_active_edge].index_from; - } - - double x_min = 1000000000000.0; - double y_min = 1000000000000.0; - double z_min = 1000000000000.0; - double x_max = -1000000000000.0; - double y_max = -1000000000000.0; - double z_max = -1000000000000.0; - - auto& points_to = sessions[index_session_to].point_clouds_container.point_clouds[index_to]; - - for (const auto& p : points_to.points_local) - { - auto pg = points_to.m_pose * p; - if (pg.x() < x_min) - x_min = pg.x(); - if (pg.y() < y_min) - y_min = pg.y(); - if (pg.z() < z_min) - z_min = pg.z(); - - if (pg.x() > x_max) - x_max = pg.x(); - if (pg.y() > y_max) - y_max = pg.y(); - if (pg.z() > z_max) - z_max = pg.z(); - } - auto& points_from = sessions[index_session_from].point_clouds_container.point_clouds[index_from]; - std::vector ground_truth; - for (const auto& p : points_from.points_local) - { - auto pg = points_from.m_pose * p; - if (pg.x() > x_min && pg.x() < x_max) - { - if (pg.y() > y_min && pg.y() < y_max) - { - if (pg.z() > z_min && pg.z() < z_max) - ground_truth.push_back(p); - } - } - } - - int number_of_iterations = 30; - PairWiseICP icp; - auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - - const std::vector& source = - sessions[edges[index_active_edge].index_session_to] - .point_clouds_container.point_clouds[edges[index_active_edge].index_to] - .points_local; - const std::vector& target = - ground_truth; // sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; - - if (icp.compute(source, target, sr, number_of_iterations, m_pose)) - edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose); - } - else - { - int number_of_iterations = 30; - PairWiseICP icp; - auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - - const std::vector& source = - sessions[edges[index_active_edge].index_session_to] - .point_clouds_container.point_clouds[edges[index_active_edge].index_to] - .points_local; - const std::vector& target = - sessions[edges[index_active_edge].index_session_from] - .point_clouds_container.point_clouds[edges[index_active_edge].index_from] - .points_local; - - if (icp.compute(source, target, sr, number_of_iterations, m_pose)) - edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose); - } - } - } + icp_active_edge(2.0f); ImGui::SameLine(); if (ImGui::Button("ICP [search radius 1m]")) - { - float sr = 1.0; - std::cout << "Iterative Closest Point" << std::endl; - if (sessions[edges[index_active_edge].index_session_from].is_ground_truth && - sessions[edges[index_active_edge].index_session_to].is_ground_truth) - { - std::cout << "Two sessions are ground truth!!! ICP is disabled" << std::endl; - } - else - { - bool is_with_ground_truth = false; - if (sessions[edges[index_active_edge].index_session_from].is_ground_truth || - sessions[edges[index_active_edge].index_session_to].is_ground_truth) - { - is_with_ground_truth = true; - } - - if (is_with_ground_truth) - { - int index_session_from = -1; - int index_session_to = -1; - int index_from = -1; - int index_to = -1; - - if (sessions[edges[index_active_edge].index_session_from].is_ground_truth) - { - index_session_from = edges[index_active_edge].index_session_from; - index_session_to = edges[index_active_edge].index_session_to; - index_from = edges[index_active_edge].index_from; - index_to = edges[index_active_edge].index_to; - } - else - { - index_session_from = edges[index_active_edge].index_session_to; - index_session_to = edges[index_active_edge].index_session_from; - index_from = edges[index_active_edge].index_to; - index_to = edges[index_active_edge].index_from; - } - - double x_min = 1000000000000.0; - double y_min = 1000000000000.0; - double z_min = 1000000000000.0; - double x_max = -1000000000000.0; - double y_max = -1000000000000.0; - double z_max = -1000000000000.0; - - auto& points_to = sessions[index_session_to].point_clouds_container.point_clouds[index_to]; - - for (const auto& p : points_to.points_local) - { - auto pg = points_to.m_pose * p; - if (pg.x() < x_min) - x_min = pg.x(); - if (pg.y() < y_min) - y_min = pg.y(); - if (pg.z() < z_min) - z_min = pg.z(); - - if (pg.x() > x_max) - x_max = pg.x(); - if (pg.y() > y_max) - y_max = pg.y(); - if (pg.z() > z_max) - z_max = pg.z(); - } - auto& points_from = sessions[index_session_from].point_clouds_container.point_clouds[index_from]; - std::vector ground_truth; - for (const auto& p : points_from.points_local) - { - auto pg = points_from.m_pose * p; - if (pg.x() > x_min && pg.x() < x_max) - { - if (pg.y() > y_min && pg.y() < y_max) - { - if (pg.z() > z_min && pg.z() < z_max) - ground_truth.push_back(p); - } - } - } - - int number_of_iterations = 30; - PairWiseICP icp; - auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - - const std::vector& source = - sessions[edges[index_active_edge].index_session_to] - .point_clouds_container.point_clouds[edges[index_active_edge].index_to] - .points_local; - const std::vector& target = - ground_truth; // sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; - - if (icp.compute(source, target, sr, number_of_iterations, m_pose)) - edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose); - } - else - { - int number_of_iterations = 30; - PairWiseICP icp; - auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - - const std::vector& source = - sessions[edges[index_active_edge].index_session_to] - .point_clouds_container.point_clouds[edges[index_active_edge].index_to] - .points_local; - const std::vector& target = - sessions[edges[index_active_edge].index_session_from] - .point_clouds_container.point_clouds[edges[index_active_edge].index_from] - .points_local; - - if (icp.compute(source, target, sr, number_of_iterations, m_pose)) - edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose); - } - } - } + icp_active_edge(1.0f); ImGui::SameLine(); if (ImGui::Button("ICP [search radius 0.5m]")) - { - float sr = 0.5; - std::cout << "Iterative Closest Point" << std::endl; - if (sessions[edges[index_active_edge].index_session_from].is_ground_truth && - sessions[edges[index_active_edge].index_session_to].is_ground_truth) - { - std::cout << "Two sessions are ground truth!!! ICP is disabled" << std::endl; - } - else - { - bool is_with_ground_truth = false; - if (sessions[edges[index_active_edge].index_session_from].is_ground_truth || - sessions[edges[index_active_edge].index_session_to].is_ground_truth) - { - is_with_ground_truth = true; - } - - if (is_with_ground_truth) - { - int index_session_from = -1; - int index_session_to = -1; - int index_from = -1; - int index_to = -1; - - if (sessions[edges[index_active_edge].index_session_from].is_ground_truth) - { - index_session_from = edges[index_active_edge].index_session_from; - index_session_to = edges[index_active_edge].index_session_to; - index_from = edges[index_active_edge].index_from; - index_to = edges[index_active_edge].index_to; - } - else - { - index_session_from = edges[index_active_edge].index_session_to; - index_session_to = edges[index_active_edge].index_session_from; - index_from = edges[index_active_edge].index_to; - index_to = edges[index_active_edge].index_from; - } - - double x_min = 1000000000000.0; - double y_min = 1000000000000.0; - double z_min = 1000000000000.0; - double x_max = -1000000000000.0; - double y_max = -1000000000000.0; - double z_max = -1000000000000.0; - - auto& points_to = sessions[index_session_to].point_clouds_container.point_clouds[index_to]; - - for (const auto& p : points_to.points_local) - { - auto pg = points_to.m_pose * p; - if (pg.x() < x_min) - x_min = pg.x(); - if (pg.y() < y_min) - y_min = pg.y(); - if (pg.z() < z_min) - z_min = pg.z(); - - if (pg.x() > x_max) - x_max = pg.x(); - if (pg.y() > y_max) - y_max = pg.y(); - if (pg.z() > z_max) - z_max = pg.z(); - } - auto& points_from = sessions[index_session_from].point_clouds_container.point_clouds[index_from]; - std::vector ground_truth; - for (const auto& p : points_from.points_local) - { - auto pg = points_from.m_pose * p; - if (pg.x() > x_min && pg.x() < x_max) - { - if (pg.y() > y_min && pg.y() < y_max) - { - if (pg.z() > z_min && pg.z() < z_max) - ground_truth.push_back(p); - } - } - } - - int number_of_iterations = 30; - PairWiseICP icp; - auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - - const std::vector& source = - sessions[edges[index_active_edge].index_session_to] - .point_clouds_container.point_clouds[edges[index_active_edge].index_to] - .points_local; - const std::vector& target = - ground_truth; // sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; - - if (icp.compute(source, target, sr, number_of_iterations, m_pose)) - { - edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose); - } - } - else - { - int number_of_iterations = 30; - PairWiseICP icp; - auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - - const std::vector& source = - sessions[edges[index_active_edge].index_session_to] - .point_clouds_container.point_clouds[edges[index_active_edge].index_to] - .points_local; - const std::vector& target = - sessions[edges[index_active_edge].index_session_from] - .point_clouds_container.point_clouds[edges[index_active_edge].index_from] - .points_local; - - if (icp.compute(source, target, sr, number_of_iterations, m_pose)) - { - edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose); - } - } - } - } + icp_active_edge(0.5f); ImGui::SameLine(); if (ImGui::Button("ICP [search radius 0.25m]")) - { - float sr = 0.25; - std::cout << "Iterative Closest Point" << std::endl; - if (sessions[edges[index_active_edge].index_session_from].is_ground_truth && - sessions[edges[index_active_edge].index_session_to].is_ground_truth) - { - std::cout << "Two sessions are ground truth!!! ICP is disabled" << std::endl; - } - else - { - bool is_with_ground_truth = false; - if (sessions[edges[index_active_edge].index_session_from].is_ground_truth || - sessions[edges[index_active_edge].index_session_to].is_ground_truth) - { - is_with_ground_truth = true; - } - - if (is_with_ground_truth) - { - int index_session_from = -1; - int index_session_to = -1; - int index_from = -1; - int index_to = -1; - - if (sessions[edges[index_active_edge].index_session_from].is_ground_truth) - { - index_session_from = edges[index_active_edge].index_session_from; - index_session_to = edges[index_active_edge].index_session_to; - index_from = edges[index_active_edge].index_from; - index_to = edges[index_active_edge].index_to; - } - else - { - index_session_from = edges[index_active_edge].index_session_to; - index_session_to = edges[index_active_edge].index_session_from; - index_from = edges[index_active_edge].index_to; - index_to = edges[index_active_edge].index_from; - } - - double x_min = 1000000000000.0; - double y_min = 1000000000000.0; - double z_min = 1000000000000.0; - double x_max = -1000000000000.0; - double y_max = -1000000000000.0; - double z_max = -1000000000000.0; - - auto& points_to = sessions[index_session_to].point_clouds_container.point_clouds[index_to]; - - for (const auto& p : points_to.points_local) - { - auto pg = points_to.m_pose * p; - if (pg.x() < x_min) - x_min = pg.x(); - if (pg.y() < y_min) - y_min = pg.y(); - if (pg.z() < z_min) - z_min = pg.z(); - - if (pg.x() > x_max) - x_max = pg.x(); - if (pg.y() > y_max) - y_max = pg.y(); - if (pg.z() > z_max) - z_max = pg.z(); - } - auto& points_from = sessions[index_session_from].point_clouds_container.point_clouds[index_from]; - std::vector ground_truth; - for (const auto& p : points_from.points_local) - { - auto pg = points_from.m_pose * p; - if (pg.x() > x_min && pg.x() < x_max) - { - if (pg.y() > y_min && pg.y() < y_max) - { - if (pg.z() > z_min && pg.z() < z_max) - ground_truth.push_back(p); - } - } - } - - int number_of_iterations = 30; - PairWiseICP icp; - auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - - const std::vector& source = - sessions[edges[index_active_edge].index_session_to] - .point_clouds_container.point_clouds[edges[index_active_edge].index_to] - .points_local; - const std::vector& target = - ground_truth; // sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; - - if (icp.compute(source, target, sr, number_of_iterations, m_pose)) - { - edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose); - } - } - else - { - int number_of_iterations = 30; - PairWiseICP icp; - auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - - const std::vector& source = - sessions[edges[index_active_edge].index_session_to] - .point_clouds_container.point_clouds[edges[index_active_edge].index_to] - .points_local; - const std::vector& target = - sessions[edges[index_active_edge].index_session_from] - .point_clouds_container.point_clouds[edges[index_active_edge].index_from] - .points_local; - - if (icp.compute(source, target, sr, number_of_iterations, m_pose)) - { - edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose); - } - } - } - } + icp_active_edge(0.25f); ImGui::SameLine(); if (ImGui::Button("ICP [search radius 0.1m]")) - { - float sr = 0.1; - std::cout << "Iterative Closest Point" << std::endl; - if (sessions[edges[index_active_edge].index_session_from].is_ground_truth && - sessions[edges[index_active_edge].index_session_to].is_ground_truth) - { - std::cout << "Two sessions are ground truth!!! ICP is disabled" << std::endl; - } - else - { - bool is_with_ground_truth = false; - if (sessions[edges[index_active_edge].index_session_from].is_ground_truth || - sessions[edges[index_active_edge].index_session_to].is_ground_truth) - { - is_with_ground_truth = true; - } - - if (is_with_ground_truth) - { - int index_session_from = -1; - int index_session_to = -1; - int index_from = -1; - int index_to = -1; - - if (sessions[edges[index_active_edge].index_session_from].is_ground_truth) - { - index_session_from = edges[index_active_edge].index_session_from; - index_session_to = edges[index_active_edge].index_session_to; - index_from = edges[index_active_edge].index_from; - index_to = edges[index_active_edge].index_to; - } - else - { - index_session_from = edges[index_active_edge].index_session_to; - index_session_to = edges[index_active_edge].index_session_from; - index_from = edges[index_active_edge].index_to; - index_to = edges[index_active_edge].index_from; - } - - double x_min = 1000000000000.0; - double y_min = 1000000000000.0; - double z_min = 1000000000000.0; - double x_max = -1000000000000.0; - double y_max = -1000000000000.0; - double z_max = -1000000000000.0; - - auto& points_to = sessions[index_session_to].point_clouds_container.point_clouds[index_to]; - - for (const auto& p : points_to.points_local) - { - auto pg = points_to.m_pose * p; - if (pg.x() < x_min) - x_min = pg.x(); - if (pg.y() < y_min) - y_min = pg.y(); - if (pg.z() < z_min) - z_min = pg.z(); - - if (pg.x() > x_max) - x_max = pg.x(); - if (pg.y() > y_max) - y_max = pg.y(); - if (pg.z() > z_max) - z_max = pg.z(); - } - auto& points_from = sessions[index_session_from].point_clouds_container.point_clouds[index_from]; - std::vector ground_truth; - for (const auto& p : points_from.points_local) - { - auto pg = points_from.m_pose * p; - if (pg.x() > x_min && pg.x() < x_max) - { - if (pg.y() > y_min && pg.y() < y_max) - { - if (pg.z() > z_min && pg.z() < z_max) - ground_truth.push_back(p); - } - } - } - - int number_of_iterations = 30; - PairWiseICP icp; - auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - - const std::vector& source = - sessions[edges[index_active_edge].index_session_to] - .point_clouds_container.point_clouds[edges[index_active_edge].index_to] - .points_local; - const std::vector& target = - ground_truth; // sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; - - if (icp.compute(source, target, sr, number_of_iterations, m_pose)) - { - edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose); - } - } - else - { - int number_of_iterations = 30; - PairWiseICP icp; - auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - - const std::vector& source = - sessions[edges[index_active_edge].index_session_to] - .point_clouds_container.point_clouds[edges[index_active_edge].index_to] - .points_local; - const std::vector& target = - sessions[edges[index_active_edge].index_session_from] - .point_clouds_container.point_clouds[edges[index_active_edge].index_from] - .points_local; - - if (icp.compute(source, target, sr, number_of_iterations, m_pose)) - { - edges[index_active_edge].relative_pose_tb = pose_tait_bryan_from_affine_matrix(m_pose); - } - } - } - } + icp_active_edge(0.1f); #if 0 if (ImGui::Button("Save src")) { @@ -1430,6 +1001,7 @@ void loop_closure_gui() ////////////////////////////////// } } + ImGui::EndDisabled(); } ImGui::End(); @@ -1525,6 +1097,33 @@ void save_trajectories_to_laz( } } +// Step 3 has no single active session (unlike step 2's "Export xz/yz/xy intersection", which +// picks one output file for the one session via a save dialog), so this calls the shared +// save_intersection() (Core/export_laz.h) once per session, auto-named -- same pattern as +// "Save all marked trajectories" above. +void export_intersection_all_sessions(bool xz_intersection, bool yz_intersection, bool xy_intersection, const std::string& suffix) +{ + 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; + } + + std::filesystem::path dir = std::filesystem::path(session_path).parent_path(); + std::string folder_name = dir.filename().string(); + std::string laz_path = (dir / (folder_name + suffix)).string(); + + std::cout << "Saving intersection to LAZ: " << laz_path << std::endl; + save_intersection(sessions[i], laz_path, xz_intersection, yz_intersection, xy_intersection, gui_intersection_width); + } + + std::cout << "Finished saving all intersections to .laz files." << std::endl; +} + void createDXFPolyline(const std::string& filename, const std::vector& points) { std::ofstream dxfFile(filename); @@ -2019,6 +1618,9 @@ void appendSession(const std::string& ps) session.point_clouds_container.xy_grid_1x1 = false; session.point_clouds_container.xy_grid_01x01 = false; + for (auto& pc : session.point_clouds_container.point_clouds) + pc.point_size = point_size; + sessions.push_back(session); if (session.is_ground_truth) index_gt = sessions.size() - 1; @@ -2091,6 +1693,7 @@ void finishLoadingSessions() void loadSessions() { sessions.clear(); + session_drag_preview_poses.clear(); for (const auto& ps : project_settings.session_file_names) appendSession(ps); loaded_sessions = true; @@ -2575,29 +2178,30 @@ void settings_gui() sessions[i].is_gizmo = (i == index_gizmo); } + // set rotation center to gizmo + const auto& session = sessions[index_gizmo]; + if (session.point_clouds_container.point_clouds.size() > 0) + { + setNewRotationCenter(sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose.translation()); + } old_index_gt = index_gt; old_index_gizmo = index_gizmo; } - if (index_gizmo != -1 && index_gizmo < sessions.size()) + // Skipped while index_gizmo's gizmo is mid-drag: that session's m_pose is + // deliberately left untouched until the drag ends (see session_drag_preview_poses), + // so resetting m_gizmo from it here every frame would feed ImGuizmo::Manipulate() + // the frozen pre-drag pose instead of its own last output, breaking the drag. + if (index_gizmo != -1 && index_gizmo < sessions.size() && + session_drag_preview_poses.find(static_cast(index_gizmo)) == session_drag_preview_poses.end()) { + HDMAPPING_ASSERT(index_gizmo < sessions.size()); + HDMAPPING_ASSERT(!sessions[index_gizmo].point_clouds_container.point_clouds.empty()); // sessions[index_gizmo].is_gizmo = true; - m_gizmo[0] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(0, 0); - m_gizmo[1] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(1, 0); - m_gizmo[2] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(2, 0); - m_gizmo[3] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(3, 0); - m_gizmo[4] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(0, 1); - m_gizmo[5] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(1, 1); - m_gizmo[6] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(2, 1); - m_gizmo[7] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(3, 1); - m_gizmo[8] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(0, 2); - m_gizmo[9] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(1, 2); - m_gizmo[10] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(2, 2); - m_gizmo[11] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(3, 2); - m_gizmo[12] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(0, 3); - m_gizmo[13] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(1, 3); - m_gizmo[14] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(2, 3); - m_gizmo[15] = (float)sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose(3, 3); + // Column-major 4x4, matching the Eigen::Map(m_gizmo) + // reads elsewhere in this file -- this is just that assignment reversed. + Eigen::Map gizmo_map(m_gizmo); + gizmo_map = sessions[index_gizmo].point_clouds_container.point_clouds[0].m_pose.matrix().cast(); } } @@ -2618,10 +2222,12 @@ void settings_gui() number_visible_sessions = 0; bool first_session_index_found = false; + visible_sessions = {}; for (size_t index = 0; index < sessions.size(); index++) { if (sessions[index].visible) { + visible_sessions.emplace(static_cast(index)); number_visible_sessions++; if (!first_session_index_found) { @@ -2693,11 +2299,6 @@ void settings_gui() if (search_radius < 0.01) search_radius = 0.01; } - - // if (!is_loop_closure_gui && prev_is_loop_closure_gui) - //{ - // exit(1); - // } } } } @@ -2723,19 +2324,10 @@ void display() viewLocal = Eigen::Affine3f::Identity(); - for (auto& s : sessions) - { - for (auto& pc : s.point_clouds_container.point_clouds) - { - pc.point_size = gui_point_size; - } - } - if (!is_ortho) { reshape((int)io.DisplaySize.x, (int)io.DisplaySize.y); - // janusz if (is_loop_closure_gui) { // sessions[first_session_index].point_clouds_container.point_clouds.at(index_loop_closure_source).render(false, @@ -2745,7 +2337,8 @@ void display() // observation_picking, viewer_decmiate_point_cloud, false, false, false, false, false, false, false, false, false, false, // false, false, 100000); - if (first_session_index < sessions[first_session_index].point_clouds_container.point_clouds.size()) + if (index_loop_closure_source >= 0 && + index_loop_closure_source < (int)sessions[first_session_index].point_clouds_container.point_clouds.size()) { if (update_rotation_center) { @@ -2817,6 +2410,7 @@ void display() captureFrameMatrices(); showAxes(); + drawIntersectionGrids(sessions); if (is_loop_closure_gui) { @@ -2845,62 +2439,74 @@ void display() int index_src = edges[index_active_edge].index_from; int index_trg = edges[index_active_edge].index_to; - Eigen::Affine3d _m_src = - sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(index_src).m_pose; + // The active edge's own sessions, not the first/second_session_index globals: those + // are re-synced every frame by settings_gui()'s visibility loop for the "pick two + // sessions for a new edge" UI, independent of which edge is active here, so they can + // (and do) drift from this edge's actual sessions and index into the wrong one. + const int active_session_from = edges[index_active_edge].index_session_from; + const int active_session_to = edges[index_active_edge].index_session_to; + + Eigen::Affine3d _m_src = sessions[active_session_from].point_clouds_container.point_clouds.at(index_src).m_pose; Eigen::Affine3d _m_trg = _m_src * affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); Eigen::Affine3d m_src_0 = - sessions[first_session_index].point_clouds_container.point_clouds.at(index_loop_closure_source).m_pose; // Todo + sessions[active_session_from].point_clouds_container.point_clouds.at(index_loop_closure_source).m_pose; for (int i = index_loop_closure_source - num_edge_extended_before; i <= index_loop_closure_source + num_edge_extended_after; i++) { - if (i >= 0 && i < sessions[first_session_index].point_clouds_container.point_clouds.size() && - sessions[first_session_index].point_clouds_container.point_clouds.size() > 0) + if (i >= 0 && i < sessions[active_session_from].point_clouds_container.point_clouds.size() && + sessions[active_session_from].point_clouds_container.point_clouds.size() > 0) { // ObservationPicking observation_picking; // point_clouds_container.point_clouds.at(i).render(false, observation_picking, 1, 1, false, false, false, 10000, // false); - Eigen::Affine3d m_src_curr = sessions[first_session_index].point_clouds_container.point_clouds.at(i).m_pose; // Todo + Eigen::Affine3d m_src_curr = sessions[active_session_from].point_clouds_container.point_clouds.at(i).m_pose; Eigen::Affine3d m_src = _m_src * (m_src_0.inverse() * m_src_curr); - // sessions[first_session_index].point_clouds_container.point_clouds.at(i).point_size = gui_point_size; - renderScanAtPose( - first_session_index, + active_session_from, i, m_src, viewer_decimate_point_cloud, viewer_reduce_rendered_trajectory, - sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(i).render_color); + sessions[active_session_from].point_clouds_container.point_clouds.at(i).render_color, + /*useSceneColorMode=*/false, + sessions[active_session_from].point_clouds_container.xz_intersection, + sessions[active_session_from].point_clouds_container.yz_intersection, + sessions[active_session_from].point_clouds_container.xy_intersection, + sessions[active_session_from].point_clouds_container.intersection_width); } } Eigen::Affine3d m_trg_0 = - sessions[second_session_index].point_clouds_container.point_clouds.at(index_loop_closure_target).m_pose; // Todo + sessions[active_session_to].point_clouds_container.point_clouds.at(index_loop_closure_target).m_pose; for (int i = index_loop_closure_target - num_edge_extended_before; i <= index_loop_closure_target + num_edge_extended_after; i++) { - if (i >= 0 && i < sessions[second_session_index].point_clouds_container.point_clouds.size() && - sessions[second_session_index].point_clouds_container.point_clouds.size() > 0) + if (i >= 0 && i < sessions[active_session_to].point_clouds_container.point_clouds.size() && + sessions[active_session_to].point_clouds_container.point_clouds.size() > 0) { // ObservationPicking observation_picking; // point_clouds_container.point_clouds.at(i).render(false, observation_picking, 1, 1, false, false, false, 10000, // false); - Eigen::Affine3d m_trg_curr = - sessions[second_session_index].point_clouds_container.point_clouds.at(i).m_pose; // Todo + Eigen::Affine3d m_trg_curr = sessions[active_session_to].point_clouds_container.point_clouds.at(i).m_pose; Eigen::Affine3d m_trg = _m_trg * (m_trg_0.inverse() * m_trg_curr); - // sessions[second_session_index].point_clouds_container.point_clouds.at(i).point_size = gui_point_size; renderScanAtPose( - second_session_index, + active_session_to, i, m_trg, viewer_decimate_point_cloud, viewer_reduce_rendered_trajectory, - sessions[edges[index_active_edge].index_session_to].point_clouds_container.point_clouds.at(i).render_color); + sessions[active_session_to].point_clouds_container.point_clouds.at(i).render_color, + /*useSceneColorMode=*/false, + sessions[active_session_to].point_clouds_container.xz_intersection, + sessions[active_session_to].point_clouds_container.yz_intersection, + sessions[active_session_to].point_clouds_container.xy_intersection, + sessions[active_session_to].point_clouds_container.intersection_width); } } } @@ -2940,10 +2546,10 @@ void display() observation_picking, viewer_decimate_point_cloud, viewer_reduce_rendered_trajectory, - false, - false, - false, - 100000, + sessions[first_session_index].point_clouds_container.xz_intersection, + sessions[first_session_index].point_clouds_container.yz_intersection, + sessions[first_session_index].point_clouds_container.xy_intersection, + sessions[first_session_index].point_clouds_container.intersection_width, false); } } @@ -2979,10 +2585,10 @@ void display() observation_picking, viewer_decimate_point_cloud, viewer_reduce_rendered_trajectory, - false, - false, - false, - 100000, + sessions[second_session_index].point_clouds_container.xz_intersection, + sessions[second_session_index].point_clouds_container.yz_intersection, + sessions[second_session_index].point_clouds_container.xy_intersection, + sessions[second_session_index].point_clouds_container.intersection_width, false); } } @@ -3080,7 +2686,33 @@ void display() { if (session.visible) { - renderSession(session, observation_picking, viewer_decimate_point_cloud, viewer_reduce_rendered_trajectory); + const size_t session_idx = static_cast(&session - sessions.data()); + const auto preview_it = session_drag_preview_poses.find(session_idx); + if (preview_it != session_drag_preview_poses.end()) + { + // This session's gizmo is actively being dragged this frame: draw straight + // from its cached GPU buffers with the in-progress pose, no re-upload. + const auto& preview_poses = preview_it->second; + for (size_t j = 0; j < session.point_clouds_container.point_clouds.size() && j < preview_poses.size(); j++) + { + renderScanAtPose( + static_cast(session_idx), + static_cast(j), + preview_poses[j], + viewer_decimate_point_cloud, + viewer_reduce_rendered_trajectory, + session.point_clouds_container.point_clouds[j].render_color, + /*useSceneColorMode=*/true, + session.point_clouds_container.xz_intersection, + session.point_clouds_container.yz_intersection, + session.point_clouds_container.xy_intersection, + session.point_clouds_container.intersection_width); + } + } + else + { + renderSession(session, observation_picking, viewer_decimate_point_cloud, viewer_reduce_rendered_trajectory); + } renderGroundControlPoints(session.ground_control_points, session.point_clouds_container); renderControlPoints(session.control_points, session.point_clouds_container); @@ -3137,69 +2769,6 @@ void display() } } - /*if (is_loop_closure_gui) - { - session.manual_pose_graph_loop_closure.Render(session.point_clouds_container, index_loop_closure_source, index_loop_closure_target); - } - else - { - for (const auto &g : available_geo_points) - { - glBegin(GL_LINES); - glColor3f(1.0f, 0.0f, 0.0f); - auto c = g.coordinates - session.point_clouds_container.offset; - glVertex3f(c.x() - 0.5, c.y(), c.z()); - glVertex3f(c.x() + 0.5, c.y(), c.z()); - - glVertex3f(c.x(), c.y() - 0.5, c.z()); - glVertex3f(c.x(), c.y() + 0.5, c.z()); - - glVertex3f(c.x(), c.y(), c.z() - 0.5); - glVertex3f(c.x(), c.y(), c.z() + 0.5); - glEnd(); - } - - // - for (const auto &pc : session.point_clouds_container.point_clouds) - { - for (const auto &gp : pc.available_geo_points) - { - if (gp.choosen) - { - auto c = pc.m_pose * gp.coordinates; - glBegin(GL_LINES); - glColor3f(1.0f, 0.0f, 0.0f); - glVertex3f(c.x() - 0.5, c.y(), c.z()); - glVertex3f(c.x() + 0.5, c.y(), c.z()); - - glVertex3f(c.x(), c.y() - 0.5, c.z()); - glVertex3f(c.x(), c.y() + 0.5, c.z()); - - glVertex3f(c.x(), c.y(), c.z() - 0.5); - glVertex3f(c.x(), c.y(), c.z() + 0.5); - glEnd(); - - glBegin(GL_LINES); - glColor3f(0.0f, 1.0f, 0.0f); - glVertex3f(c.x(), c.y(), c.z()); - glVertex3f(gp.coordinates.x(), gp.coordinates.y(), gp.coordinates.z()); - glEnd(); - - glColor3f(0.0f, 0.0f, 0.0f); - glBegin(GL_LINES); - glVertex3f(c.x(), c.y(), c.z()); - glVertex3f(c.x() + 10, c.y(), c.z()); - glEnd(); - - glRasterPos3f(c.x() + 10, c.y(), c.z()); - glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, (const unsigned char *)gp.name.c_str()); - } - } - } - }*/ - - // gnss.render(session.point_clouds_container); - rlImGuiBegin(); ShowMainDockSpace(); @@ -3208,6 +2777,27 @@ void display() { Eigen::Affine3d prev_pose_manipulated = Eigen::Affine3d::Identity(); Eigen::Affine3d prev_pose_after_gismo = Eigen::Affine3d::Identity(); + bool gizmo_dragging_this_frame = false; + + // Commits `candidate_poses` into session i's point clouds for real: writes m_pose, + // pose and the gui_translation/gui_rotation mirrors. This is the only place that + // mutates the session data model, so it's the only frame syncSessionRenderers() + // sees a pose change and rebuilds that session's GPU buffers. + auto commit_session_poses = [](size_t i, const std::vector& candidate_poses) + { + for (size_t j = 0; j < candidate_poses.size(); j++) + { + auto& pc = sessions[i].point_clouds_container.point_clouds[j]; + pc.m_pose = candidate_poses[j]; + pc.pose = pose_tait_bryan_from_affine_matrix(pc.m_pose); + pc.gui_translation[0] = (float)pc.pose.px; + pc.gui_translation[1] = (float)pc.pose.py; + pc.gui_translation[2] = (float)pc.pose.pz; + pc.gui_rotation[0] = (float)(pc.pose.om * RAD_TO_DEG); + pc.gui_rotation[1] = (float)(pc.pose.fi * RAD_TO_DEG); + pc.gui_rotation[2] = (float)(pc.pose.ka * RAD_TO_DEG); + } + }; for (size_t i = 0; i < sessions.size(); i++) { @@ -3252,48 +2842,44 @@ void display() m_gizmo, NULL); - sessions[i].point_clouds_container.point_clouds[0].m_pose = Eigen::Map(m_gizmo).cast(); - prev_pose_after_gismo = sessions[i].point_clouds_container.point_clouds[0].m_pose; - sessions[i].point_clouds_container.point_clouds[0].pose = - pose_tait_bryan_from_affine_matrix(sessions[i].point_clouds_container.point_clouds[0].m_pose); - - sessions[i].point_clouds_container.point_clouds[0].gui_translation[0] = - (float)sessions[i].point_clouds_container.point_clouds[0].pose.px; - sessions[i].point_clouds_container.point_clouds[0].gui_translation[1] = - (float)sessions[i].point_clouds_container.point_clouds[0].pose.py; - sessions[i].point_clouds_container.point_clouds[0].gui_translation[2] = - (float)sessions[i].point_clouds_container.point_clouds[0].pose.pz; - - sessions[i].point_clouds_container.point_clouds[0].gui_rotation[0] = - (float)(sessions[i].point_clouds_container.point_clouds[0].pose.om * RAD_TO_DEG); - sessions[i].point_clouds_container.point_clouds[0].gui_rotation[1] = - (float)(sessions[i].point_clouds_container.point_clouds[0].pose.fi * RAD_TO_DEG); - sessions[i].point_clouds_container.point_clouds[0].gui_rotation[2] = - (float)(sessions[i].point_clouds_container.point_clouds[0].pose.ka * RAD_TO_DEG); - - Eigen::Affine3d curr_m_pose = sessions[i].point_clouds_container.point_clouds[0].m_pose; - for (size_t j = 1; j < sessions[i].point_clouds_container.point_clouds.size(); j++) + gizmo_dragging_this_frame = ImGuizmo::IsUsing(); + + // Candidate pose chain from this frame's gizmo value. Computed either way + // (cheap, CPU-only) but only ever written into the session data model -- + // which is what triggers ScanRenderer's GPU re-upload -- once the drag ends. + const Eigen::Affine3d new_pose0(Eigen::Map(m_gizmo).cast()); + prev_pose_after_gismo = new_pose0; + + std::vector candidate_poses(sessions[i].point_clouds_container.point_clouds.size()); + candidate_poses[0] = new_pose0; + Eigen::Affine3d curr_m_pose = new_pose0; + for (size_t j = 1; j < all_m_poses.size(); j++) { curr_m_pose = curr_m_pose * (all_m_poses[j - 1].inverse() * all_m_poses[j]); - sessions[i].point_clouds_container.point_clouds[j].m_pose = curr_m_pose; - sessions[i].point_clouds_container.point_clouds[j].pose = - pose_tait_bryan_from_affine_matrix(sessions[i].point_clouds_container.point_clouds[j].m_pose); - - sessions[i].point_clouds_container.point_clouds[j].gui_translation[0] = - (float)sessions[i].point_clouds_container.point_clouds[j].pose.px; - sessions[i].point_clouds_container.point_clouds[j].gui_translation[1] = - (float)sessions[i].point_clouds_container.point_clouds[j].pose.py; - sessions[i].point_clouds_container.point_clouds[j].gui_translation[2] = - (float)sessions[i].point_clouds_container.point_clouds[j].pose.pz; - - sessions[i].point_clouds_container.point_clouds[j].gui_rotation[0] = - (float)(sessions[i].point_clouds_container.point_clouds[j].pose.om * RAD_TO_DEG); - sessions[i].point_clouds_container.point_clouds[j].gui_rotation[1] = - (float)(sessions[i].point_clouds_container.point_clouds[j].pose.fi * RAD_TO_DEG); - sessions[i].point_clouds_container.point_clouds[j].gui_rotation[2] = - (float)(sessions[i].point_clouds_container.point_clouds[j].pose.ka * RAD_TO_DEG); + candidate_poses[j] = curr_m_pose; + } + + if (gizmo_dragging_this_frame) + { + // Actively being dragged this frame: don't touch the session's pose -- + // that's what used to force ScanRenderer::syncPoses() to tear down and + // re-upload every scan's full GPU buffer every single frame. The render + // loop draws this session's preview from this entry instead. + session_drag_preview_poses[i] = std::move(candidate_poses); + } + else + { + const bool was_dragging = session_drag_preview_poses.erase(i) > 0; + if (was_dragging) + { + // Drag just ended: commit the final pose for real, exactly once -- + // the only frame this session's GPU buffers actually get rebuilt. + commit_session_poses(i, candidate_poses); + } + // else: gizmo is merely selected, not being dragged -- leave the + // session's pose untouched, so selecting a gizmo doesn't by itself + // cause any per-frame GPU churn. } - //} } } } @@ -3302,61 +2888,39 @@ void display() for (size_t i = 0; i < sessions.size(); i++) { // guizmo_all_sessions; - if (!sessions[i].is_gizmo && !sessions[i].is_ground_truth) + if (!sessions[i].is_gizmo && !sessions[i].is_ground_truth && !sessions[i].point_clouds_container.point_clouds.empty()) { std::vector all_m_poses; for (size_t j = 0; j < sessions[i].point_clouds_container.point_clouds.size(); j++) all_m_poses.push_back(sessions[i].point_clouds_container.point_clouds[j].m_pose); - Eigen::Affine3d m_rel_org = prev_pose_manipulated.inverse() * sessions[i].point_clouds_container.point_clouds[0].m_pose; - + Eigen::Affine3d m_rel_org = prev_pose_manipulated.inverse() * all_m_poses[0]; Eigen::Affine3d m_new = prev_pose_after_gismo * m_rel_org; - sessions[i].point_clouds_container.point_clouds[0].m_pose = m_new; - sessions[i].point_clouds_container.point_clouds[0].pose = - pose_tait_bryan_from_affine_matrix(sessions[i].point_clouds_container.point_clouds[0].m_pose); - - sessions[i].point_clouds_container.point_clouds[i].gui_translation[0] = - (float)sessions[i].point_clouds_container.point_clouds[0].pose.px; - sessions[i].point_clouds_container.point_clouds[i].gui_translation[1] = - (float)sessions[i].point_clouds_container.point_clouds[0].pose.py; - sessions[i].point_clouds_container.point_clouds[i].gui_translation[2] = - (float)sessions[i].point_clouds_container.point_clouds[0].pose.pz; - - sessions[i].point_clouds_container.point_clouds[i].gui_rotation[0] = - (float)(sessions[i].point_clouds_container.point_clouds[0].pose.om * RAD_TO_DEG); - sessions[i].point_clouds_container.point_clouds[i].gui_rotation[1] = - (float)(sessions[i].point_clouds_container.point_clouds[0].pose.fi * RAD_TO_DEG); - sessions[i].point_clouds_container.point_clouds[i].gui_rotation[2] = - (float)(sessions[i].point_clouds_container.point_clouds[0].pose.ka * RAD_TO_DEG); - - Eigen::Affine3d curr_m_pose = sessions[i].point_clouds_container.point_clouds[0].m_pose; - for (size_t j = 1; j < sessions[i].point_clouds_container.point_clouds.size(); j++) + std::vector candidate_poses(all_m_poses.size()); + candidate_poses[0] = m_new; + Eigen::Affine3d curr_m_pose = m_new; + for (size_t j = 1; j < all_m_poses.size(); j++) { curr_m_pose = curr_m_pose * (all_m_poses[j - 1].inverse() * all_m_poses[j]); - sessions[i].point_clouds_container.point_clouds[j].m_pose = curr_m_pose; - sessions[i].point_clouds_container.point_clouds[j].pose = - pose_tait_bryan_from_affine_matrix(sessions[i].point_clouds_container.point_clouds[j].m_pose); - - sessions[i].point_clouds_container.point_clouds[j].gui_translation[0] = - (float)sessions[i].point_clouds_container.point_clouds[j].pose.px; - sessions[i].point_clouds_container.point_clouds[j].gui_translation[1] = - (float)sessions[i].point_clouds_container.point_clouds[j].pose.py; - sessions[i].point_clouds_container.point_clouds[j].gui_translation[2] = - (float)sessions[i].point_clouds_container.point_clouds[j].pose.pz; - - sessions[i].point_clouds_container.point_clouds[j].gui_rotation[0] = - (float)(sessions[i].point_clouds_container.point_clouds[j].pose.om * RAD_TO_DEG); - sessions[i].point_clouds_container.point_clouds[j].gui_rotation[1] = - (float)(sessions[i].point_clouds_container.point_clouds[j].pose.fi * RAD_TO_DEG); - sessions[i].point_clouds_container.point_clouds[j].gui_rotation[2] = - (float)(sessions[i].point_clouds_container.point_clouds[j].pose.ka * RAD_TO_DEG); + candidate_poses[j] = curr_m_pose; + } + + if (gizmo_dragging_this_frame) + { + session_drag_preview_poses[i] = std::move(candidate_poses); + } + else + { + const bool was_dragging = session_drag_preview_poses.erase(i) > 0; + if (was_dragging) + commit_session_poses(i, candidate_poses); } } } } } - else + else // if (is_loop_closure_gui) { // ImGuizmo ----------------------------------------------- if (edge_gizmo && edges.size() > 0) @@ -3402,215 +2966,6 @@ void display() } } - /*if (!is_loop_closure_gui) -{ - for (size_t i = 0; i < session.point_clouds_container.point_clouds.size(); i++) - { - if (session.point_clouds_container.point_clouds[i].gizmo) - { - std::vector all_m_poses; - for (size_t j = 0; j < session.point_clouds_container.point_clouds.size(); j++) - all_m_poses.push_back(session.point_clouds_container.point_clouds[j].m_pose); - - ImGuiIO &io = ImGui::GetIO(); - // ImGuizmo ----------------------------------------------- - ImGuizmo::BeginFrame(); - ImGuizmo::Enable(true); - ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); - - if (!is_ortho) - { - GLfloat projection[16]; - glGetFloatv(GL_PROJECTION_MATRIX, projection); - - GLfloat modelview[16]; - glGetFloatv(GL_MODELVIEW_MATRIX, modelview); - - ImGuizmo::Manipulate(&modelview[0], &projection[0], ImGuizmo::TRANSLATE | ImGuizmo::ROTATE_Z | ImGuizmo::ROTATE_X | -ImGuizmo::ROTATE_Y, ImGuizmo::WORLD, m_gizmo, NULL); - } - else - ImGuizmo::Manipulate(m_ortho_gizmo_view, m_ortho_projection, ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y | -ImGuizmo::ROTATE_Z, ImGuizmo::WORLD, m_gizmo, NULL); - - session.point_clouds_container.point_clouds[i].m_pose(0, 0) = m_gizmo[0]; - session.point_clouds_container.point_clouds[i].m_pose(1, 0) = m_gizmo[1]; - session.point_clouds_container.point_clouds[i].m_pose(2, 0) = m_gizmo[2]; - session.point_clouds_container.point_clouds[i].m_pose(3, 0) = m_gizmo[3]; - session.point_clouds_container.point_clouds[i].m_pose(0, 1) = m_gizmo[4]; - session.point_clouds_container.point_clouds[i].m_pose(1, 1) = m_gizmo[5]; - session.point_clouds_container.point_clouds[i].m_pose(2, 1) = m_gizmo[6]; - session.point_clouds_container.point_clouds[i].m_pose(3, 1) = m_gizmo[7]; - session.point_clouds_container.point_clouds[i].m_pose(0, 2) = m_gizmo[8]; - session.point_clouds_container.point_clouds[i].m_pose(1, 2) = m_gizmo[9]; - session.point_clouds_container.point_clouds[i].m_pose(2, 2) = m_gizmo[10]; - session.point_clouds_container.point_clouds[i].m_pose(3, 2) = m_gizmo[11]; - session.point_clouds_container.point_clouds[i].m_pose(0, 3) = m_gizmo[12]; - session.point_clouds_container.point_clouds[i].m_pose(1, 3) = m_gizmo[13]; - session.point_clouds_container.point_clouds[i].m_pose(2, 3) = m_gizmo[14]; - session.point_clouds_container.point_clouds[i].m_pose(3, 3) = m_gizmo[15]; - session.point_clouds_container.point_clouds[i].pose = -pose_tait_bryan_from_affine_matrix(session.point_clouds_container.point_clouds[i].m_pose); - - session.point_clouds_container.point_clouds[i].gui_translation[0] = -(float)session.point_clouds_container.point_clouds[i].pose.px; session.point_clouds_container.point_clouds[i].gui_translation[1] = -(float)session.point_clouds_container.point_clouds[i].pose.py; session.point_clouds_container.point_clouds[i].gui_translation[2] = -(float)session.point_clouds_container.point_clouds[i].pose.pz; - - session.point_clouds_container.point_clouds[i].gui_rotation[0] = (float)(session.point_clouds_container.point_clouds[i].pose.om -* RAD_TO_DEG); session.point_clouds_container.point_clouds[i].gui_rotation[1] = -(float)(session.point_clouds_container.point_clouds[i].pose.fi * RAD_TO_DEG); session.point_clouds_container.point_clouds[i].gui_rotation[2] -= (float)(session.point_clouds_container.point_clouds[i].pose.ka * RAD_TO_DEG); - - if (!manipulate_only_marked_gizmo) - { - Eigen::Affine3d curr_m_pose = session.point_clouds_container.point_clouds[i].m_pose; - for (size_t j = i + 1; j < session.point_clouds_container.point_clouds.size(); j++) - { - curr_m_pose = curr_m_pose * (all_m_poses[j - 1].inverse() * all_m_poses[j]); - session.point_clouds_container.point_clouds[j].m_pose = curr_m_pose; - session.point_clouds_container.point_clouds[j].pose = -pose_tait_bryan_from_affine_matrix(session.point_clouds_container.point_clouds[j].m_pose); - - session.point_clouds_container.point_clouds[j].gui_translation[0] = -(float)session.point_clouds_container.point_clouds[j].pose.px; session.point_clouds_container.point_clouds[j].gui_translation[1] = -(float)session.point_clouds_container.point_clouds[j].pose.py; session.point_clouds_container.point_clouds[j].gui_translation[2] = -(float)session.point_clouds_container.point_clouds[j].pose.pz; - - session.point_clouds_container.point_clouds[j].gui_rotation[0] = -(float)(session.point_clouds_container.point_clouds[j].pose.om * RAD_TO_DEG); session.point_clouds_container.point_clouds[j].gui_rotation[1] -= (float)(session.point_clouds_container.point_clouds[j].pose.fi * RAD_TO_DEG); - session.point_clouds_container.point_clouds[j].gui_rotation[2] = -(float)(session.point_clouds_container.point_clouds[j].pose.ka * RAD_TO_DEG); - } - } - } - } - - session.point_clouds_container.render(observation_picking, viewer_decmiate_point_cloud); - observation_picking.render(); - - glPushAttrib(GL_ALL_ATTRIB_BITS); - glPointSize(5); - for (const auto &obs : observation_picking.observations) - { - for (const auto &[key1, value1] : obs) - { - for (const auto &[key2, value2] : obs) - { - if (key1 != key2) - { - Eigen::Vector3d p1, p2; - if (session.point_clouds_container.show_with_initial_pose) - { - p1 = session.point_clouds_container.point_clouds[key1].m_initial_pose * value1; - p2 = session.point_clouds_container.point_clouds[key2].m_initial_pose * value2; - } - else - { - p1 = session.point_clouds_container.point_clouds[key1].m_pose * value1; - p2 = session.point_clouds_container.point_clouds[key2].m_pose * value2; - } - glColor3f(0, 1, 0); - glBegin(GL_POINTS); - glVertex3f(p1.x(), p1.y(), p1.z()); - glVertex3f(p2.x(), p2.y(), p2.z()); - glEnd(); - glColor3f(1, 0, 0); - glBegin(GL_LINES); - glVertex3f(p1.x(), p1.y(), p1.z()); - glVertex3f(p2.x(), p2.y(), p2.z()); - glEnd(); - } - } - } - } - glPopAttrib(); - - for (const auto &obs : observation_picking.observations) - { - Eigen::Vector3d mean(0, 0, 0); - int counter = 0; - for (const auto &[key1, value1] : obs) - { - mean += session.point_clouds_container.point_clouds[key1].m_initial_pose * value1; - counter++; - } - if (counter > 0) - { - mean /= counter; - - glColor3f(1, 0, 0); - glBegin(GL_LINE_STRIP); - glVertex3f(mean.x() - 1, mean.y() - 1, mean.z()); - glVertex3f(mean.x() + 1, mean.y() - 1, mean.z()); - glVertex3f(mean.x() + 1, mean.y() + 1, mean.z()); - glVertex3f(mean.x() - 1, mean.y() + 1, mean.z()); - glVertex3f(mean.x() - 1, mean.y() - 1, mean.z()); - glEnd();F - } - } - - glColor3f(1, 0, 1); - glBegin(GL_POINTS); - for (auto p : picked_points) - { - glVertex3f(p.x(), p.y(), p.z()); - } - glEnd(); -} -else -{ - // ImGuizmo ----------------------------------------------- - if (session.manual_pose_graph_loop_closure.gizmo && session.manual_pose_graph_loop_closure.edges.size() > 0) - { - ImGuizmo::BeginFrame(); - ImGuizmo::Enable(true); - ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); - - if (!is_ortho) - { - GLfloat projection[16]; - glGetFloatv(GL_PROJECTION_MATRIX, projection); - - GLfloat modelview[16]; - glGetFloatv(GL_MODELVIEW_MATRIX, modelview); - - ImGuizmo::Manipulate(&modelview[0], &projection[0], ImGuizmo::TRANSLATE | ImGuizmo::ROTATE_Z | ImGuizmo::ROTATE_X | -ImGuizmo::ROTATE_Y, ImGuizmo::WORLD, m_gizmo, NULL); - } - else - ImGuizmo::Manipulate(m_ortho_gizmo_view, m_ortho_projection, ImGuizmo::TRANSLATE_X | ImGuizmo::TRANSLATE_Y | ImGuizmo::ROTATE_Z, -ImGuizmo::WORLD, m_gizmo, NULL); - - Eigen::Affine3d m_g = Eigen::Affine3d::Identity(); - - m_g(0, 0) = m_gizmo[0]; - m_g(1, 0) = m_gizmo[1]; - m_g(2, 0) = m_gizmo[2]; - m_g(3, 0) = m_gizmo[3]; - m_g(0, 1) = m_gizmo[4]; - m_g(1, 1) = m_gizmo[5]; - m_g(2, 1) = m_gizmo[6]; - m_g(3, 1) = m_gizmo[7]; - m_g(0, 2) = m_gizmo[8]; - m_g(1, 2) = m_gizmo[9]; - m_g(2, 2) = m_gizmo[10]; - m_g(3, 2) = m_gizmo[11]; - m_g(0, 3) = m_gizmo[12]; - m_g(1, 3) = m_gizmo[13]; - m_g(2, 3) = m_gizmo[14]; - m_g(3, 3) = m_gizmo[15]; - - const int &index_src = -session.manual_pose_graph_loop_closure.edges[session.manual_pose_graph_loop_closure.index_active_edge].index_from; - - const Eigen::Affine3d &m_src = session.point_clouds_container.point_clouds.at(index_src).m_pose; - session.manual_pose_graph_loop_closure.edges[session.manual_pose_graph_loop_closure.index_active_edge].relative_pose_tb = -pose_tait_bryan_from_affine_matrix(m_src.inverse() * m_g); - } -}*/ - view_kbd_shortcuts(); if (IsFileDropped()) @@ -4230,6 +3585,65 @@ pose_tait_bryan_from_affine_matrix(m_src.inverse() * m_g); ImGui::EndMenu(); } + if (ImGui::BeginMenu("Intersections")) + { + bool changed = false; + + ImGui::SetNextItemWidth(ImGuiNumberWidth); + changed |= ImGui::InputDouble("Intersection width [m]", &gui_intersection_width, 0.0, 0.0, "%.2f"); + if (gui_intersection_width < 0.001) + gui_intersection_width = 0.001; + + ImGui::Separator(); + changed |= ImGui::MenuItem("xz_intersection", nullptr, &gui_xz_intersection); + changed |= ImGui::MenuItem("10m grid##xz", nullptr, &gui_xz_grid_10x10); + changed |= ImGui::MenuItem("1m grid##xz", nullptr, &gui_xz_grid_1x1); + changed |= ImGui::MenuItem("0.1m grid##xz", nullptr, &gui_xz_grid_01x01); + if (ImGui::MenuItem("Export xz intersection", nullptr, false, gui_xz_intersection)) + export_intersection_all_sessions(true, false, false, "_xz_intersection.laz"); + + ImGui::Separator(); + changed |= ImGui::MenuItem("yz_intersection", nullptr, &gui_yz_intersection); + changed |= ImGui::MenuItem("10m grid##yz", nullptr, &gui_yz_grid_10x10); + changed |= ImGui::MenuItem("1m grid##yz", nullptr, &gui_yz_grid_1x1); + changed |= ImGui::MenuItem("0.1m grid##yz", nullptr, &gui_yz_grid_01x01); + if (ImGui::MenuItem("Export yz intersection", nullptr, false, gui_yz_intersection)) + export_intersection_all_sessions(false, true, false, "_yz_intersection.laz"); + + ImGui::Separator(); + changed |= ImGui::MenuItem("xy_intersection", nullptr, &gui_xy_intersection); + changed |= ImGui::MenuItem("10m grid##xy", nullptr, &gui_xy_grid_10x10); + changed |= ImGui::MenuItem("1m grid##xy", nullptr, &gui_xy_grid_1x1); + changed |= ImGui::MenuItem("0.1m grid##xy", nullptr, &gui_xy_grid_01x01); + if (ImGui::MenuItem("Export xy intersection", nullptr, false, gui_xy_intersection)) + export_intersection_all_sessions(false, false, true, "_xy_intersection.laz"); + + if (changed) + { + for (auto& session : sessions) + { + session.point_clouds_container.xz_intersection = gui_xz_intersection; + session.point_clouds_container.yz_intersection = gui_yz_intersection; + session.point_clouds_container.xy_intersection = gui_xy_intersection; + session.point_clouds_container.intersection_width = gui_intersection_width; + + session.point_clouds_container.xz_grid_10x10 = gui_xz_grid_10x10; + session.point_clouds_container.xz_grid_1x1 = gui_xz_grid_1x1; + session.point_clouds_container.xz_grid_01x01 = gui_xz_grid_01x01; + session.point_clouds_container.yz_grid_10x10 = gui_yz_grid_10x10; + session.point_clouds_container.yz_grid_1x1 = gui_yz_grid_1x1; + session.point_clouds_container.yz_grid_01x01 = gui_yz_grid_01x01; + session.point_clouds_container.xy_grid_10x10 = gui_xy_grid_10x10; + session.point_clouds_container.xy_grid_1x1 = gui_xy_grid_1x1; + session.point_clouds_container.xy_grid_01x01 = gui_xy_grid_01x01; + } + } + + ImGui::EndMenu(); + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Intersection menu"); + if (ImGui::BeginMenu("View")) { ImGui::BeginDisabled(!(sessions.size() > 0)); @@ -4386,7 +3800,10 @@ pose_tait_bryan_from_affine_matrix(m_src.inverse() * m_g); project_settings.session_file_names.erase(project_settings.session_file_names.begin() + idx); if (idx < sessions.size()) + { sessions.erase(sessions.begin() + idx); + session_drag_preview_poses.clear(); + } } } session_marked_for_removal.clear(); @@ -4415,8 +3832,24 @@ pose_tait_bryan_from_affine_matrix(m_src.inverse() * m_g); if (is_ndt_gui) ndt_gui(); + if (!prev_is_loop_closure_gui && is_loop_closure_gui) + { + spdlog::debug("Open loop open gui"); + on_loop_closure_gui_open(); + } if (is_loop_closure_gui) + { + // loop_closure_gui() passes &is_loop_closure_gui as the window's p_open, so clicking + // its own close (X) button flips is_loop_closure_gui to false inside this call -- the + // close check below has to run after, or that transition is never observed. loop_closure_gui(); + } + if (prev_is_loop_closure_gui && !is_loop_closure_gui) + { + spdlog::debug("Closed loop closure gui"); + on_loop_closure_gui_close(); + } + prev_is_loop_closure_gui = is_loop_closure_gui; cor_window(); @@ -4585,12 +4018,6 @@ int main(int argc, char* argv[]) { std::cerr << "System is out of memory : " << e.what() << std::endl; mandeye::fd::OutOfMemMessage(); - } catch (const std::exception& e) - { - std::cout << e.what(); - } catch (...) - { - std::cerr << "Unknown fatal error occurred." << std::endl; } return 0; diff --git a/apps/multi_session_registration/raylib_utils.cpp b/apps/multi_session_registration/raylib_utils.cpp index c7b8a55c..7d8a0618 100644 --- a/apps/multi_session_registration/raylib_utils.cpp +++ b/apps/multi_session_registration/raylib_utils.cpp @@ -109,6 +109,78 @@ namespace { return session_index >= 0 && session_index < static_cast(renderers.size()) ? renderers[session_index].get() : nullptr; } + + // Ported from PointClouds::draw_grids() (core/src/point_clouds.cpp, legacy immediate-mode GL, + // shared with the GLUT apps so it can't be changed) -- rl*() rename, one helper per cutting + // plane instead of one copy-pasted block per grid density. Same port as step 2's + // drawGridXZ/YZ/XY (apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp); + // duplicated here rather than shared because this file is itself a from-scratch per-app + // raylib port (see the header comment), not a shared library. + void drawGridXZ(float step, Color color, const PointClouds::PointCloudDimensions& dims) + { + float x_min = std::floor(dims.x_min / step) * step; + float x_max = std::ceil(dims.x_max / step) * step; + float z_min = std::floor(dims.z_min / step) * step; + float z_max = std::ceil(dims.z_max / step) * step; + + rlBegin(RL_LINES); + rlColor3f(color.r / 255.f, color.g / 255.f, color.b / 255.f); + for (float x = x_min; x <= x_max; x += step) + { + rlVertex3f(x, 0.0f, z_min); + rlVertex3f(x, 0.0f, z_max); + } + for (float z = z_min; z <= z_max; z += step) + { + rlVertex3f(x_min, 0.0f, z); + rlVertex3f(x_max, 0.0f, z); + } + rlEnd(); + } + + void drawGridYZ(float step, Color color, const PointClouds::PointCloudDimensions& dims) + { + float y_min = std::floor(dims.y_min / step) * step; + float y_max = std::ceil(dims.y_max / step) * step; + float z_min = std::floor(dims.z_min / step) * step; + float z_max = std::ceil(dims.z_max / step) * step; + + rlBegin(RL_LINES); + rlColor3f(color.r / 255.f, color.g / 255.f, color.b / 255.f); + for (float y = y_min; y <= y_max; y += step) + { + rlVertex3f(0.0f, y, z_min); + rlVertex3f(0.0f, y, z_max); + } + for (float z = z_min; z <= z_max; z += step) + { + rlVertex3f(0.0f, y_min, z); + rlVertex3f(0.0f, y_max, z); + } + rlEnd(); + } + + void drawGridXY(float step, Color color, const PointClouds::PointCloudDimensions& dims) + { + float x_min = std::floor(dims.x_min / step) * step; + float x_max = std::ceil(dims.x_max / step) * step; + float y_min = std::floor(dims.y_min / step) * step; + float y_max = std::ceil(dims.y_max / step) * step; + + rlBegin(RL_LINES); + rlColor3f(color.r / 255.f, color.g / 255.f, color.b / 255.f); + for (float x = x_min; x <= x_max; x += step) + { + rlVertex3f(x, y_min, 0.0f); + rlVertex3f(x, y_max, 0.0f); + } + for (float y = y_min; y <= y_max; y += step) + { + rlVertex3f(x_min, y, 0.0f); + rlVertex3f(x_max, y, 0.0f); + } + rlEnd(); + } } // namespace std::string truncPath(const std::string& fullPath) @@ -238,6 +310,41 @@ void showAxes() rlEnd(); } +// Draws whichever of the 9 grid-density/plane checkboxes (Intersections menu) are on, spanning +// the whole scene's bounding box. Step 3 has no single active session, so -- same pattern as the +// xz/yz/xy_intersection toggles -- the Intersections menu pushes these flags to every session +// identically; sessions[0] is read here only as the (representative) place they live. +void drawIntersectionGrids(const std::vector& sessions) +{ + if (sessions.empty()) + return; + + const auto& pcc = sessions[0].point_clouds_container; + const Color light = ColorFromNormalized(Vector4{ 0.7f, 0.7f, 0.7f, 1.0f }); + const Color dark = ColorFromNormalized(Vector4{ 0.3f, 0.3f, 0.3f, 1.0f }); + + if (pcc.xz_grid_10x10) + drawGridXZ(10.0f, light, scene_dims); + if (pcc.xz_grid_1x1) + drawGridXZ(1.0f, dark, scene_dims); + if (pcc.xz_grid_01x01) + drawGridXZ(0.1f, dark, scene_dims); + + if (pcc.yz_grid_10x10) + drawGridYZ(10.0f, light, scene_dims); + if (pcc.yz_grid_1x1) + drawGridYZ(1.0f, dark, scene_dims); + if (pcc.yz_grid_01x01) + drawGridYZ(0.1f, dark, scene_dims); + + if (pcc.xy_grid_10x10) + drawGridXY(10.0f, light, scene_dims); + if (pcc.xy_grid_1x1) + drawGridXY(1.0f, dark, scene_dims); + if (pcc.xy_grid_01x01) + drawGridXY(0.1f, dark, scene_dims); +} + void updateCameraTransition() { camera.updateEulerTransition(GetFrameTime()); @@ -447,6 +554,12 @@ void setNewRotationCenter(int x, int y) camera.moveEulerRotationCenterTo(toVec3(c)); } +void setNewRotationCenter(const Eigen::Vector3d& center) +{ + std::cout << "Setting new rotation center to:\n" << center << std::endl; + camera.moveEulerRotationCenterTo(toVec3(center)); +} + bool checkClHelp(int argc, char** argv) { for (int i = 1; i < argc; ++i) @@ -619,10 +732,10 @@ void renderScan( const ObservationPicking&, int decimate, int reduce_trajectory, - bool, - bool, - bool, - double, + bool xz_intersection, + bool yz_intersection, + bool xy_intersection, + double intersection_width, bool) { ScanRenderer* r = rendererOf(session_index); @@ -630,10 +743,31 @@ void renderScan( return; const PointCloud& pc = renderers_base[session_index].point_clouds_container.point_clouds[index]; renderScanAtPose( - session_index, index, show_with_initial_pose ? pc.m_initial_pose : pc.m_pose, decimate, reduce_trajectory, pc.render_color); + session_index, + index, + show_with_initial_pose ? pc.m_initial_pose : pc.m_pose, + decimate, + reduce_trajectory, + pc.render_color, + /*useSceneColorMode=*/false, + xz_intersection, + yz_intersection, + xy_intersection, + intersection_width); } -void renderScanAtPose(int session_index, int index, const Eigen::Affine3d& pose, int, int reduce_trajectory, const float color[3]) +void renderScanAtPose( + int session_index, + int index, + const Eigen::Affine3d& pose, + int, + int reduce_trajectory, + const float color[3], + bool useSceneColorMode, + bool xzIntersection, + bool yzIntersection, + bool xyIntersection, + double intersectionWidth) { ScanRenderer* r = rendererOf(session_index); if (!r || index < 0 || index >= static_cast(renderers_base[session_index].point_clouds_container.point_clouds.size())) @@ -642,12 +776,21 @@ void renderScanAtPose(int session_index, int index, const Eigen::Affine3d& pose, if (!pc.visible) return; + const Vector3 rc = camera.euler.rotationCenter; r->drawCachedWithTransform( static_cast(index), pose * pc.m_pose.inverse(), ColorFromNormalized(Vector4{ color[0], color[1], color[2], 1.f }), static_cast(pc.point_size), - false); + useSceneColorMode ? color_mode : ScanColorMode::Flat, + static_cast(scene_dims.z_min), + static_cast(scene_dims.z_max), + Eigen::Vector3d(rc.x, rc.y, rc.z), + static_cast(std::max({ scene_dims.length, scene_dims.width, scene_dims.height, 1.0 })), + xzIntersection, + yzIntersection, + xyIntersection, + static_cast(intersectionWidth)); const size_t stride = std::max(1, reduce_trajectory); rlBegin(RL_LINES); diff --git a/apps/multi_session_registration/raylib_utils.h b/apps/multi_session_registration/raylib_utils.h index f796f9cc..aa12d9c9 100644 --- a/apps/multi_session_registration/raylib_utils.h +++ b/apps/multi_session_registration/raylib_utils.h @@ -78,6 +78,8 @@ void motion(int x, int y); void reshape(int w, int h); bool initGL(int* argc, char** argv, const std::string& winTitle, void (*display)(), void (*mouse)(int, int, int, int)); void showAxes(); +//! Draws the active Intersections-menu grid overlays (10m/1m/0.1m, per xz/yz/xy plane). +void drawIntersectionGrids(const std::vector& sessions); void updateCameraTransition(); void updateOrthoView(); void camMenu(); @@ -100,6 +102,9 @@ void getClosestTrajectoriesPoint( bool KeyShift, double& time_stamp_offset); 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. +void setNewRotationCenter(const Eigen::Vector3d& center); bool checkClHelp(int argc, char** argv); //! Replaces glutMainLoop(): polls raylib input into the mouse()/motion()/wheel() callbacks, calls display() per frame. @@ -145,7 +150,25 @@ void renderScan( double intersection_width, bool visible_imu_diff); //! Replaces PointCloud::render(pose, ...): points drawn at `pose`, trajectory at the scan's m_pose. -void renderScanAtPose(int session_index, int index, const Eigen::Affine3d& pose, int decimate, int reduce_trajectory, const float color[3]); +//! useSceneColorMode: shade points the same way the live render does (View > Points color's +//! color_mode, with the current scene's elevation/distance bounds) instead of flat `color` -- for +//! a gizmo-drag preview that should otherwise look identical to the real render. Off (the default) +//! keeps existing callers' flat highlight color (e.g. the loop-closure edge preview's red/blue). +//! xzIntersection/yzIntersection/xyIntersection/intersectionWidth: same cross-section slicing +//! draw() applies (View > Intersections); off (the default) draws every point, for callers that +//! want an unsliced preview regardless of the current global setting (e.g. the edge highlight). +void renderScanAtPose( + int session_index, + int index, + const Eigen::Affine3d& pose, + int decimate, + int reduce_trajectory, + const float color[3], + bool useSceneColorMode = false, + bool xzIntersection = false, + bool yzIntersection = false, + bool xyIntersection = false, + double intersectionWidth = 0.1); //! Replace GroundControlPoints::render() / ControlPoints::render(pcs, false), which use legacy GL. void renderGroundControlPoints(const GroundControlPoints& ground_control_points, const PointClouds& point_clouds_container); void renderControlPoints(const ControlPoints& control_points, const PointClouds& point_clouds_container); diff --git a/apps/multi_view_tls_registration/multi_view_tls_registration.cpp b/apps/multi_view_tls_registration/multi_view_tls_registration.cpp index a9473ea7..941ca1be 100644 --- a/apps/multi_view_tls_registration/multi_view_tls_registration.cpp +++ b/apps/multi_view_tls_registration/multi_view_tls_registration.cpp @@ -26,85 +26,6 @@ void initial_pose_to_identity(Session& session) } } -void save_intersection( - const Session& session, - std::string output_las_name, - bool xz_intersection, - bool yz_intersection, - bool xy_intersection, - double intersection_width) -{ - std::vector pointcloud; - std::vector intensity; - std::vector timestamps; - - for (auto& p : session.point_clouds_container.point_clouds) - { - if (p.visible) - { - for (int i = 0; i < p.points_local.size(); i++) - { - const auto& pp = p.points_local[i]; - Eigen::Vector3d vp; - vp = p.m_pose * pp; // + session.point_clouds_container.offset; - - bool is_inside = false; - if (xz_intersection) - { - if (fabs(vp.y()) < intersection_width) - { - is_inside = true; - } - } - - if (yz_intersection) - { - if (fabs(vp.x()) < intersection_width) - { - is_inside = true; - } - } - - if (xy_intersection) - { - if (fabs(vp.z()) < intersection_width) - { - is_inside = true; - } - } - - if (is_inside) - { - pointcloud.push_back(vp); - if (i < p.intensities.size()) - { - intensity.push_back(p.intensities[i]); - } - else - { - intensity.push_back(0); - } - if (i < p.timestamps.size()) - { - timestamps.push_back(p.timestamps[i]); - } - } - } - } - } - if (!exportLaz( - output_las_name, - pointcloud, - intensity, - timestamps, - session.point_clouds_container.offset.x(), - session.point_clouds_container.offset.y(), - session.point_clouds_container.offset.z())) - { - std::cout << "problem with saving file: " << output_las_name << std::endl; - } -} - void save_separately_to_las(const Session& session, fs::path outwd, std::string extension) { const auto& clouds = session.point_clouds_container.point_clouds; 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 b7a75c90..d5448f60 100644 --- a/apps/multi_view_tls_registration/multi_view_tls_registration.h +++ b/apps/multi_view_tls_registration/multi_view_tls_registration.h @@ -148,14 +148,6 @@ bool has_extension(const std::string file_path, const std::string extension); void initial_pose_to_identity(Session& session); -void save_intersection( - const Session& session, - std::string output_las_name, - bool xz_intersection, - bool yz_intersection, - bool xy_intersection, - double intersection_width); - void save_separately_to_las(const Session& session, fs::path outwd, std::string extension = ".las"); void save_trajectories_to_laz( 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 6a4d2c44..27af27ec 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 @@ -3704,7 +3704,7 @@ void renderLoopClosure( static_cast(pointClouds[idx].render_color[2] * 255.f), 255 }; scan_renderer.drawCachedWithTransform( - static_cast(idx), delta, c, static_cast(pointClouds[idx].point_size), false); + static_cast(idx), delta, c, static_cast(pointClouds[idx].point_size), ScanColorMode::Flat); } } } @@ -5680,9 +5680,9 @@ void display() save_intersection( session, output_file_name, - session.point_clouds_container.xz_intersection, - session.point_clouds_container.yz_intersection, - session.point_clouds_container.xy_intersection, + /*xz_intersection=*/true, + /*yz_intersection=*/false, + /*xy_intersection=*/false, session.point_clouds_container.intersection_width); } } @@ -5704,9 +5704,9 @@ void display() save_intersection( session, output_file_name, - session.point_clouds_container.xz_intersection, - session.point_clouds_container.yz_intersection, - session.point_clouds_container.xy_intersection, + /*xz_intersection=*/false, + /*yz_intersection=*/true, + /*xy_intersection=*/false, session.point_clouds_container.intersection_width); } } @@ -5728,9 +5728,9 @@ void display() save_intersection( session, output_file_name, - session.point_clouds_container.xz_intersection, - session.point_clouds_container.yz_intersection, - session.point_clouds_container.xy_intersection, + /*xz_intersection=*/false, + /*yz_intersection=*/false, + /*xy_intersection=*/true, session.point_clouds_container.intersection_width); } } diff --git a/apps/multi_view_tls_registration_legacy/multi_view_tls_registration.cpp b/apps/multi_view_tls_registration_legacy/multi_view_tls_registration.cpp index a9473ea7..941ca1be 100644 --- a/apps/multi_view_tls_registration_legacy/multi_view_tls_registration.cpp +++ b/apps/multi_view_tls_registration_legacy/multi_view_tls_registration.cpp @@ -26,85 +26,6 @@ void initial_pose_to_identity(Session& session) } } -void save_intersection( - const Session& session, - std::string output_las_name, - bool xz_intersection, - bool yz_intersection, - bool xy_intersection, - double intersection_width) -{ - std::vector pointcloud; - std::vector intensity; - std::vector timestamps; - - for (auto& p : session.point_clouds_container.point_clouds) - { - if (p.visible) - { - for (int i = 0; i < p.points_local.size(); i++) - { - const auto& pp = p.points_local[i]; - Eigen::Vector3d vp; - vp = p.m_pose * pp; // + session.point_clouds_container.offset; - - bool is_inside = false; - if (xz_intersection) - { - if (fabs(vp.y()) < intersection_width) - { - is_inside = true; - } - } - - if (yz_intersection) - { - if (fabs(vp.x()) < intersection_width) - { - is_inside = true; - } - } - - if (xy_intersection) - { - if (fabs(vp.z()) < intersection_width) - { - is_inside = true; - } - } - - if (is_inside) - { - pointcloud.push_back(vp); - if (i < p.intensities.size()) - { - intensity.push_back(p.intensities[i]); - } - else - { - intensity.push_back(0); - } - if (i < p.timestamps.size()) - { - timestamps.push_back(p.timestamps[i]); - } - } - } - } - } - if (!exportLaz( - output_las_name, - pointcloud, - intensity, - timestamps, - session.point_clouds_container.offset.x(), - session.point_clouds_container.offset.y(), - session.point_clouds_container.offset.z())) - { - std::cout << "problem with saving file: " << output_las_name << std::endl; - } -} - void save_separately_to_las(const Session& session, fs::path outwd, std::string extension) { const auto& clouds = session.point_clouds_container.point_clouds; 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 d9645053..26e32189 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 @@ -148,14 +148,6 @@ bool has_extension(const std::string file_path, const std::string extension); void initial_pose_to_identity(Session& session); -void save_intersection( - const Session& session, - std::string output_las_name, - bool xz_intersection, - bool yz_intersection, - bool xy_intersection, - double intersection_width); - void save_separately_to_las(const Session& session, fs::path outwd, std::string extension = ".las"); void save_trajectories_to_laz( 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 4dee9eda..84c26ef2 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 @@ -3421,9 +3421,9 @@ void display() save_intersection( session, output_file_name, - session.point_clouds_container.xz_intersection, - session.point_clouds_container.yz_intersection, - session.point_clouds_container.xy_intersection, + /*xz_intersection=*/true, + /*yz_intersection=*/false, + /*xy_intersection=*/false, session.point_clouds_container.intersection_width); } } @@ -3445,9 +3445,9 @@ void display() save_intersection( session, output_file_name, - session.point_clouds_container.xz_intersection, - session.point_clouds_container.yz_intersection, - session.point_clouds_container.xy_intersection, + /*xz_intersection=*/false, + /*yz_intersection=*/true, + /*xy_intersection=*/false, session.point_clouds_container.intersection_width); } } @@ -3469,9 +3469,9 @@ void display() save_intersection( session, output_file_name, - session.point_clouds_container.xz_intersection, - session.point_clouds_container.yz_intersection, - session.point_clouds_container.xy_intersection, + /*xz_intersection=*/false, + /*yz_intersection=*/false, + /*xy_intersection=*/true, session.point_clouds_container.intersection_width); } } diff --git a/core/include/Core/export_laz.h b/core/include/Core/export_laz.h index b3a6ef09..b2545507 100644 --- a/core/include/Core/export_laz.h +++ b/core/include/Core/export_laz.h @@ -1,4 +1,4 @@ -#pragma once +#pragma once // TODO(mwlasiuk) : do this at the end (print -> spdlog) as it is .h and not cpp + hpp and that breaks some subprojects ... @@ -402,6 +402,61 @@ inline void points_to_vector( } } +//! Writes the points of `session` that fall inside the active xz/yz/xy intersection slab(s) (within +//! `intersection_width` of the respective plane) to one laz file. Shared by step 2 (one session, +//! picked via a file dialog) and step 3 (one call per session, auto-named -- see +//! export_intersection_all_sessions() in multi_session_registration.cpp). +inline void save_intersection( + const Session& session, + const std::string& output_las_name, + bool xz_intersection, + bool yz_intersection, + bool xy_intersection, + double intersection_width) +{ + std::vector pointcloud; + std::vector intensity; + std::vector timestamps; + + for (auto& p : session.point_clouds_container.point_clouds) + { + if (!p.visible) + continue; + + for (size_t i = 0; i < p.points_local.size(); i++) + { + Eigen::Vector3d vp = p.m_pose * p.points_local[i]; + + bool is_inside = false; + if (xz_intersection && fabs(vp.y()) < intersection_width) + is_inside = true; + if (yz_intersection && fabs(vp.x()) < intersection_width) + is_inside = true; + if (xy_intersection && fabs(vp.z()) < intersection_width) + is_inside = true; + + if (is_inside) + { + pointcloud.push_back(vp); + intensity.push_back(i < p.intensities.size() ? p.intensities[i] : 0); + timestamps.push_back(i < p.timestamps.size() ? p.timestamps[i] : 0.0); + } + } + } + + if (!exportLaz( + output_las_name, + pointcloud, + intensity, + timestamps, + session.point_clouds_container.offset.x(), + session.point_clouds_container.offset.y(), + session.point_clouds_container.offset.z())) + { + std::cerr << "problem with saving file: " << output_las_name << std::endl; + } +} + inline void save_all_to_las(const Session& session, std::string output_las_name, bool as_local, bool skip_ts_0) { Eigen::Affine3d first_pose = Eigen::Affine3d::Identity(); diff --git a/core/include/Core/raylib_render.hpp b/core/include/Core/raylib_render.hpp index 5bead031..8b5d8614 100644 --- a/core/include/Core/raylib_render.hpp +++ b/core/include/Core/raylib_render.hpp @@ -198,13 +198,30 @@ class ScanRenderer // it's folded into the MVP matrix for this draw call rather than applied // to the points themselves. For overlays that need to preview a scan at // a pose other than its stored one without paying for a CPU retransform - // every frame (e.g. a loop-closure edge's in-progress relative pose). If - // useIntensityColor is set, points use the same jet colormap draw()'s - // colorByIntensity uses; otherwise every point draws flat in color - // (color.a is used as alpha either way). Does nothing if index is out - // of range or the scan has no cached buffer yet. + // every frame (e.g. a loop-closure edge's in-progress relative pose, or a + // gizmo-drag preview). colorMode selects the shader's gradient exactly as + // draw()'s own colorMode does (elevationMin/Max and distanceCenter/Max only + // matter for Elevation/Distance); the default, Flat, draws every point in + // `color` with no gradient, matching this function's behavior before these + // four trailing parameters existed (color.a is used as alpha either way). + // Does nothing if index is out of range or the scan has no cached buffer yet. + // xzIntersection/yzIntersection/xyIntersection/intersectionWidth: same cross-section slab + // filter draw() applies (see its own doc comment); off by default, so a caller that doesn't + // pass these draws every point regardless of the current global setting. void drawCachedWithTransform( - size_t index, const Eigen::Affine3d& extraTransform, Color color, float pointSize, bool useIntensityColor) const; + size_t index, + const Eigen::Affine3d& extraTransform, + Color color, + float pointSize, + ScanColorMode colorMode = ScanColorMode::Flat, + float elevationMin = 0.f, + float elevationMax = 1.f, + const Eigen::Vector3d& distanceCenter = Eigen::Vector3d::Zero(), + float distanceMax = 1.f, + bool xzIntersection = false, + bool yzIntersection = false, + bool xyIntersection = false, + float intersectionWidth = 0.1f) 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 diff --git a/core/src/raylib_render.cpp b/core/src/raylib_render.cpp index a8b9af1a..3a3c34ec 100644 --- a/core/src/raylib_render.cpp +++ b/core/src/raylib_render.cpp @@ -396,7 +396,19 @@ namespace } // namespace void ScanRenderer::drawCachedWithTransform( - size_t index, const Eigen::Affine3d& extraTransform, Color color, float pointSize, bool useIntensityColor) const + size_t index, + const Eigen::Affine3d& extraTransform, + Color color, + float pointSize, + ScanColorMode colorMode, + float elevationMin, + float elevationMax, + const Eigen::Vector3d& distanceCenter, + float distanceMax, + bool xzIntersection, + bool yzIntersection, + bool xyIntersection, + float intersectionWidth) const { if (!shaderValid_ || index >= clouds_.size()) { @@ -418,20 +430,30 @@ void ScanRenderer::drawCachedWithTransform( Matrix mvp = MatrixMultiply(toRaylibMatrix(extraTransform), mvpBase); float colorF[4] = { color.r / 255.f, color.g / 255.f, color.b / 255.f, color.a / 255.f }; - int colorMode = useIntensityColor ? 1 : 0; + int colorModeInt = static_cast(colorMode); rlEnableShader(shader_.id); rlSetUniformMatrix(locMVP_, mvp); rlSetUniform(locPointSize_, &pointSize, RL_SHADER_UNIFORM_FLOAT, 1); rlSetUniform(locColor_, colorF, RL_SHADER_UNIFORM_VEC4, 1); - rlSetUniform(locColorMode_, &colorMode, RL_SHADER_UNIFORM_INT, 1); - // Explicitly off: uniform program state persists across draw calls - // sharing this shader, and this preview draw should never be - // slab-filtered regardless of what draw() last set these to. - int intersectionOff = 0; - rlSetUniform(locXzOn_, &intersectionOff, RL_SHADER_UNIFORM_INT, 1); - rlSetUniform(locYzOn_, &intersectionOff, RL_SHADER_UNIFORM_INT, 1); - rlSetUniform(locXyOn_, &intersectionOff, RL_SHADER_UNIFORM_INT, 1); + rlSetUniform(locColorMode_, &colorModeInt, RL_SHADER_UNIFORM_INT, 1); + rlSetUniform(locElevMin_, &elevationMin, RL_SHADER_UNIFORM_FLOAT, 1); + rlSetUniform(locElevMax_, &elevationMax, RL_SHADER_UNIFORM_FLOAT, 1); + float distCenterF[3] = { static_cast(distanceCenter.x()), + static_cast(distanceCenter.y()), + static_cast(distanceCenter.z()) }; + rlSetUniform(locDistCenter_, distCenterF, RL_SHADER_UNIFORM_VEC3, 1); + rlSetUniform(locDistMax_, &distanceMax, RL_SHADER_UNIFORM_FLOAT, 1); + // Set unconditionally (not just when on): uniform program state persists across + // draw calls sharing this shader, so a caller that wants this draw unsliced + // regardless of what draw() last set these to must still explicitly turn them off. + int xzOnInt = xzIntersection ? 1 : 0; + int yzOnInt = yzIntersection ? 1 : 0; + int xyOnInt = xyIntersection ? 1 : 0; + rlSetUniform(locXzOn_, &xzOnInt, RL_SHADER_UNIFORM_INT, 1); + rlSetUniform(locYzOn_, &yzOnInt, RL_SHADER_UNIFORM_INT, 1); + rlSetUniform(locXyOn_, &xyOnInt, RL_SHADER_UNIFORM_INT, 1); + rlSetUniform(locIntersectionWidth_, &intersectionWidth, RL_SHADER_UNIFORM_FLOAT, 1); for (const auto& part : gpu.parts) { diff --git a/shared/include/HDMapping/Assert.h b/shared/include/HDMapping/Assert.h new file mode 100644 index 00000000..830038ba --- /dev/null +++ b/shared/include/HDMapping/Assert.h @@ -0,0 +1,31 @@ +#pragma once + +#include +#include + +// Unlike assert(), HDMAPPING_ASSERT is never compiled out by NDEBUG: a failed invariant is +// always reported. Debug builds abort immediately so the bug is impossible to miss during +// development; release builds (NDEBUG) only log a warning and continue, since aborting a +// shipped app on an invariant violation is worse for users than limping on in a degraded +// state (e.g. a swallowed std::out_of_range from an unchecked session/point index). +// +// Define HDMAPPING_ASSERT_ALWAYS_ABORT (e.g. via target_compile_definitions on one target) +// to force the abort even with NDEBUG defined -- for debugging a RelWithDebInfo/Release +// build without flipping NDEBUG, and everything else gated on it, for the whole project. +#if !defined(NDEBUG) || defined(HDMAPPING_ASSERT_ALWAYS_ABORT) +#define HDMAPPING_ASSERT_ABORT() std::abort() +#else +#define HDMAPPING_ASSERT_ABORT() ((void)0) +#endif + +// message is optional: HDMAPPING_ASSERT(cond) or HDMAPPING_ASSERT(cond, "why") +#define HDMAPPING_ASSERT(condition, ...) \ + do \ + { \ + if (!(condition)) \ + { \ + std::cerr << "HDMAPPING_ASSERT failed: " #condition << " at " << __FILE__ << ":" \ + << __LINE__ __VA_OPT__(<< " -- " << (__VA_ARGS__)) << std::endl; \ + HDMAPPING_ASSERT_ABORT(); \ + } \ + } while (false)