From 2061088095ac9d831e046bc83e542c649d8f32b7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Fri, 2 Oct 2026 14:55:37 +0200 Subject: [PATCH 1/3] Changes to step 3. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - GPU churn fix (headline): gizmo drag no longer rewrites every scan's pose each frame — candidate poses are stashed and drawn via a transform-only path until the drag ends, so point buffers aren't torn down/re-uploaded every frame. Drag preview also now respects the real color mode (not flat-only). - Loop-closure edge UI: added sliders for active edge / source / target indices (fixes #549), aligned on one line with their InputInts. - Intersections (xz/yz/xy slicing): ported step 2's View > Intersections menu to step 3 (global toggles instead of per-session), added grid overlays and per-session export, and wired it into the loop-closure/drag preview rendering too. - save_intersection() dedup: moved the per-session slab-export code into shared Core/export_laz.h; fixed a latent array-desync bug (timestamps array could end up shorter than intensity/pointcloud) found while moving it; removed the now-ambiguous legacy duplicate that broke MSVC/Windows CI. - ICP ground-truth bug: fixed source being built from the edge's raw (un-reordered) index instead of the swapped local, which silently aligned the ground-truth scan against a filtered copy of itself when ground truth was on the edge's "to" side. - Edge-activation crash hardening: fixed inverted bounds check + wrong index type in canEdgeBeActivated(), stale-session bug in setActiveEdge(), a visibility-set collapse bug in settings_gui(), and a session-index mismatch in display()'s active-edge rendering (indexing one session's data with another session's bounds). Also fixed loop-closure open/close lifecycle not firing on title-bar close, and a disabled "Set Active" button that didn't actually disable. - Safety net: removed a blanket top-level catch(...) that was silently swallowing exceptions (including the above bugs) and returning 0; added HDMAPPING_ASSERT (survives NDEBUG, aborts in debug, logs in release unless HDMAPPING_ASSERT_ALWAYS_ABORT). - Cleanup: removed ~200 lines of dead pre-raylib/GLUT code from multi_session_registration.cpp. Co-Authored-By: Claude Sonnet 5 --- CMakeLists.txt | 7 + .../multi_session_registration/CMakeLists.txt | 3 +- .../multi_session_registration.cpp | 1616 ++++++----------- .../raylib_utils.cpp | 157 +- .../multi_session_registration/raylib_utils.h | 25 +- .../multi_view_tls_registration.cpp | 79 - .../multi_view_tls_registration.h | 8 - .../multi_view_tls_registration_gui.cpp | 2 +- .../multi_view_tls_registration.cpp | 79 - .../multi_view_tls_registration.h | 8 - core/include/Core/export_laz.h | 57 +- core/include/Core/raylib_render.hpp | 29 +- core/src/raylib_render.cpp | 42 +- shared/include/HDMapping/Assert.h | 31 + 14 files changed, 850 insertions(+), 1293 deletions(-) create mode 100644 shared/include/HDMapping/Assert.h 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..96f8831c 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 @@ -34,6 +36,7 @@ #endif #include +#include #ifdef _WIN32 #include "resource.h" @@ -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,178 @@ 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 +579,28 @@ 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.size() > 1, "First session should have at least one cloud"); + HDMAPPING_ASSERT(secondSessionRef.point_clouds_container.point_clouds.size() > 1, "Second session should have at least one cloud"); + + const int firstSessionUpper = firstSessionRef.point_clouds_container.point_clouds.size(); + const int secondSessionUpper = secondSessionRef.point_clouds_container.point_clouds.size(); - ImGui::InputInt("gui_point_size", &gui_point_size); - if (gui_point_size < 1) - gui_point_size = 1; + { + // 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,31 @@ 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 +2223,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) { @@ -2694,10 +2301,6 @@ void settings_gui() search_radius = 0.01; } - // if (!is_loop_closure_gui && prev_is_loop_closure_gui) - //{ - // exit(1); - // } } } } @@ -2723,19 +2326,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 +2339,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 +2412,7 @@ void display() captureFrameMatrices(); showAxes(); + drawIntersectionGrids(sessions); if (is_loop_closure_gui) { @@ -2845,62 +2441,76 @@ void display() int index_src = edges[index_active_edge].index_from; int index_trg = edges[index_active_edge].index_to; + // 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[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds.at(index_src).m_pose; + 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 + 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 +2550,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 +2589,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 +2690,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 +2773,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 +2781,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 +2846,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 +2892,40 @@ 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 +2971,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 +3590,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(gui_xz_intersection, gui_yz_intersection, gui_xy_intersection, "_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(gui_xz_intersection, gui_yz_intersection, gui_xy_intersection, "_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(gui_xz_intersection, gui_yz_intersection, gui_xy_intersection, "_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 +3805,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 +3837,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("Closed 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 +4023,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..a996325c 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); } } } 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/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..4dc801c7 --- /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) From 0f41456531615cdfe9d99ee36f6a6172c825bf7c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Tue, 6 Oct 2026 13:49:22 +0200 Subject: [PATCH 2/3] Format with clang-format 21 to match CI Co-Authored-By: Claude Sonnet 5 --- .../multi_session_registration.cpp | 29 ++++++++----------- shared/include/HDMapping/Assert.h | 18 ++++++------ 2 files changed, 21 insertions(+), 26 deletions(-) diff --git a/apps/multi_session_registration/multi_session_registration.cpp b/apps/multi_session_registration/multi_session_registration.cpp index 96f8831c..f2e195d3 100644 --- a/apps/multi_session_registration/multi_session_registration.cpp +++ b/apps/multi_session_registration/multi_session_registration.cpp @@ -35,8 +35,8 @@ #undef ShowCursor #endif -#include #include +#include #ifdef _WIN32 #include "resource.h" @@ -401,8 +401,9 @@ 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"); + 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); @@ -415,7 +416,7 @@ bool canEdgeBeActivated(const int i) { return false; } - const auto &e = edges[i]; + const auto& e = edges[i]; return visible_sessions.contains(e.index_session_from) && visible_sessions.contains(e.index_session_to); } @@ -424,7 +425,7 @@ bool canEdgeBeActivated(const int i) // otherwise it would write the previous edge's pose into this one. void setActiveEdge(const int i) { - HDMAPPING_ASSERT(i >= 0 && i < (int)edges.size(), "Edge outside bounds" ); + HDMAPPING_ASSERT(i >= 0 && i < (int)edges.size(), "Edge outside bounds"); if (i < 0 || i >= (int)edges.size()) return; @@ -580,7 +581,6 @@ void on_loop_closure_gui_open() s.is_gizmo = false; } manipulate_active_edge = false; - } void on_loop_closure_gui_close() { @@ -2182,7 +2182,6 @@ void settings_gui() 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; @@ -2300,7 +2299,6 @@ void settings_gui() if (search_radius < 0.01) search_radius = 0.01; } - } } } @@ -2448,8 +2446,7 @@ void display() 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_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 = @@ -2495,8 +2492,7 @@ void display() // 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[active_session_to].point_clouds_container.point_clouds.at(i).m_pose; + 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); renderScanAtPose( @@ -2892,8 +2888,7 @@ void display() for (size_t i = 0; i < sessions.size(); i++) { // guizmo_all_sessions; - if (!sessions[i].is_gizmo && !sessions[i].is_ground_truth && - !sessions[i].point_clouds_container.point_clouds.empty()) + 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++) @@ -2925,7 +2920,7 @@ void display() } } } - else //if (is_loop_closure_gui) + else // if (is_loop_closure_gui) { // ImGuizmo ----------------------------------------------- if (edge_gizmo && edges.size() > 0) @@ -3837,7 +3832,7 @@ void display() if (is_ndt_gui) ndt_gui(); - if (!prev_is_loop_closure_gui && is_loop_closure_gui) + if (!prev_is_loop_closure_gui && is_loop_closure_gui) { spdlog::debug("Closed loop open gui"); on_loop_closure_gui_open(); @@ -3849,7 +3844,7 @@ void display() // 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) + if (prev_is_loop_closure_gui && !is_loop_closure_gui) { spdlog::debug("Closed loop closure gui"); on_loop_closure_gui_close(); diff --git a/shared/include/HDMapping/Assert.h b/shared/include/HDMapping/Assert.h index 4dc801c7..830038ba 100644 --- a/shared/include/HDMapping/Assert.h +++ b/shared/include/HDMapping/Assert.h @@ -19,13 +19,13 @@ #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(); \ - } \ +#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) From 408f34b7ceffa5956981cf37d715568620dab613 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Tue, 6 Oct 2026 17:46:04 +0200 Subject: [PATCH 3/3] Fix Export intersection menu exporting the union of all enabled planes Each "Export xz/yz/xy intersection" item passed all three xz_intersection/yz_intersection/xy_intersection flags to save_intersection()/export_intersection_all_sessions() instead of just its own plane, so enabling more than one plane made every export button dump the same union of slabs. Fixed in step 3, step 2, and the legacy step 2 app. Co-Authored-By: Claude Sonnet 5 --- .../multi_session_registration.cpp | 12 ++++++------ .../multi_view_tls_registration_gui.cpp | 18 +++++++++--------- .../multi_view_tls_registration_gui.cpp | 18 +++++++++--------- 3 files changed, 24 insertions(+), 24 deletions(-) diff --git a/apps/multi_session_registration/multi_session_registration.cpp b/apps/multi_session_registration/multi_session_registration.cpp index f2e195d3..f613f3fe 100644 --- a/apps/multi_session_registration/multi_session_registration.cpp +++ b/apps/multi_session_registration/multi_session_registration.cpp @@ -616,8 +616,8 @@ void loop_closure_gui() // visible session const auto& firstSessionRef = sessions[first_session_index]; const auto& secondSessionRef = sessions[second_session_index]; - HDMAPPING_ASSERT(firstSessionRef.point_clouds_container.point_clouds.size() > 1, "First session should have at least one cloud"); - HDMAPPING_ASSERT(secondSessionRef.point_clouds_container.point_clouds.size() > 1, "Second session should have at least one cloud"); + 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"); const int firstSessionUpper = firstSessionRef.point_clouds_container.point_clouds.size(); const int secondSessionUpper = secondSessionRef.point_clouds_container.point_clouds.size(); @@ -3600,7 +3600,7 @@ void display() 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(gui_xz_intersection, gui_yz_intersection, gui_xy_intersection, "_xz_intersection.laz"); + export_intersection_all_sessions(true, false, false, "_xz_intersection.laz"); ImGui::Separator(); changed |= ImGui::MenuItem("yz_intersection", nullptr, &gui_yz_intersection); @@ -3608,7 +3608,7 @@ void display() 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(gui_xz_intersection, gui_yz_intersection, gui_xy_intersection, "_yz_intersection.laz"); + export_intersection_all_sessions(false, true, false, "_yz_intersection.laz"); ImGui::Separator(); changed |= ImGui::MenuItem("xy_intersection", nullptr, &gui_xy_intersection); @@ -3616,7 +3616,7 @@ void display() 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(gui_xz_intersection, gui_yz_intersection, gui_xy_intersection, "_xy_intersection.laz"); + export_intersection_all_sessions(false, false, true, "_xy_intersection.laz"); if (changed) { @@ -3834,7 +3834,7 @@ void display() if (!prev_is_loop_closure_gui && is_loop_closure_gui) { - spdlog::debug("Closed loop open gui"); + spdlog::debug("Open loop open gui"); on_loop_closure_gui_open(); } if (is_loop_closure_gui) 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 a996325c..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 @@ -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_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); } }