From 920ee017e604cc55c16a6e2a7b9b7991cf648787 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Sat, 25 Jul 2026 03:10:24 +0200 Subject: [PATCH 01/17] Port step2 (multi_view_tls_registration) from GLUT to raylib Replaces GLUT + legacy immediate-mode OpenGL with raylib in place, in multi_view_tls_registration_gui.cpp: same globals, function names, and structure as before, but windowing/input/camera/picking/mini-compass (previously from the shared core/src/utils.cpp, which other GLUT apps still depend on and so can't be changed) are now local re-implementations using rlgl's rl*() legacy-GL-emulation API. Point-cloud and loop-closure rendering (previously core's shared legacy-GL .render() methods) go through a new core_raylib target's ScanRenderer instead, which also adds per-point jet-colormap gradient coloring by intensity, height, or distance from the rotation center (view menu), and pose/edge index number labels in the loop closure overlay -- none of which the GLUT app had. GNSS/ControlPoints/GroundControlPoints 3D rendering and the observation- picking Intersection wireframes are not yet ported (their ImGui panels still work); noted with comments at each call site. Co-Authored-By: Claude Sonnet 5 Claude-Session: https://claude.ai/code/session_01XgxCVLnEEAftKKRvZHPUB9 --- CMakeLists.txt | 9 +- .../CMakeLists.txt | 41 +- .../multi_view_tls_registration_gui.cpp | 2274 +++++++++++++++-- cmake/raylib.cmake | 69 + core/CMakeLists.txt | 41 +- core/include/Core/raylib_render.hpp | 174 ++ core/src/raylib_render.cpp | 580 +++++ 7 files changed, 2916 insertions(+), 272 deletions(-) create mode 100644 cmake/raylib.cmake create mode 100644 core/include/Core/raylib_render.hpp create mode 100644 core/src/raylib_render.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 1fe58065..169448ee 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -90,6 +90,13 @@ include(cmake/implot.cmake) # ============================================================================ include(cmake/dependencies.cmake) +# raylib/imgui_raylib/rlimgui/imguizmo_raylib targets, needed before +# add_subdirectory(core) below since core's own CMakeLists.txt defines a +# core_raylib target (holding shared raylib rendering/camera/picking code, +# e.g. core/include/Core/raylib_render.hpp) that links against them. Required +# unconditionally: apps/multi_view_tls_registration (step2) is raylib-based. +include(cmake/raylib.cmake) + # ============================================================================ # Core Library # ============================================================================ @@ -106,7 +113,7 @@ if(BUILD_WITH_HD_MAPPER_APPLICATION) add_subdirectory(apps/hd_mapper) endif() -add_subdirectory(apps/lidar_odometry_step_1) +add_subdirectory(apps/lidar_odometry_step_1) add_subdirectory(apps/manual_color) add_subdirectory(apps/multi_session_registration) add_subdirectory(apps/multi_view_tls_registration) diff --git a/apps/multi_view_tls_registration/CMakeLists.txt b/apps/multi_view_tls_registration/CMakeLists.txt index 495300ff..5e523051 100644 --- a/apps/multi_view_tls_registration/CMakeLists.txt +++ b/apps/multi_view_tls_registration/CMakeLists.txt @@ -3,11 +3,23 @@ cmake_minimum_required(VERSION 4.0.0) project(multi_view_tls_registration_step_2) # Source files +# +# multi_view_tls_registration_gui.cpp is raylib-based: camera/picking/mini- +# compass/GLUT-window-and-input-loop code that used to come from +# core/src/utils.cpp (shared with the remaining GLUT apps, so it can't be +# changed) is now implemented locally in the .cpp itself, using rlgl's rl*() +# legacy-GL-emulation API and Core/raylib_render.hpp's ScanRenderer for the +# point-cloud GPU rendering that used to go through core's own legacy-GL +# PointCloud::render()/PointClouds::render(). +# +# perform_experiment.cpp still #includes GLUT/glew/imgui-GLUT-backend headers, +# but never calls anything from them (verified: no glut*/GL_*/ImGui_Impl* +# symbols referenced in its body) -- those are dead includes left as-is, kept +# resolvable via the include dirs below rather than editing that file. set(SOURCES - multi_view_tls_registration.cpp perform_experiment.cpp + multi_view_tls_registration.cpp perform_experiment.cpp multi_view_tls_registration_gui.cpp multi_view_tls_registration.h ../lidar_odometry_step_1/lidar_odometry_utils.cpp - "../../core/src/utils.cpp" ) # Windows: add resource file @@ -25,12 +37,14 @@ target_include_directories( multi_view_tls_registration_step_2 PRIVATE include ${REPOSITORY_DIRECTORY}/core/include - ${THIRDPARTY_DIRECTORY}/glm ${EIGEN3_INCLUDE_DIR} ${THIRDPARTY_DIRECTORY} + ${THIRDPARTY_DIRECTORY}/glm + # For perform_experiment.cpp's unused-but-still-#included glm/GLUT/ + # glew/imgui-GLUT-backend headers (see note above) -- not otherwise + # needed now that rendering goes through core_raylib. ${THIRDPARTY_DIRECTORY}/imgui ${THIRDPARTY_DIRECTORY}/imgui/backends - ${THIRDPARTY_DIRECTORY}/ImGuizmo ${THIRDPARTY_DIRECTORY}/glew-cmake/include ${FREEGLUT_INCLUDE_DIR} ${THIRDPARTY_DIRECTORY}/json/include @@ -42,18 +56,23 @@ target_include_directories( target_link_libraries( multi_view_tls_registration_step_2 - PRIVATE + PRIVATE + # core_raylib brings in core + raylib transitively (PUBLIC link, + # see core/CMakeLists.txt) -- ScanRenderer (the GPU point-cloud + # renderer this app uses) lives there. FREEGLUT_LIBRARY is linked + # PUBLIC by core_raylib rather than here, so it lands correctly + # ordered relative to libcore.a on the final link line -- see the + # comment there for why. + core_raylib + imgui_raylib + rlimgui + imguizmo_raylib WGS84toCartesian wgs84_do_puwg92 unordered_dense::unordered_dense spdlog::spdlog - OpenGL::GLU - ${FREEGLUT_LIBRARY} - ${OPENGL_gl_LIBRARY} ${PLATFORM_LASZIP_LIB} ${PLATFORM_MISCELLANEOUS_LIBS} - ${CORE_LIBRARIES} - ${GUI_LIBRARIES} PROJ::proj) if(WIN32) @@ -71,4 +90,4 @@ if (MSVC) target_compile_options(multi_view_tls_registration_step_2 PRIVATE /bigobj) endif() -install (TARGETS multi_view_tls_registration_step_2 DESTINATION bin) \ No newline at end of file +install (TARGETS multi_view_tls_registration_step_2 DESTINATION bin) 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 70f21708..d5b583ce 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 @@ -1,10 +1,28 @@ #include -#include +// This app used to be built on GLUT + legacy immediate-mode OpenGL +// (glBegin/glVertex/gluPerspective/gluUnProject/glMatrixMode/...) via +// core/src/utils.cpp. utils.cpp is shared by several other GLUT apps and +// can't be changed, and raylib's context here is OpenGL 3.3 core profile +// (no fixed-function pipeline), so this file no longer includes +// -- instead it locally re-declares the same globals and +// re-implements the same functions it used to get from there (same names, +// same call sites throughout this file), backed by rlgl's rl*() legacy-GL +// emulation API (rlMatrixMode/rlBegin/rlVertex3f/... -- a software matrix +// stack + immediate-mode layer that works under core profile) instead of +// real gl*() calls. The handful of call sites that used to reach into +// core's own legacy-GL .render()/::Render() methods (PointClouds::render(), +// ManualPoseGraphLoopClosure::Render(), etc. -- compiled once into `core`, +// shared with GLUT apps, so they can't be changed either) are replaced with +// Core/raylib_render.hpp's ScanRenderer instead. See each local +// function/global below for what it replaces. +#include "raylib.h" +#include "rlImGui.h" +#include "rlgl.h" +#include "raymath.h" +#include "external/glad.h" #include -#include -#include #include #include @@ -25,10 +43,11 @@ #include #include #include +#include #include #include +#include #include -#include #include @@ -54,6 +73,165 @@ #endif +/////////////////////////////////////////////////////////////////////////////////// +// Formerly 's constants/enum/extern globals -- local now (see the +// big comment at the top of this file for why). +/////////////////////////////////////////////////////////////////////////////////// + +const float DEG_TO_RAD = M_PI / 180.0f; +const float RAD_TO_DEG = 180.0f / M_PI; + +const ImVec4 orangeBorder(1.0f, 0.5f, 0.0f, 1.0f); + +const std::string out_fn = "Output file name"; + +constexpr float ImGuiNumberWidth = 120.0f; +constexpr const char* omText = "Roll (left/right)"; +constexpr const char* fiText = "Pitch (up/down)"; +constexpr const char* kaText = "Yaw (turning left/right)"; +constexpr const char* xText = "Longitudinal (forward/backward)"; +constexpr const char* yText = "Lateral (left/right)"; +constexpr const char* zText = "Vertical (up/down)"; + +const uint32_t window_width = 1600; +const uint32_t window_height = 900; + +const float camera_transition_speed = 1.0f; // higher = faster + +enum CameraPreset +{ + CAMERA_FRONT, + CAMERA_BACK, + CAMERA_LEFT, + CAMERA_RIGHT, + CAMERA_TOP, + CAMERA_BOTTOM, + CAMERA_ISO, + CAMERA_RESET +}; + +enum ColorScheme +{ + CS_SOLID, // fixed color + CS_RANDOM, // random + CS_GRAD_INTENS, // gradient based on intensity + CS_GRAD_ELEV, // gradient based on elevation + CS_GRAD_DIST, // gradient based on distance from rotation center + CS_FOLLOW // valid for trajectory +}; + +int viewer_decimate_point_cloud = 1000; + +int mouse_old_x, mouse_old_y; +int mouse_buttons = 0; +float mouse_sensitivity = 1.0; + +bool is_ortho = false; +bool lock_z = false; +bool show_axes = true; +ImVec4 bg_color = ImVec4(0.65f, 0.65f, 0.65f, 1.00f); +int point_size = 1; + +bool info_gui = false; +bool compass_ruler = true; + +Eigen::Affine3f viewLocal; + +Eigen::Vector3f rotation_center = Eigen::Vector3f::Zero(); +float rotate_x = -35.264f, rotate_y = 135.0f; +float translate_x, translate_y = 0.0; +float translate_z = -50.0; + +double camera_ortho_xy_view_zoom = 10; +double camera_ortho_xy_view_shift_x = 0.0; +double camera_ortho_xy_view_shift_y = 0.0; +double camera_mode_ortho_z_center_h = 0.0; + +// Target camera state for smooth transitions +Eigen::Vector3f new_rotation_center = rotation_center; +float new_rotate_x = rotate_x; +float new_rotate_y = rotate_y; +float new_translate_x = translate_x; +float new_translate_y = translate_y; +float new_translate_z = translate_z; + +bool cor_gui = false; + +// Transition timing +bool camera_transition_active = false; + +bool scroll_hint_enabled = true; +bool scroll_hint_active = false; +int scroll_hint_count = 0; +float scroll_hint_accu = 0.0f; +double scroll_hint_lastT = 0.0; + +bool show_about = false; + +// Unlike the original (which probed GL_LINE_WIDTH_RANGE), rlgl's line +// width support is uniform enough here not to need a runtime check. +bool glLineWidthSupport = true; + +float m_ortho_projection[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; +float m_ortho_gizmo_view[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; + +struct ShortcutEntry +{ + std::string type; + std::string shortcut; + std::string description; +}; + +// GPU (rlgl-based) point cloud renderer -- replaces core's legacy-GL +// PointCloud::render()/PointClouds::render() (see Core/raylib_render.hpp). +// Rebuilt on session load and whenever a scan's pose changes; syncPoses() +// is called once per frame in display() as a safety net for pose-mutating +// code paths that don't explicitly call rebuild(). +ScanRenderer scan_renderer; + +// This frame's 3D model-view-projection matrix, captured right after the +// camera transform is set up in display() (before the projection/modelview +// stack gets reset to the 2D screen ortho for ImGui -- see +// end3DMatrixStack()). renderLoopClosureLabels() uses it to project pose +// world positions to screen space for DrawText, since it runs after that +// reset (2D text needs the 2D ortho active, but still needs to know where +// each 3D point landed on screen). +Matrix frame_mvp_3d{}; + +// Forward declarations for the functions defined near display() below +// (formerly declared in , plus a few new ones for this +// port) -- panel functions earlier in this file call several of these. +std::string truncPath(const std::string& fullPath); +void wheel(int button, int dir, int x, int y); +void reshape(int w, int h); +void motion(int x, int y); +void ShowMainDockSpace(); +void showAxes(); +void updateCameraTransition(); +void breakCameraTransition(); +void setCameraPreset(CameraPreset preset); +void camMenu(); +void view_kbd_shortcuts(); +void cor_window(); +void ImGuiHyperlink(const char* url, ImVec4 color = ImVec4(0.2f, 0.4f, 0.8f, 1.0f)); +void ShowShortcutsTable(const std::vector appShortcuts); +void info_window(const std::vector& infoLines, const std::vector& appShortcuts); +void drawMiniCompassWithRuler(); +float distanceToPlane(const RegistrationPlaneFeature::Plane& plane, const Eigen::Vector3d& p); +Eigen::Vector3d rayIntersection(const LaserBeam& laser_beam, const RegistrationPlaneFeature::Plane& plane); +LaserBeam GetLaserBeam(int x, int y); +double distance_point_to_line(const Eigen::Vector3d& point, const LaserBeam& line); +void getClosestTrajectoryPoint(Session& session_, int x, int y, bool gcpPicking, int& picked_index); +void setNewRotationCenter(int x, int y); +bool checkClHelp(int argc, char** argv); +void updateOrthoView(); +void end3DMatrixStack(); +void observationPickingRender(const ObservationPicking& observation_picking); +void renderLoopClosure(PointClouds& point_clouds_container, int index_loop_closure_source, int index_loop_closure_target, int before, int after); +void renderLoopClosureLabels(PointClouds& point_clouds_container); +void display(); +void mouse(int glut_button, int state, int x, int y); + /////////////////////////////////////////////////////////////////////////////////// #ifdef _WIN32 @@ -231,6 +409,28 @@ int index_end = 0; ColorScheme csPointCloud = CS_SOLID; ColorScheme csTrajectory = CS_SOLID; + +// New (not in the original GLUT app): CS_GRAD_INTENS/CS_GRAD_ELEV/CS_GRAD_DIST +// were declared in the original's ColorScheme enum but never actually wired +// to a menu item or the renderer -- this hooks them up to scan_renderer's +// per-point jet-colormap shader modes (see Core/raylib_render.hpp's +// ScanColorMode), alongside the two modes (Solid/Random) the original did +// implement. +ScanColorMode scanColorModeFromScheme(ColorScheme cs) +{ + switch (cs) + { + case CS_GRAD_INTENS: + return ScanColorMode::Intensity; + case CS_GRAD_ELEV: + return ScanColorMode::Elevation; + case CS_GRAD_DIST: + return ScanColorMode::Distance; + default: + return ScanColorMode::Flat; + } +} + float m_gizmo[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; bool manipulate_only_marked_gizmo = false; @@ -1528,12 +1728,14 @@ void loadSession(const std::string& session_file_name) index_end = session.point_clouds_container.point_clouds.size() - 1; std::string newTitle = winTitle + " - " + truncPath(session_file_name); - glutSetWindowTitle(newTitle.c_str()); + SetWindowTitle(newTitle.c_str()); for (const auto& pc : session.point_clouds_container.point_clouds) session_total_number_of_points += pc.points_local.size(); session_dims = session.point_clouds_container.compute_point_cloud_dimension(); + + scan_renderer.rebuildAll(session.point_clouds_container.point_clouds); } } @@ -1668,7 +1870,7 @@ void openLaz(bool fillInSession) index_end = session.point_clouds_container.point_clouds.size() - 1; std::string newTitle = winTitle + " - " + fs::path(input_file_names[0]).parent_path().string(); - glutSetWindowTitle(newTitle.c_str()); + SetWindowTitle(newTitle.c_str()); for (const auto& pc : session.point_clouds_container.point_clouds) session_total_number_of_points += pc.points_local.size(); @@ -2217,237 +2419,1684 @@ void settings_gui() ImGui::End(); } -void display() +// ============================================================================ +// Formerly core/src/utils.cpp -- local now (see the big comment at the top +// of this file for why). Everywhere the original was pure ImGui/Eigen/GLM +// (no gl*/glu*/glut* calls), it's copied verbatim. Everywhere it touched +// legacy GL, it's reimplemented with rlgl's rl*() legacy-emulation API +// (a software matrix stack + immediate-mode layer that mirrors gl*()'s +// call shape but works under a core-profile context), or with raylib/ +// raymath equivalents (gluUnProject -> Vector3Unproject, glutBitmapCharacter +// -> DrawText). Function names/signatures/globals are unchanged so every +// call site elsewhere in this file (display(), mouse(), the panel +// functions, ...) needed no changes. +// ============================================================================ + +std::string truncPath(const std::string& fullPath) { - ImGuiIO& io = ImGui::GetIO(); - glViewport(0, 0, (GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); - - glClearColor(bg_color.x * bg_color.w, bg_color.y * bg_color.w, bg_color.z * bg_color.w, bg_color.w); - glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - glEnable(GL_DEPTH_TEST); + namespace fspath = std::filesystem; + fspath::path path(fullPath); - glMatrixMode(GL_PROJECTION); - glLoadIdentity(); - float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); + auto parent1 = path.parent_path().filename().string(); + auto parent2 = path.parent_path().parent_path().filename().string(); // second to last folder + auto filename = path.filename().string(); - updateCameraTransition(); + return "..\\" + parent2 + "\\" + parent1 + "\\" + filename; +} - viewLocal = Eigen::Affine3f::Identity(); +void wheel(int button, int dir, int x, int y) +{ + ImGuiIO& io = ImGui::GetIO(); + io.MouseWheel += dir; // or direction * 1.0f depending on your setup - if (!is_ortho) + if (!ImGui::IsWindowHovered(ImGuiHoveredFlags_AnyWindow)) { - reshape((GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); - - // janusz - if (is_loop_closure_gui) + if (dir > 0) { - if (new_loop_closure_index) + if (is_ortho) { - // if (index_loop_closure_source < session.point_clouds_container.point_clouds.size()) - //{ - // new_rotation_center.x() = - // session.point_clouds_container.point_clouds[index_loop_closure_source].m_pose.translation().x(); - // new_rotation_center.y() = - // session.point_clouds_container.point_clouds[index_loop_closure_source].m_pose.translation().y(); - // new_rotation_center.z() = - // session.point_clouds_container.point_clouds[index_loop_closure_source].m_pose.translation().z(); - // - // new_translate_x = -new_rotation_center.x(); - // new_translate_y = -new_rotation_center.y(); - // camera_transition_active = true; - //} + camera_ortho_xy_view_zoom -= 0.1f * camera_ortho_xy_view_zoom; - if (session.pose_graph_loop_closure.manipulate_active_edge) + if (camera_ortho_xy_view_zoom < 0.1) { - if (session.pose_graph_loop_closure.edges.size() > 0) - { - if (session.pose_graph_loop_closure.index_active_edge < session.pose_graph_loop_closure.edges.size()) - { - new_rotation_center.x() = - session.point_clouds_container - .point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge] - .index_from] - .m_pose.translation() - .x(); - new_rotation_center.y() = - session.point_clouds_container - .point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge] - .index_from] - .m_pose.translation() - .y(); - new_rotation_center.z() = - session.point_clouds_container - .point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge] - .index_from] - .m_pose.translation() - .z(); - } - } - - new_rotate_x = rotate_x; - new_rotate_y = rotate_y; - new_translate_x = -new_rotation_center.x(); - new_translate_y = -new_rotation_center.y(); - new_translate_z = translate_z; - camera_transition_active = true; + camera_ortho_xy_view_zoom = 0.1; } - - new_loop_closure_index = false; + } + else + { + if (io.KeyShift) + translate_z += 5.0f; + else + translate_z += 1.0f; + } + } + else + { + if (is_ortho) + camera_ortho_xy_view_zoom += 0.1 * camera_ortho_xy_view_zoom; + else + { + if (io.KeyShift) + translate_z -= 5.0f; + else + translate_z -= 1.0f; } } - viewLocal.translate(rotation_center); + mouse_sensitivity = fabs(translate_z) / 100; // 1 for translate_z 50 (default zoom) + camera_transition_active = false; - viewLocal.translate(Eigen::Vector3f(translate_x, translate_y, translate_z)); - if (!lock_z) - viewLocal.rotate(Eigen::AngleAxisf(rotate_x * DEG_TO_RAD, Eigen::Vector3f::UnitX())); - else - viewLocal.rotate(Eigen::AngleAxisf(-90.0 * DEG_TO_RAD, Eigen::Vector3f::UnitX())); - viewLocal.rotate(Eigen::AngleAxisf(rotate_y * DEG_TO_RAD, Eigen::Vector3f::UnitZ())); + if (scroll_hint_enabled) + { + if (!scroll_hint_active) + { + scroll_hint_accu += fabs(dir); - viewLocal.translate(-rotation_center); + if (scroll_hint_accu > 30.0f) // tweak threshold + { + scroll_hint_accu = 0.0f; + scroll_hint_active = true; + scroll_hint_count++; + } + } - glLoadMatrixf(viewLocal.matrix().data()); - } - else - updateOrthoView(); + if (scroll_hint_active) + scroll_hint_lastT = ImGui::GetTime(); - showAxes(); + // Reset and disable hint if Shift is pressed while scrolling + if (io.KeyShift || scroll_hint_count > 3) + { + scroll_hint_active = false; + scroll_hint_enabled = false; + } + } + } +} - if (session.control_points.is_imgui) - session.control_points.render(session.point_clouds_container, true); +// Was glMatrixMode/glLoadIdentity/gluPerspective/glOrtho -- rewritten with +// rlgl's software matrix-stack API (RL_PROJECTION/RL_MODELVIEW), which +// works under raylib's core-profile context. gluPerspective(fovy, aspect, +// near, far) has no rl* equivalent, so it's expanded to the equivalent +// rlFrustum() call by hand (standard fovy -> frustum-bounds formula). +void reshape(int w, int h) +{ + rlViewport(0, 0, (int)w, (int)h); + rlMatrixMode(RL_PROJECTION); + rlLoadIdentity(); + if (!is_ortho) + { + const double fovy = 60.0; + const double aspect = (double)w / (double)h; + const double nearP = 0.01, farP = 10000.0; + const double top = nearP * tan(fovy * 0.5 * M_PI / 180.0); + const double bottom = -top; + const double right = top * aspect; + const double left = -right; + rlFrustum(left, right, bottom, top, nearP, farP); + } else { - if (is_loop_closure_gui) - session.pose_graph_loop_closure.Render( - session.point_clouds_container, - index_loop_closure_source, - index_loop_closure_target, - num_edge_extended_before, - num_edge_extended_after); - - tls_registration.gnss.render(session.point_clouds_container); - session.ground_control_points.render(session.point_clouds_container); - session.control_points.render(session.point_clouds_container, false); + ImGuiIO& io = ImGui::GetIO(); + float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); + + rlOrtho( + -camera_ortho_xy_view_zoom, + camera_ortho_xy_view_zoom, + -camera_ortho_xy_view_zoom / ratio, + camera_ortho_xy_view_zoom / ratio, + -100000, + 100000); } + rlMatrixMode(RL_MODELVIEW); + rlLoadIdentity(); +} - int prev_index_pose = session.control_points.index_pose; +// GL-free -- copied verbatim, minus the trailing glutPostRedisplay() (a +// no-op here: this app's main loop already redraws every frame). +void motion(int x, int y) +{ + ImGuiIO& io = ImGui::GetIO(); + io.MousePos = ImVec2((float)x, (float)y); - if (prev_index_pose != session.control_points.index_pose) + if (!io.WantCaptureMouse) { - session.control_points.index_picked_point = -1; // reset picked point when pose changes + float dx, dy; + dx = (float)(x - mouse_old_x); + dy = (float)(y - mouse_old_y); - new_rotation_center.x() = session.point_clouds_container.point_clouds[session.control_points.index_pose].m_pose.translation().x(); - new_rotation_center.y() = session.point_clouds_container.point_clouds[session.control_points.index_pose].m_pose.translation().y(); - new_rotation_center.z() = session.point_clouds_container.point_clouds[session.control_points.index_pose].m_pose.translation().z(); - - new_rotate_x = rotate_x; - new_rotate_y = rotate_y; - if (session.control_points.track_pose_with_camera) + if (mouse_buttons & 1) // left button { - new_translate_x = -new_rotation_center.x(); - new_translate_y = -new_rotation_center.y(); + rotate_x += dy * 0.2f; + rotate_y += dx * 0.2f; + breakCameraTransition(); } - else + + if (mouse_buttons & 4) // right button { - new_translate_x = translate_x; - new_translate_y = translate_y; + if (is_ortho) + { + float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); + Eigen::Vector3d v( + dx * (camera_ortho_xy_view_zoom / (float)io.DisplaySize.x * 2), + dy * (camera_ortho_xy_view_zoom / (float)io.DisplaySize.y * 2 / ratio), + 0); + TaitBryanPose pose_tb; + pose_tb.px = 0.0; + pose_tb.py = 0.0; + pose_tb.pz = 0.0; + pose_tb.om = 0.0; + pose_tb.fi = 0.0; + pose_tb.ka = (rotate_x + rotate_y) * M_PI / 180.0; + auto m = affine_matrix_from_pose_tait_bryan(pose_tb); + Eigen::Vector3d v_t = m * v; + camera_ortho_xy_view_shift_x += v_t.x(); + camera_ortho_xy_view_shift_y += v_t.y(); + } + else + { + translate_x += dx * 0.1f * mouse_sensitivity; + translate_y -= dy * 0.1f * mouse_sensitivity; + breakCameraTransition(); + } } - new_translate_z = translate_z; - camera_transition_active = true; + + mouse_old_x = x; + mouse_old_y = y; } +} - ImGui_ImplOpenGL2_NewFrame(); - ImGui_ImplGLUT_NewFrame(); - ImGui::NewFrame(); +// GL-free -- copied verbatim. +static bool first_time = true; - ShowMainDockSpace(); +void ShowMainDockSpace() +{ + ImGuiWindowFlags window_flags = ImGuiWindowFlags_NoDocking | ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_NoCollapse | + ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoBringToFrontOnFocus | ImGuiWindowFlags_NoNavFocus | + ImGuiWindowFlags_NoBackground | ImGuiWindowFlags_NoInputs; - if (session.control_points.is_imgui) - session.control_points.imgui(session.point_clouds_container, rotation_center); + ImGuiViewport* viewport = ImGui::GetMainViewport(); + ImGui::SetNextWindowPos(viewport->WorkPos); + ImGui::SetNextWindowSize(viewport->WorkSize); + ImGui::SetNextWindowViewport(viewport->ID); - if (session.ground_control_points.is_imgui) - session.ground_control_points.imgui(session.point_clouds_container); + ImGui::PushStyleVar(ImGuiStyleVar_WindowRounding, 0.0f); + ImGui::PushStyleVar(ImGuiStyleVar_WindowBorderSize, 0.0f); - if (!session.control_points.is_imgui) - { - 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); - } + ImGui::Begin("MainDockSpace", nullptr, window_flags); - ImGuiIO& io = ImGui::GetIO(); - // ImGuizmo ----------------------------------------------- - ImGuizmo::BeginFrame(); - ImGuizmo::Enable(true); - ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); + ImGui::PopStyleVar(2); - if (!is_ortho) - { - GLfloat projection[16]; - glGetFloatv(GL_PROJECTION_MATRIX, projection); + // This is the dockspace! + ImGuiID dockspace_id = ImGui::GetID("MyDockSpace"); + ImGui::DockSpace(dockspace_id, ImVec2(0, 0), ImGuiDockNodeFlags_PassthruCentralNode | ImGuiDockNodeFlags_NoDockingInCentralNode); - GLfloat modelview[16]; - glGetFloatv(GL_MODELVIEW_MATRIX, modelview); + if (first_time) + { + first_time = false; - 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); + auto dock_id_left = ImGui::DockBuilderSplitNode(dockspace_id, ImGuiDir_Left, 0.2f, nullptr, &dockspace_id); + auto dock_id_bottom = ImGui::DockBuilderSplitNode(dockspace_id, ImGuiDir_Down, 0.2f, nullptr, &dockspace_id); - 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); + ImGui::DockBuilderDockWindow("Console", dock_id_bottom); + ImGui::DockBuilderFinish(dockspace_id); + } - 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; + ImGui::End(); +} - 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); +// Was glBegin(GL_LINES)/glColor3f/glVertex3f/glEnd -- rl* rename. +void showAxes() +{ + if (show_axes || ImGui::GetIO().KeyCtrl) // rotation center axes + { + rlBegin(RL_LINES); + rlColor3f(1.f, 1.f, 1.f); + rlVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z()); + rlVertex3f(rotation_center.x() + 1.f, rotation_center.y(), rotation_center.z()); + rlVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z()); + rlVertex3f(rotation_center.x() - 1.f, rotation_center.y(), rotation_center.z()); + rlVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z()); + rlVertex3f(rotation_center.x(), rotation_center.y() - 1.f, rotation_center.z()); + rlVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z()); + rlVertex3f(rotation_center.x(), rotation_center.y() + 1.f, rotation_center.z()); + rlVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z()); + rlVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z() - 1.f); + rlVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z()); + rlVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z() + 1.f); + rlEnd(); + } - if (!manipulate_only_marked_gizmo) + if (show_axes || ImGui::GetIO().KeyCtrl) // origin axes + { + rlBegin(RL_LINES); + rlColor3f(1.0f, 0.0f, 0.0f); + rlVertex3f(0.0f, 0.0f, 0.0f); + rlVertex3f(100, 0.0f, 0.0f); + + rlColor3f(0.0f, 1.0f, 0.0f); + rlVertex3f(0.0f, 0.0f, 0.0f); + rlVertex3f(0.0f, 100, 0.0f); + + rlColor3f(0.0f, 0.0f, 1.0f); + rlVertex3f(0.0f, 0.0f, 0.0f); + rlVertex3f(0.0f, 0.0f, 100); + rlEnd(); + } +} + +// GL-free -- copied verbatim. +void updateCameraTransition() +{ + if (!camera_transition_active) + return; + + float t = 1.0f - powf(1.0f - std::min(ImGui::GetIO().DeltaTime * camera_transition_speed, 1.0f), 3.0f); + + bool doneXrc = fabs(new_rotation_center.x() - rotation_center.x()) < 0.01f; + bool doneYrc = fabs(new_rotation_center.y() - rotation_center.y()) < 0.01f; + bool doneZrc = fabs(new_rotation_center.z() - rotation_center.z()) < 0.01f; + bool doneXr = fabs(new_rotate_x - rotate_x) < 0.01f; + bool doneYr = fabs(new_rotate_y - rotate_y) < 0.01f; + bool doneXt = fabs(new_translate_x - translate_x) < 0.01f; + bool doneYt = fabs(new_translate_y - translate_y) < 0.01f; + bool doneZt = fabs(new_translate_z - translate_z) < 0.01f; + + if (!doneXrc) + rotation_center.x() += (new_rotation_center.x() - rotation_center.x()) * t; + if (!doneYrc) + rotation_center.y() += (new_rotation_center.y() - rotation_center.y()) * t; + if (!doneZrc) + rotation_center.z() += (new_rotation_center.z() - rotation_center.z()) * t; + if (!doneXr) + rotate_x += (new_rotate_x - rotate_x) * t; + if (!doneYr) + rotate_y += (new_rotate_y - rotate_y) * t; + if (!doneXt) + translate_x += (new_translate_x - translate_x) * t; + if (!doneYt) + translate_y += (new_translate_y - translate_y) * t; + if (!doneZt) + translate_z += (new_translate_z - translate_z) * t; + + camera_transition_active = !(doneXrc && doneYrc && doneZrc && doneXr && doneYr && doneXt && doneYt && doneZt); + + if (!camera_transition_active) + { + rotation_center = new_rotation_center; + rotate_x = new_rotate_x; + rotate_y = new_rotate_y; + translate_x = new_translate_x; + translate_y = new_translate_y; + translate_z = new_translate_z; + } +} + +// GL-free -- copied verbatim. +void breakCameraTransition() +{ + if (camera_transition_active == false) + return; + rotation_center = new_rotation_center; + camera_transition_active = false; +} + +// GL-free -- copied verbatim. +void setCameraPreset(CameraPreset preset) +{ + bool triggered = false; + + switch (preset) + { + case CAMERA_FRONT: + new_rotate_x = -90.0f; + new_rotate_y = +90.0f; + triggered = true; + break; + case CAMERA_BACK: + new_rotate_x = -90.0f; + new_rotate_y = -90.0f; + triggered = true; + break; + case CAMERA_LEFT: + new_rotate_x = -90.0f; + new_rotate_y = 180.0f; + triggered = true; + break; + case CAMERA_RIGHT: + new_rotate_x = -90.0f; + new_rotate_y = 0.0f; + triggered = true; + break; + case CAMERA_TOP: + new_rotate_x = 0.0f; + new_rotate_y = 90.0f; + triggered = true; + break; + case CAMERA_BOTTOM: + new_rotate_x = 180.0f; + new_rotate_y = -90.0f; + triggered = true; + break; + case CAMERA_ISO: + new_rotate_x = -35.264f; + new_rotate_y = 135.0f; + triggered = true; + break; + case CAMERA_RESET: + new_rotation_center = Eigen::Vector3f::Zero(); + new_rotate_x = 0; + new_rotate_y = 0; + new_translate_x = 0; + new_translate_y = 0; + new_translate_z = -50.0f; + mouse_sensitivity = fabs(translate_z) / 100; + + camera_ortho_xy_view_zoom = 10; + camera_ortho_xy_view_shift_x = 0.0; + camera_ortho_xy_view_shift_y = 0.0; + camera_mode_ortho_z_center_h = 0.0; + + viewer_decimate_point_cloud = 1000; + triggered = false; + break; + } + + if (triggered) + { + new_rotation_center = rotation_center; + new_translate_x = translate_x; + new_translate_y = translate_y; + new_translate_z = translate_z; + } + + camera_transition_active = true; +} + +// GL-free -- copied verbatim. +void camMenu() +{ + if (ImGui::BeginMenu("Camera")) + { + if (ImGui::MenuItem("Front (yz view)", "key F")) + setCameraPreset(CAMERA_FRONT); + if (ImGui::MenuItem("Back", "key B")) + setCameraPreset(CAMERA_BACK); + if (ImGui::MenuItem("Left (xz view)", "key L")) + setCameraPreset(CAMERA_LEFT); + if (ImGui::MenuItem("Right", "key R")) + setCameraPreset(CAMERA_RIGHT); + if (ImGui::MenuItem("Top (xy view)", "key T")) + setCameraPreset(CAMERA_TOP); + if (ImGui::MenuItem("Bottom", "key U")) + setCameraPreset(CAMERA_BOTTOM); + if (ImGui::MenuItem("Isometric", "key I")) + setCameraPreset(CAMERA_ISO); + ImGui::Separator(); + if (ImGui::MenuItem("Reset", "key Z")) + setCameraPreset(CAMERA_RESET); + + ImGui::EndMenu(); + } + if (ImGui::IsItemHovered()) + { + ImGui::BeginTooltip(); + ImGui::Text("Change camera view to fixed positions"); + ImGui::Separator(); + ImGui::Text("Metrics:"); + if (ImGui::BeginTable("Metrics", 4)) + { + ImGui::TableSetupColumn("Coord"); + ImGui::TableSetupColumn("rotate"); + ImGui::TableSetupColumn("translate"); + ImGui::TableSetupColumn("rot center"); + ImGui::TableHeadersRow(); + + ImGui::TableNextRow(); + ImGui::TableSetColumnIndex(0); + + std::string text = "X"; + float centered = ImGui::GetColumnWidth() - ImGui::CalcTextSize(text.c_str()).x; + ImGui::SetCursorPosX(ImGui::GetCursorPosX() + centered * 0.5f); + ImGui::Text("X"); + + ImGui::TableSetColumnIndex(1); + ImGui::Text("%.3f", rotate_x); + ImGui::TableSetColumnIndex(2); + ImGui::Text("%.3f", translate_x); + ImGui::TableSetColumnIndex(3); + ImGui::Text("%.3f", rotation_center.x()); + + ImGui::TableNextRow(); + ImGui::TableSetColumnIndex(0); + ImGui::SetCursorPosX(ImGui::GetCursorPosX() + centered * 0.5f); + ImGui::Text("Y"); + + ImGui::TableSetColumnIndex(1); + ImGui::Text("%.3f", rotate_y); + ImGui::TableSetColumnIndex(2); + ImGui::Text("%.3f", translate_y); + ImGui::TableSetColumnIndex(3); + ImGui::Text("%.3f", rotation_center.y()); + + ImGui::TableNextRow(); + ImGui::TableSetColumnIndex(0); + ImGui::SetCursorPosX(ImGui::GetCursorPosX() + centered * 0.5f); + ImGui::Text("Z"); + + ImGui::TableSetColumnIndex(2); + ImGui::Text("%.3f", translate_z); + ImGui::TableSetColumnIndex(3); + ImGui::Text("%.3f", rotation_center.y()); + + ImGui::EndTable(); + } + ImGui::Text("Mouse sensitivity: %.4f", mouse_sensitivity); + + ImGui::EndTooltip(); + } + + if (scroll_hint_active) + { + ImVec2 mousePos = ImGui::GetMousePos(); + ImGui::SetNextWindowPos(ImVec2(mousePos.x + 20, mousePos.y - 40)); + ImGui::SetNextWindowBgAlpha(0.7f); + ImGui::BeginTooltip(); + ImGui::Text("Tip: To accelerate hold Shift + scroll"); + ImGui::EndTooltip(); + + if (ImGui::GetTime() - scroll_hint_lastT > 1) + scroll_hint_active = false; + } +} + +// GL-free -- copied verbatim. +void view_kbd_shortcuts() +{ + ImGuiIO& io = ImGui::GetIO(); + + if (io.WantCaptureKeyboard) + return; + + if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_RightArrow, true)) + { + translate_x += 0.5f * mouse_sensitivity; + breakCameraTransition(); + } + if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_LeftArrow, true)) + { + translate_x -= 0.5f * mouse_sensitivity; + breakCameraTransition(); + } + + if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_UpArrow, true)) + { + translate_y += 0.5f * mouse_sensitivity; + breakCameraTransition(); + } + if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_DownArrow, true)) + { + translate_y -= 0.5f * mouse_sensitivity; + breakCameraTransition(); + } + + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_RightArrow, true)) + { + rotate_y -= 0.6; + breakCameraTransition(); + } + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_LeftArrow, true)) + { + rotate_y += 0.6; + breakCameraTransition(); + } + + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_UpArrow, true)) + { + rotate_x -= 0.6; + breakCameraTransition(); + } + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_DownArrow, true)) + { + rotate_x += 0.6; + breakCameraTransition(); + } + + if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_R, false)) + cor_gui = true; + + if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_Z, false) && !is_ortho) + lock_z = !lock_z; + + if (io.KeyCtrl || io.KeyAlt || io.KeyShift) + return; + + if (ImGui::IsKeyPressed(ImGuiKey_B)) + setCameraPreset(CAMERA_BACK); + if (ImGui::IsKeyPressed(ImGuiKey_F)) + setCameraPreset(CAMERA_FRONT); + if (ImGui::IsKeyPressed(ImGuiKey_I)) + setCameraPreset(CAMERA_ISO); + if (ImGui::IsKeyPressed(ImGuiKey_L)) + setCameraPreset(CAMERA_LEFT); + if (ImGui::IsKeyPressed(ImGuiKey_R)) + setCameraPreset(CAMERA_RIGHT); + if (ImGui::IsKeyPressed(ImGuiKey_T)) + setCameraPreset(CAMERA_TOP); + if (ImGui::IsKeyPressed(ImGuiKey_U)) + setCameraPreset(CAMERA_BOTTOM); + if (ImGui::IsKeyPressed(ImGuiKey_Z)) + setCameraPreset(CAMERA_RESET); + + if (ImGui::IsKeyPressed(ImGuiKey_C, false)) + compass_ruler = !compass_ruler; + if (ImGui::IsKeyPressed(ImGuiKey_O, false)) + is_ortho = !is_ortho; + if (ImGui::IsKeyPressed(ImGuiKey_X, false)) + show_axes = !show_axes; + + if (ImGui::IsKeyPressed(ImGuiKey_1)) + point_size = 1; + if (ImGui::IsKeyPressed(ImGuiKey_2)) + point_size = 2; + if (ImGui::IsKeyPressed(ImGuiKey_3)) + point_size = 3; + if (ImGui::IsKeyPressed(ImGuiKey_4)) + point_size = 4; + if (ImGui::IsKeyPressed(ImGuiKey_5)) + point_size = 5; + if (ImGui::IsKeyPressed(ImGuiKey_6)) + point_size = 6; + if (ImGui::IsKeyPressed(ImGuiKey_7)) + point_size = 7; + if (ImGui::IsKeyPressed(ImGuiKey_8)) + point_size = 8; + if (ImGui::IsKeyPressed(ImGuiKey_9)) + point_size = 9; +} + +// GL-free -- copied verbatim. +void cor_window() +{ + if (cor_gui) + { + ImGui::OpenPopup("Center of rotation"); + cor_gui = false; + } + + if (ImGui::BeginPopupModal("Center of rotation", NULL, ImGuiWindowFlags_AlwaysAutoResize)) + { + ImGui::Text("Select new center of rotation [m]:"); + ImGui::PushItemWidth(ImGuiNumberWidth); + ImGui::InputFloat("X", &new_rotation_center.x(), 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(xText); + ImGui::SameLine(); + ImGui::InputFloat("Y", &new_rotation_center.y(), 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(yText); + ImGui::SameLine(); + ImGui::InputFloat("Z", &new_rotation_center.z(), 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(zText); + ImGui::PopItemWidth(); + + ImGui::Separator(); + + if (ImGui::Button("Set")) + { + new_rotate_x = rotate_x; + new_rotate_y = rotate_y; + new_translate_x = -new_rotation_center.x(); + new_translate_y = -new_rotation_center.y(); + new_translate_z = translate_z; + + camera_transition_active = true; + + ImGui::CloseCurrentPopup(); + } + + ImGui::SameLine(); + if (ImGui::Button("Cancel")) + { + ImGui::CloseCurrentPopup(); + } + + ImGui::EndPopup(); + } +} + +// GL-free -- copied verbatim. +void ImGuiHyperlink(const char* url, ImVec4 color) +{ + ImGui::PushStyleColor(ImGuiCol_Text, color); + ImGui::TextUnformatted(url); + ImGui::PopStyleColor(); + + ImVec2 pos = ImGui::GetItemRectMin(); + ImVec2 size = ImGui::GetItemRectSize(); + + if (ImGui::IsItemHovered()) + ImGui::SetMouseCursor(ImGuiMouseCursor_Hand); + + if (ImGui::IsItemHovered()) + { + ImDrawList* draw_list = ImGui::GetWindowDrawList(); + draw_list->AddLine(ImVec2(pos.x, pos.y + size.y), ImVec2(pos.x + size.x, pos.y + size.y), ImColor(color)); + } + + if (ImGui::IsItemClicked()) + { +#ifdef _WIN32 + ShellExecuteA(0, "open", url, 0, 0, SW_SHOWNORMAL); +#elif __APPLE__ + std::string cmd = std::string("open ") + url; + system(cmd.c_str()); +#else + std::string cmd = std::string("xdg-open ") + url; + system(cmd.c_str()); +#endif + } +} + +// General shortcuts applicable to any app -- GL-free, copied verbatim. +static const std::vector shortcuts = { { "Normal keys", "A", "" }, + { "", "Ctrl+A", "" }, + { "", "B", "camera Back" }, + { "", "Ctrl+B", "" }, + { "", "C", "Compass/ruler" }, + { "", "Ctrl+C", "" }, + { "", "D", "" }, + { "", "Ctrl+D", "" }, + { "", "E", "" }, + { "", "Ctrl+E", "" }, + { "", "F", "camera Front" }, + { "", "Ctrl+F", "" }, + { "", "G", "" }, + { "", "Ctrl+G", "" }, + { "", "H", "" }, + { "", "Ctrl+H", "" }, + { "", "I", "camera Isometric" }, + { "", "Ctrl+I", "" }, + { "", "J", "" }, + { "", "Ctrl+J", "" }, + { "", "K", "" }, + { "", "Ctrl+K", "" }, + { "", "L", "camera Left" }, + { "", "Ctrl+L", "" }, + { "", "M", "" }, + { "", "Ctrl+M", "" }, + { "", "N", "" }, + { "", "Ctrl+N", "" }, + { "", "O", "Ortographic view" }, + { "", "Ctrl+O", "Open/load session/data" }, + { "", "P", "" }, + { "", "Ctrl+P", "" }, + { "", "Q", "" }, + { "", "Ctrl+Q", "" }, + { "", "R", "camera Right" }, + { "", "Ctrl+R", "" }, + { "", "Shift+R", "Rotation center" }, + { "", "S", "" }, + { "", "Ctrl+S", "" }, + { "", "Ctrl+Shift+S", "" }, + { "", "T", "camera Top" }, + { "", "Ctrl+T", "" }, + { "", "U", "camera bottom (Under)" }, + { "", "Ctrl+U", "" }, + { "", "V", "" }, + { "", "Ctrl+V", "" }, + { "", "W", "" }, + { "", "Ctrl+W", "" }, + { "", "X", "show aXes" }, + { "", "Ctrl+X", "" }, + { "", "Y", "" }, + { "", "Ctrl+Y", "" }, + { "", "Z", "camera reset" }, + { "", "Ctrl+Z", "" }, + { "", "Shift+Z", "Lock Z" }, + { "", "1-9", "point size" }, + { "Special keys", "Up arrow", "" }, + { "", "Shift + up arrow", "camera translate Up" }, + { "", "Ctrl + up arrow", "" }, + { "", "Down arrow", "" }, + { "", "Shift + down arrow", "camera translate Down" }, + { "", "Ctrl + down arrow", "" }, + { "", "Left arrow", "" }, + { "", "Shift + left arrow", "camera translate Left" }, + { "", "Ctrl + left arrow", "" }, + { "", "Right arrow", "" }, + { "", "Shift + right arrow", "camera translate Right" }, + { "", "Ctrl + right arrow", "" }, + { "", "Pg down", "" }, + { "", "Pg up", "" }, + { "", "- key", "" }, + { "", "+ key", "" }, + { "Mouse related", "Left click + drag", "camera rotate" }, + { "", "Right click + drag", "camera pan" }, + { "", "Scroll", "camera zoom" }, + { "", "Shift + scroll", "camera 5x zoom" }, + { "", "Shift + drag", "Dock window to screen edges" }, + { "", "Ctrl + left click", "" }, + { "", "Ctrl + right click", "change center of rotation" }, + { "", "Ctrl + middle click", "change center of rotation (if no CP GUI active)" } }; + +// GL-free -- copied verbatim. +void ShowShortcutsTable(const std::vector appShortcuts) +{ + if (ImGui::BeginTable( + "ShortcutsTable", 2, ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg | ImGuiTableFlags_ScrollY, ImVec2(-FLT_MIN, 200))) + { + ImGui::TableSetupScrollFreeze(0, 1); + ImGui::TableSetupColumn("Shortcut", ImGuiTableColumnFlags_WidthFixed, 120); + ImGui::TableSetupColumn("Description"); + ImGui::TableHeadersRow(); + + std::string lastType; + + for (size_t i = 0; i < shortcuts.size(); ++i) + { + const auto& s = shortcuts[i]; + + if (!s.type.empty() && s.type != lastType) + { + lastType = s.type; + ImGui::TableNextRow(); + + ImGui::TableSetBgColor(ImGuiTableBgTarget_RowBg0, IM_COL32(70, 70, 140, 255)); + + ImGui::TableSetColumnIndex(0); + ImGui::TextColored(ImVec4(0.8f, 0.8f, 1.0f, 1.0f), "%s", lastType.c_str()); + ImGui::TableSetColumnIndex(1); + ImGui::TextUnformatted(""); + } + + auto description = s.description; + + if (description.empty()) + description = appShortcuts[i].description; + + if (!description.empty()) + { + ImGui::TableNextRow(); + ImGui::TableSetColumnIndex(0); + ImGui::TextUnformatted(s.shortcut.c_str()); + ImGui::TableSetColumnIndex(1); + ImGui::TextUnformatted(description.c_str()); + } + } + + ImGui::EndTable(); + } +} + +// GL-free -- copied verbatim (glGetString(GL_RENDERER/...) is a plain +// string query, still valid under a core-profile context). +void info_window(const std::vector& infoLines, const std::vector& appShortcuts) +{ + if (!info_gui) + return; + + if (ImGui::Begin( + "Info", + &info_gui, + ImGuiWindowFlags_NoResize | ImGuiWindowFlags_AlwaysAutoResize | ImGuiWindowFlags_NoDocking | ImGuiWindowFlags_NoCollapse)) + { + bool firstLine = true; + for (const auto& line : infoLines) + { + if (line.empty()) + ImGui::NewLine(); + else if (line.rfind("https://", 0) == 0) + ImGuiHyperlink(line.c_str()); + else + ImGui::Text(line.c_str()); + + if (firstLine) + { + ImGui::SameLine( + ImGui::GetWindowWidth() - ImGui::CalcTextSize("ImGui").x - ImGui::GetStyle().ItemSpacing.x * 2 - + ImGui::GetStyle().FramePadding.x * 2); + if (ImGui::Button("ImGui")) + show_about = true; + if (ImGui::IsItemHovered()) + { + ImGui::BeginTooltip(); + const GLubyte* renderer = glGetString(GL_RENDERER); + const GLubyte* version = glGetString(GL_VERSION); + const GLubyte* glslVersion = glGetString(GL_SHADING_LANGUAGE_VERSION); + + ImGui::Text("Renderer: %s", renderer); + ImGui::Text("OpenGL version supported: %s", version); + ImGui::Text("GLSL version: %s", glslVersion); + ImGui::EndTooltip(); + } + + firstLine = false; + } + } + + ImGui::NewLine(); + ImGui::Text("Author: Janusz Bedkowski & contributors"); + ImGui::NewLine(); + ImGui::Text("Part of HDMapping software suite"); + ImGui::Text("Version: %s (%s)", HDMAPPING_VERSION_STRING, __DATE__); + ImGui::Text("Project page: "); + ImGui::SameLine(); + ImGuiHyperlink("https://github.com/MapsHD/HDMapping"); + + ImGui::NewLine(); + ImGui::Separator(); + ImGui::NewLine(); + + ShowShortcutsTable(appShortcuts); + + if (show_about) + ImGui::ShowAboutWindow(&show_about); + } + + ImGui::End(); +} + +// Was a dedicated 200x200 GL sub-viewport with its own glOrtho projection, +// rotation-only modelview (viewLocal's rotation), and GLUT bitmap-font text +// (glRasterPos + glutBitmapCharacter). Reimplemented as a pure 2D +// screen-space overlay instead: project each world axis direction through +// viewLocal's rotation to get eye-space X/Y (screen right/up), and draw +// with raylib's DrawLineEx/DrawText -- same bottom-left placement, same +// "nice number" ruler tied to zoom (translate_z), no sub-viewport or GLUT +// font needed. +void drawMiniCompassWithRuler() +{ + const float compassSize = 200.0f; + const float originX = compassSize * 0.5f; + const float originY = static_cast(GetScreenHeight()) - compassSize * 0.5f; + const float axisPixelLength = compassSize * 0.35f; + + struct Axis + { + Eigen::Vector3f dir; + const char* label; + Color color; + }; + const Axis axes[3] = { + {Eigen::Vector3f::UnitX(), "X (long.)", RED}, + {Eigen::Vector3f::UnitY(), "Y (lat.)", GREEN}, + {Eigen::Vector3f::UnitZ(), "Z (vert.)", BLUE}, + }; + + for (const auto& axis : axes) + { + Eigen::Vector3f eyeDir = viewLocal.rotation() * axis.dir; + Vector2 tip = {originX + eyeDir.x() * axisPixelLength, originY - eyeDir.y() * axisPixelLength}; + DrawLineEx(Vector2{originX, originY}, tip, 2.f, axis.color); + DrawText(axis.label, (int)tip.x + 4, (int)tip.y - 6, 12, axis.color); + } + + // Ruler: "nice" (1/2/5 x 10^n) length, mirroring the original's + // 0.1 * fabs(translate_z) heuristic (translate_z is this app's + // zoom/dolly distance). + float rawUnit = std::max(0.001f, 0.1f * fabsf(translate_z)); + float base = powf(10.0f, floorf(log10f(rawUnit))); + float normalized = rawUnit / base; + float niceUnit = normalized < 2.0f ? 1.0f : (normalized < 5.0f ? 2.0f : 5.0f); + float worldLength = niceUnit * base; + + char label[32]; + if (worldLength >= 1000.0f) + snprintf(label, sizeof(label), "%.0f [km]", worldLength / 1000.0f); + else if (worldLength >= 1.0f) + snprintf(label, sizeof(label), "%.0f [m]", worldLength); + else if (worldLength >= 0.01f) + snprintf(label, sizeof(label), "%.0f [cm]", worldLength * 100.0f); + else + snprintf(label, sizeof(label), "<1 [cm]"); + + float rulerY = originY + compassSize * 0.45f; + Color rulerColor = ColorFromNormalized(Vector4{1.0f - bg_color.x, 1.0f - bg_color.y, 1.0f - bg_color.z, 1.0f}); + DrawLineEx(Vector2{originX - 40.f, rulerY}, Vector2{originX + 40.f, rulerY}, 2.f, rulerColor); + DrawLineEx(Vector2{originX - 40.f, rulerY - 5.f}, Vector2{originX - 40.f, rulerY + 5.f}, 2.f, rulerColor); + DrawLineEx(Vector2{originX + 40.f, rulerY - 5.f}, Vector2{originX + 40.f, rulerY + 5.f}, 2.f, rulerColor); + DrawText(label, (int)originX - 20, (int)rulerY + 6, 14, rulerColor); +} + +// GL-free -- copied verbatim. +float distanceToPlane(const RegistrationPlaneFeature::Plane& plane, const Eigen::Vector3d& p) +{ + return (plane.a * p.x() + plane.b * p.y() + plane.c * p.z() + plane.d); +} + +// GL-free -- copied verbatim. +Eigen::Vector3d rayIntersection(const LaserBeam& laser_beam, const RegistrationPlaneFeature::Plane& plane) +{ + float TOLERANCE = 0.0001; + Eigen::Vector3d out_point; + out_point.x() = laser_beam.position.x(); + out_point.y() = laser_beam.position.y(); + out_point.z() = laser_beam.position.z(); + + float a = plane.a * laser_beam.direction.x() + plane.b * laser_beam.direction.y() + plane.c * laser_beam.direction.z(); + + if (a > -TOLERANCE && a < TOLERANCE) + { + return out_point; + } + + float distance = distanceToPlane(plane, out_point); + + out_point.x() = laser_beam.position.x() - laser_beam.direction.x() * (distance / a); + out_point.y() = laser_beam.position.y() - laser_beam.direction.y() * (distance / a); + out_point.z() = laser_beam.position.z() - laser_beam.direction.z() * (distance / a); + + return out_point; +} + +// Was gluUnProject(winX, winY, winZ, modelview, projection, viewport, ...) +// against glGetDoublev(GL_MODELVIEW/PROJECTION_MATRIX) -- rewritten with +// raymath's Vector3Unproject against rlgl's current matrix stack +// (rlGetMatrixModelview/Projection), following the same NDC-space +// conversion raylib's own GetScreenToWorldRayEx uses. The original's +// far point used winZ=-1000 (an out-of-range hack to get a point far along +// the ray, since gluUnProject doesn't clamp); using the actual far-plane +// NDC z=1 here is equally valid for the same purpose (only direction, not +// magnitude, of laser_beam.direction matters to callers). +LaserBeam GetLaserBeam(int x, int y) +{ + int width = GetScreenWidth(); + int height = GetScreenHeight(); + + float ndcX = (2.0f * (float)x) / (float)width - 1.0f; + float ndcY = 1.0f - (2.0f * (float)y) / (float)height; + + Matrix matView = rlGetMatrixModelview(); + Matrix matProj = rlGetMatrixProjection(); + + Vector3 nearPoint = Vector3Unproject(Vector3{ndcX, ndcY, 0.0f}, matProj, matView); + Vector3 farPoint = Vector3Unproject(Vector3{ndcX, ndcY, 1.0f}, matProj, matView); + + LaserBeam laser_beam; + laser_beam.position = Eigen::Vector3d(nearPoint.x, nearPoint.y, nearPoint.z); + laser_beam.direction = Eigen::Vector3d(farPoint.x - nearPoint.x, farPoint.y - nearPoint.y, farPoint.z - nearPoint.z); + return laser_beam; +} + +// GL-free -- copied verbatim. +double distance_point_to_line(const Eigen::Vector3d& point, const LaserBeam& line) +{ + Eigen::Vector3d AP = point - line.position; + return (AP.cross(line.direction)).norm(); +} + +// GL-free -- copied verbatim. +void getClosestTrajectoryPoint(Session& session_, int x, int y, bool gcpPicking, int& picked_index) +{ + picked_index = -1; + + const auto laser_beam = GetLaserBeam(x, y); + double min_distance = std::numeric_limits::max(); + int index_i = -1; + int index_j = -1; + + for (int i = 0; i < session_.point_clouds_container.point_clouds.size(); i++) + { + for (int j = 0; j < session_.point_clouds_container.point_clouds[i].local_trajectory.size(); j++) + { + const auto& p = session_.point_clouds_container.point_clouds[i].local_trajectory[j].m_pose.translation(); + Eigen::Vector3d vp = session_.point_clouds_container.point_clouds[i].m_pose * p; + + double dist = distance_point_to_line(vp, laser_beam); + + if (dist < min_distance) + { + min_distance = dist; + index_i = i; + index_j = j; + + new_rotation_center.x() = static_cast(vp.x()); + new_rotation_center.y() = static_cast(vp.y()); + new_rotation_center.z() = static_cast(vp.z()); + + if (gcpPicking) + { + session_.ground_control_points.picking_mode_index_to_node_inner = index_i; + session_.ground_control_points.picking_mode_index_to_node_outer = index_j; + } + + picked_index = index_i; + } + } + } + + new_rotate_x = rotate_x; + new_rotate_y = rotate_y; + new_translate_x = -new_rotation_center.x(); + new_translate_y = -new_rotation_center.y(); + new_translate_z = translate_z; + camera_transition_active = true; +} + +// GL-free -- copied verbatim. +void setNewRotationCenter(int x, int y) +{ + const auto laser_beam = GetLaserBeam(x, y); + + RegistrationPlaneFeature::Plane pl; + + pl.a = 0; + pl.b = 0; + pl.c = 1; + pl.d = 0; + new_rotation_center = rayIntersection(laser_beam, pl).cast(); + + std::cout << "Setting new rotation center to:\n" << new_rotation_center << std::endl; + + new_rotate_x = rotate_x; + new_rotate_y = rotate_y; + new_translate_x = -new_rotation_center.x(); + new_translate_y = -new_rotation_center.y(); + new_translate_z = translate_z; + + camera_transition_active = true; +} + +// GL-free -- copied verbatim. +bool checkClHelp(int argc, char** argv) +{ + for (int i = 1; i < argc; ++i) + { + std::string arg(argv[i]); + + if (arg == "-h" || arg == "/h" || arg == "--help" || arg == "/?") + { + return true; + } + } + return false; +} + +// Was glOrtho + gluLookAt (folded into GL_PROJECTION, matching the +// original's call order -- gluLookAt ran before the GL_MODELVIEW switch +// below) -- rewritten as rlOrtho + rlMultMatrixf with the same lookAt +// matrix already computed via GLM for m_ortho_gizmo_view just above it. +void updateOrthoView() +{ + // still updating viewLocal for compass + viewLocal.rotate(Eigen::AngleAxisf((rotate_x + rotate_y) * DEG_TO_RAD, Eigen::Vector3f::UnitZ())); + + ImGuiIO& io = ImGui::GetIO(); + float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); + + rlOrtho( + -camera_ortho_xy_view_zoom, + camera_ortho_xy_view_zoom, + -camera_ortho_xy_view_zoom / ratio, + camera_ortho_xy_view_zoom / ratio, + -100000, + 100000); + + glm::mat4 proj = glm::orthoLH_ZO( + -camera_ortho_xy_view_zoom, + camera_ortho_xy_view_zoom, + -camera_ortho_xy_view_zoom / ratio, + camera_ortho_xy_view_zoom / ratio, + -100, + 100); + + std::copy(&proj[0][0], &proj[3][3], m_ortho_projection); + + Eigen::Vector3d v_eye_t(-camera_ortho_xy_view_shift_x, camera_ortho_xy_view_shift_y, camera_mode_ortho_z_center_h + 10); + Eigen::Vector3d v_center_t(-camera_ortho_xy_view_shift_x, camera_ortho_xy_view_shift_y, camera_mode_ortho_z_center_h); + Eigen::Vector3d v(0, 1, 0); + + TaitBryanPose pose_tb; + pose_tb.px = 0.0; + pose_tb.py = 0.0; + pose_tb.pz = 0.0; + pose_tb.om = 0.0; + pose_tb.fi = 0.0; + pose_tb.ka = -(rotate_x + rotate_y) * DEG_TO_RAD; + auto m = affine_matrix_from_pose_tait_bryan(pose_tb); + + Eigen::Vector3d v_t = m * v; + + glm::mat4 lookat = glm::lookAt( + glm::vec3(v_eye_t.x(), v_eye_t.y(), v_eye_t.z()), + glm::vec3(v_center_t.x(), v_center_t.y(), v_center_t.z()), + glm::vec3(v_t.x(), v_t.y(), v_t.z())); + std::copy(&lookat[0][0], &lookat[3][3], m_ortho_gizmo_view); + + rlMultMatrixf(&lookat[0][0]); + + rlMatrixMode(RL_MODELVIEW); + rlLoadIdentity(); +} + +// Restores rlgl's default 2D screen-space projection (matches what +// raylib's own EndMode3D() does), since this app drives the rlgl matrix +// stack manually (rlMatrixMode/rlFrustum/rlMultMatrixf in reshape()/ +// display() above) instead of using raylib's BeginMode3D/EndMode3D +// wrapper. Must be called after all 3D drawing and before any 2D drawing +// (the mini-compass, ImGui) each frame. +void end3DMatrixStack() +{ + rlDrawRenderBatchActive(); + rlMatrixMode(RL_PROJECTION); + rlLoadIdentity(); + rlOrtho(0, GetScreenWidth(), GetScreenHeight(), 0, 0.0f, 1.0f); + rlMatrixMode(RL_MODELVIEW); + rlLoadIdentity(); + rlDisableDepthTest(); +} + +// Was ObservationPicking::render() (core/src/observation_picking.cpp) -- +// legacy-GL, compiled once into `core`, shared with the remaining GLUT +// apps, so it can't be changed. Reimplemented here via rl* renames (the +// original is pure immediate-mode grid/point/line drawing, no matrix-stack +// work of its own). The per-intersection wireframe boxes (Intersection:: +// render(), also in `core`) and the GLUT-bitmap-font index labels are not +// ported (a niche sub-feature of an already-niche picking mode) -- picking +// itself and the current/committed observation markers below are. +void observationPickingRender(const ObservationPicking& observation_picking) +{ + if (observation_picking.is_observation_picking_mode) + { + auto drawGrid = [&](float step, float r, float g, float b) + { + rlColor3f(r, g, b); + rlBegin(RL_LINES); + for (float x = -observation_picking.max_xy; x <= observation_picking.max_xy; x += step) + { + rlVertex3f(x, -observation_picking.max_xy, observation_picking.picking_plane_height); + rlVertex3f(x, observation_picking.max_xy, observation_picking.picking_plane_height); + } + for (float y = -observation_picking.max_xy; y <= observation_picking.max_xy; y += step) + { + rlVertex3f(-observation_picking.max_xy, y, observation_picking.picking_plane_height); + rlVertex3f(observation_picking.max_xy, y, observation_picking.picking_plane_height); + } + rlEnd(); + }; + + if (observation_picking.grid10x10m) + drawGrid(10.0f, 0.7f, 0.7f, 0.7f); + if (observation_picking.grid1x1m) + drawGrid(1.0f, 0.3f, 0.3f, 0.3f); + if (observation_picking.grid01x01m) + drawGrid(0.1f, 0.1f, 0.1f, 0.1f); + if (observation_picking.grid001x001m) + drawGrid(0.01f, 0.8f, 0.8f, 0.8f); + } + + // rlgl's rlBegin() only supports RL_LINES/RL_TRIANGLES/RL_QUADS (no + // point-mode immediate drawing), so point markers use small spheres. + for (const auto& [key, value] : observation_picking.current_observation) + { + DrawSphere(Vector3{static_cast(value.x()), static_cast(value.y()), static_cast(value.z())}, 0.05f, WHITE); + } + + rlColor3f(1.0f, 0.2f, 0.2f); + rlBegin(RL_LINES); + for (const auto& [key1, value1] : observation_picking.current_observation) + { + for (const auto& [key2, value2] : observation_picking.current_observation) + { + if (key1 != key2) + { + rlVertex3f(value1.x(), value1.y(), value1.z()); + rlVertex3f(value2.x(), value2.y(), value2.z()); + } + } + } + rlEnd(); +} + +// Was ManualPoseGraphLoopClosure::Render() (core/src/manual_pose_graph_loop_closure.cpp) -- +// legacy-GL, compiled once into `core`, shared with the remaining GLUT +// apps, so it can't be changed. Reimplemented here using scan_renderer +// (marks for source/target highlighting, straight from each scan's already- +// cached GPU buffer) plus DrawSphere/DrawCylinderEx/DrawLine3D for the +// green pose-sequence trail and the per-edge lines/flagpole markers (these +// work against whatever rlgl projection/modelview is currently active, same +// as this app's own manually-driven matrix stack -- no BeginMode3D needed). +// Coordinates are used directly (Z-up, no remap -- see raylib_render.hpp). +void renderLoopClosure(PointClouds& point_clouds_container, int index_loop_closure_source, int index_loop_closure_target, int before, int after) +{ + auto& pointClouds = point_clouds_container.point_clouds; + if (pointClouds.empty()) + { + return; + } + + scan_renderer.clearMarks(); + + // Matches the original: while loop closure editing is active, only the + // source/target (or active-edge) range renders -- not the whole + // session -- since ManualPoseGraphLoopClosure::Render() drew just those + // scans directly rather than going through the bulk point-cloud render + // call (which display() only makes when !is_loop_closure_gui). + for (auto& pc : pointClouds) + { + pc.visible = false; + } + + auto markRange = [&](int center, Color color) + { + for (int i = center - before; i <= center + after; ++i) + { + if (i >= 0 && static_cast(i) < pointClouds.size()) + { + pointClouds[i].visible = true; + if (session.pose_graph_loop_closure.render_source_as_red_target_as_blue) + { + scan_renderer.setMarkColor(static_cast(i), color); + } + } + } + }; + + if (!session.pose_graph_loop_closure.manipulate_active_edge) + { + markRange(index_loop_closure_source, RED); + markRange(index_loop_closure_target, BLUE); + } + else if (!session.pose_graph_loop_closure.edges.empty()) + { + const auto& activeEdge = session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge]; + markRange(activeEdge.index_from, RED); + + // Live preview of the target side at the edge's in-progress + // (not-yet-committed) relative_pose_tb, drawn straight from each + // scan's cached GPU buffer with the delta folded into the MVP + // rather than re-transforming points on the CPU. + int indexSrcEdge = activeEdge.index_from; + int indexTrgEdge = activeEdge.index_to; + if (indexSrcEdge >= 0 && static_cast(indexSrcEdge) < pointClouds.size() && indexTrgEdge >= 0 && + static_cast(indexTrgEdge) < pointClouds.size()) + { + const Eigen::Affine3d& mSrc = pointClouds[indexSrcEdge].m_pose; + for (int i = -before; i <= after; ++i) + { + int idx = indexTrgEdge + i; + if (idx < 0 || static_cast(idx) >= pointClouds.size()) + { + continue; + } + Eigen::Affine3d mTrg = mSrc * affine_matrix_from_pose_tait_bryan(activeEdge.relative_pose_tb); + Eigen::Affine3d mRel = pointClouds[indexTrgEdge].m_pose.inverse() * pointClouds[idx].m_pose; + mTrg = mTrg * mRel; + + Eigen::Affine3d delta = mTrg * pointClouds[idx].m_pose.inverse(); + Color c = session.pose_graph_loop_closure.render_source_as_red_target_as_blue + ? BLUE + : Color{static_cast(pointClouds[idx].render_color[0] * 255.f), + static_cast(pointClouds[idx].render_color[1] * 255.f), + 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); + } + } + } + + // The marked/visible-restricted range set above (source/target or + // active-edge scans, at their normal stored pose). + scan_renderer.draw( + pointClouds, static_cast(point_size), scanColorModeFromScheme(csPointCloud), static_cast(session_dims.z_min), + static_cast(session_dims.z_max), Eigen::Vector3d(rotation_center.x(), rotation_center.y(), rotation_center.z()), + static_cast(std::max({session_dims.length, session_dims.width, session_dims.height, 1.0})), 1); + + // Pose-sequence trail across the whole session, as a chain of thick + // green cylinders (sphere at each joint), sized relative to the current + // zoom (translate_z) so it stays visible next to the point cloud. + const float tubeRadius = std::max(0.05f, fabsf(translate_z) * 0.01f); + bool first = true; + Vector3 prev{}; + for (const auto& pc : pointClouds) + { + Vector3 p = Vector3{ + static_cast(pc.m_pose.translation().x()), static_cast(pc.m_pose.translation().y()), + static_cast(pc.m_pose.translation().z())}; + DrawSphere(p, tubeRadius, GREEN); + if (!first) + { + DrawCylinderEx(prev, p, tubeRadius, tubeRadius, 8, GREEN); + } + prev = p; + first = false; + } + + // Edge lines + flagpole markers (red = active edge, blue = others). + for (size_t i = 0; i < session.pose_graph_loop_closure.edges.size(); ++i) + { + const auto& edge = session.pose_graph_loop_closure.edges[i]; + if (edge.index_from < 0 || static_cast(edge.index_from) >= pointClouds.size() || edge.index_to < 0 || + static_cast(edge.index_to) >= pointClouds.size()) + { + continue; + } + + Color c = (static_cast(i) == session.pose_graph_loop_closure.index_active_edge) ? RED : BLUE; + + Eigen::Vector3d worldSrc = pointClouds[edge.index_from].m_pose.translation(); + Eigen::Vector3d worldTrg = pointClouds[edge.index_to].m_pose.translation(); + Vector3 pSrc = Vector3{static_cast(worldSrc.x()), static_cast(worldSrc.y()), static_cast(worldSrc.z())}; + Vector3 pTrg = Vector3{static_cast(worldTrg.x()), static_cast(worldTrg.y()), static_cast(worldTrg.z())}; + DrawCylinderEx(pSrc, pTrg, tubeRadius, tubeRadius, 8, c); + + Eigen::Vector3d mid = (worldSrc + worldTrg) * 0.5; + Eigen::Vector3d midUp = mid + Eigen::Vector3d(0, 0, 10); + Vector3 pMid = Vector3{static_cast(mid.x()), static_cast(mid.y()), static_cast(mid.z())}; + Vector3 pMidUp = Vector3{static_cast(midUp.x()), static_cast(midUp.y()), static_cast(midUp.z())}; + DrawCylinderEx(pMid, pMidUp, tubeRadius, tubeRadius, 8, c); + } +} + +// Was ManualPoseGraphLoopClosure::Render()'s per-pose glRasterPos3f + +// glutBitmapString(std::to_string(i)) labels (one per point cloud, plus one +// per edge at its flagpole top) -- neither has an rlgl/raylib equivalent +// (no matrix-anchored bitmap fonts under core profile), so this projects +// each world position to screen space by hand (using frame_mvp_3d, captured +// in display() while the 3D projection/modelview was still active -- see +// its declaration) and draws with DrawText instead. Must run after +// end3DMatrixStack() (2D screen-space drawing). +void renderLoopClosureLabels(PointClouds& point_clouds_container) +{ + auto& pointClouds = point_clouds_container.point_clouds; + + auto worldToScreen = [](const Eigen::Vector3d& world) -> Vector2 + { + // Manual clip-space transform (mat * [x,y,z,1]^T) -- raymath's + // Vector3Transform computes the same x/y/z but drops w, which the + // perspective divide below needs, so it can't be reused here. + const Matrix& m = frame_mvp_3d; + float x = static_cast(world.x()); + float y = static_cast(world.y()); + float z = static_cast(world.z()); + float clipX = m.m0 * x + m.m4 * y + m.m8 * z + m.m12; + float clipY = m.m1 * x + m.m5 * y + m.m9 * z + m.m13; + float clipW = m.m3 * x + m.m7 * y + m.m11 * z + m.m15; + if (fabsf(clipW) < 1e-6f) + { + return Vector2{-1000.f, -1000.f}; + } + float ndcX = clipX / clipW; + float ndcY = clipY / clipW; + return Vector2{ + (ndcX * 0.5f + 0.5f) * static_cast(GetScreenWidth()), (1.0f - (ndcY * 0.5f + 0.5f)) * static_cast(GetScreenHeight())}; + }; + + for (size_t i = 0; i < pointClouds.size(); ++i) + { + Vector2 screen = worldToScreen(pointClouds[i].m_pose.translation()); + DrawText(TextFormat("%d", static_cast(i)), static_cast(screen.x), static_cast(screen.y), 18, GREEN); + } + + for (size_t i = 0; i < session.pose_graph_loop_closure.edges.size(); ++i) + { + const auto& edge = session.pose_graph_loop_closure.edges[i]; + if (edge.index_from < 0 || static_cast(edge.index_from) >= pointClouds.size() || edge.index_to < 0 || + static_cast(edge.index_to) >= pointClouds.size()) + { + continue; + } + + Eigen::Vector3d worldSrc = pointClouds[edge.index_from].m_pose.translation(); + Eigen::Vector3d worldTrg = pointClouds[edge.index_to].m_pose.translation(); + Eigen::Vector3d midUp = (worldSrc + worldTrg) * 0.5 + Eigen::Vector3d(0, 0, 10); + + Vector2 screen = worldToScreen(midUp); + Color c = (static_cast(i) == session.pose_graph_loop_closure.index_active_edge) ? RED : BLUE; + DrawText(TextFormat("%d", static_cast(i)), static_cast(screen.x), static_cast(screen.y), 20, c); + } +} + +void display() +{ + // Safety net: rebuilds any scan whose m_pose no longer matches its + // cached GPU buffer, regardless of what changed it (registration + // panels, gizmo, translate tool, settings, scan editor, loop closure + // Gui() -- which can move point cloud poses through paths that don't + // individually call scan_renderer.rebuild()). + scan_renderer.syncPoses(session.point_clouds_container.point_clouds); + + ImGuiIO& io = ImGui::GetIO(); + rlViewport(0, 0, (int)io.DisplaySize.x, (int)io.DisplaySize.y); + + ClearBackground(ColorFromNormalized( + Vector4{bg_color.x * bg_color.w, bg_color.y * bg_color.w, bg_color.z * bg_color.w, bg_color.w})); + rlEnableDepthTest(); + + rlMatrixMode(RL_PROJECTION); + rlLoadIdentity(); + float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); + + updateCameraTransition(); + + viewLocal = Eigen::Affine3f::Identity(); + + if (!is_ortho) + { + reshape((GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); + + // janusz + if (is_loop_closure_gui) + { + if (new_loop_closure_index) + { + // if (index_loop_closure_source < session.point_clouds_container.point_clouds.size()) + //{ + // new_rotation_center.x() = + // session.point_clouds_container.point_clouds[index_loop_closure_source].m_pose.translation().x(); + // new_rotation_center.y() = + // session.point_clouds_container.point_clouds[index_loop_closure_source].m_pose.translation().y(); + // new_rotation_center.z() = + // session.point_clouds_container.point_clouds[index_loop_closure_source].m_pose.translation().z(); + // + // new_translate_x = -new_rotation_center.x(); + // new_translate_y = -new_rotation_center.y(); + // camera_transition_active = true; + //} + + if (session.pose_graph_loop_closure.manipulate_active_edge) + { + if (session.pose_graph_loop_closure.edges.size() > 0) + { + if (session.pose_graph_loop_closure.index_active_edge < session.pose_graph_loop_closure.edges.size()) + { + new_rotation_center.x() = + session.point_clouds_container + .point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge] + .index_from] + .m_pose.translation() + .x(); + new_rotation_center.y() = + session.point_clouds_container + .point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge] + .index_from] + .m_pose.translation() + .y(); + new_rotation_center.z() = + session.point_clouds_container + .point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge] + .index_from] + .m_pose.translation() + .z(); + } + } + + new_rotate_x = rotate_x; + new_rotate_y = rotate_y; + new_translate_x = -new_rotation_center.x(); + new_translate_y = -new_rotation_center.y(); + new_translate_z = translate_z; + camera_transition_active = true; + } + + new_loop_closure_index = false; + } + } + + viewLocal.translate(rotation_center); + + viewLocal.translate(Eigen::Vector3f(translate_x, translate_y, translate_z)); + if (!lock_z) + viewLocal.rotate(Eigen::AngleAxisf(rotate_x * DEG_TO_RAD, Eigen::Vector3f::UnitX())); + else + viewLocal.rotate(Eigen::AngleAxisf(-90.0 * DEG_TO_RAD, Eigen::Vector3f::UnitX())); + viewLocal.rotate(Eigen::AngleAxisf(rotate_y * DEG_TO_RAD, Eigen::Vector3f::UnitZ())); + + viewLocal.translate(-rotation_center); + + rlMultMatrixf(viewLocal.matrix().data()); + } + else + updateOrthoView(); + + frame_mvp_3d = MatrixMultiply(rlGetMatrixModelview(), rlGetMatrixProjection()); + + showAxes(); + + // GNSS/GroundControlPoints/ControlPoints 3D rendering (.render()) is + // legacy-GL, compiled once into `core` and shared with the remaining + // GLUT apps -- not yet ported to raylib (their ImGui panels below, + // .imgui()/.Gui(), are pure ImGui/data and still fully work). Loop + // closure's 3D rendering (below) is ported since it was this session's + // actual focus. + if (!session.control_points.is_imgui) + { + if (is_loop_closure_gui) + renderLoopClosure( + session.point_clouds_container, index_loop_closure_source, index_loop_closure_target, num_edge_extended_before, + num_edge_extended_after); + } + + int prev_index_pose = session.control_points.index_pose; + + if (prev_index_pose != session.control_points.index_pose) + { + session.control_points.index_picked_point = -1; // reset picked point when pose changes + + new_rotation_center.x() = session.point_clouds_container.point_clouds[session.control_points.index_pose].m_pose.translation().x(); + new_rotation_center.y() = session.point_clouds_container.point_clouds[session.control_points.index_pose].m_pose.translation().y(); + new_rotation_center.z() = session.point_clouds_container.point_clouds[session.control_points.index_pose].m_pose.translation().z(); + + new_rotate_x = rotate_x; + new_rotate_y = rotate_y; + if (session.control_points.track_pose_with_camera) + { + new_translate_x = -new_rotation_center.x(); + new_translate_y = -new_rotation_center.y(); + } + else + { + new_translate_x = translate_x; + new_translate_y = translate_y; + } + new_translate_z = translate_z; + camera_transition_active = true; + } + + // rlImGuiBegin() only polls raylib input into ImGui's IO and calls + // ImGui::NewFrame() -- it doesn't touch rlgl's matrix stack, so the 3D + // projection/modelview set up above stays active through all the + // interleaved 3D drawing + ImGui panel-building code below, exactly + // like the original (glBegin/glVertex calls and ImGui:: calls building + // up a draw list are independent of each other either way -- the ImGui + // draw list only actually hits the GPU once, at rlImGuiEnd() near the + // end of this function). + rlImGuiBegin(); + + ShowMainDockSpace(); + + if (session.control_points.is_imgui) + session.control_points.imgui(session.point_clouds_container, rotation_center); + + if (session.ground_control_points.is_imgui) + session.ground_control_points.imgui(session.point_clouds_container); + + if (!session.control_points.is_imgui) + { + 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) + { + // Named-field copy (not a raw struct memcpy): Matrix's + // declared field order isn't guaranteed to match the + // m0..m15 column-major numbering its names imply. + Matrix projMat = rlGetMatrixProjection(); + Matrix modelMat = rlGetMatrixModelview(); + float projection[16] = { + projMat.m0, projMat.m1, projMat.m2, projMat.m3, projMat.m4, projMat.m5, projMat.m6, projMat.m7, + projMat.m8, projMat.m9, projMat.m10, projMat.m11, projMat.m12, projMat.m13, projMat.m14, projMat.m15}; + float modelview[16] = { + modelMat.m0, modelMat.m1, modelMat.m2, modelMat.m3, modelMat.m4, modelMat.m5, modelMat.m6, modelMat.m7, + modelMat.m8, modelMat.m9, modelMat.m10, modelMat.m11, modelMat.m12, modelMat.m13, modelMat.m14, modelMat.m15}; + + 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++) @@ -2475,15 +4124,20 @@ void display() } } - session.point_clouds_container.render(observation_picking, viewer_decimate_point_cloud, 1, session_dims); - - // spdlog::info("session.point_clouds_container.xy_grid_10x10 " << (int)session.point_clouds_container.xy_grid_10x10 << - // std::endl; + // Was PointClouds::render() (legacy-GL, in `core`, shared with + // GLUT apps) -- replaced with scan_renderer, which is kept in + // sync via rebuildAll()/syncPoses() at session-load/pose-change + // sites and each frame (see main()/loadSession() below). + scan_renderer.draw( + session.point_clouds_container.point_clouds, static_cast(point_size), scanColorModeFromScheme(csPointCloud), + static_cast(session_dims.z_min), static_cast(session_dims.z_max), + Eigen::Vector3d(rotation_center.x(), rotation_center.y(), rotation_center.z()), + static_cast(std::max({session_dims.length, session_dims.width, session_dims.height, 1.0})), + viewer_decimate_point_cloud); + scan_renderer.drawTrajectories(session.point_clouds_container.point_clouds, 1, session.point_clouds_container.show_imu_to_lio_diff); - observation_picking.render(); + observationPickingRender(observation_picking); - glPushAttrib(GL_ALL_ATTRIB_BITS); - glPointSize(5); for (const auto& obs : observation_picking.observations) { for (const auto& [key1, value1] : obs) @@ -2503,21 +4157,17 @@ void display() 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(); + DrawSphere(Vector3{static_cast(p1.x()), static_cast(p1.y()), static_cast(p1.z())}, 0.05f, GREEN); + DrawSphere(Vector3{static_cast(p2.x()), static_cast(p2.y()), static_cast(p2.z())}, 0.05f, GREEN); + rlColor3f(1, 0, 0); + rlBegin(RL_LINES); + rlVertex3f(p1.x(), p1.y(), p1.z()); + rlVertex3f(p2.x(), p2.y(), p2.z()); + rlEnd(); } } } } - glPopAttrib(); for (const auto& obs : observation_picking.observations) { @@ -2532,24 +4182,27 @@ void display() { 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(); + // RL_LINE_STRIP isn't supported by rlBegin() (only + // RL_LINES/RL_TRIANGLES/RL_QUADS are) -- each edge of + // the square drawn as its own line segment instead. + rlColor3f(1, 0, 0); + rlBegin(RL_LINES); + rlVertex3f(mean.x() - 1, mean.y() - 1, mean.z()); + rlVertex3f(mean.x() + 1, mean.y() - 1, mean.z()); + rlVertex3f(mean.x() + 1, mean.y() - 1, mean.z()); + rlVertex3f(mean.x() + 1, mean.y() + 1, mean.z()); + rlVertex3f(mean.x() + 1, mean.y() + 1, mean.z()); + rlVertex3f(mean.x() - 1, mean.y() + 1, mean.z()); + rlVertex3f(mean.x() - 1, mean.y() + 1, mean.z()); + rlVertex3f(mean.x() - 1, mean.y() - 1, mean.z()); + rlEnd(); } } - glColor3f(1, 0, 1); - glBegin(GL_POINTS); for (auto p : picked_points) { - glVertex3f(p.x(), p.y(), p.z()); + DrawSphere(Vector3{static_cast(p.x()), static_cast(p.y()), static_cast(p.z())}, 0.05f, MAGENTA); } - glEnd(); } else { @@ -2562,11 +4215,14 @@ void display() if (!is_ortho) { - GLfloat projection[16]; - glGetFloatv(GL_PROJECTION_MATRIX, projection); - - GLfloat modelview[16]; - glGetFloatv(GL_MODELVIEW_MATRIX, modelview); + Matrix projMat = rlGetMatrixProjection(); + Matrix modelMat = rlGetMatrixModelview(); + float projection[16] = { + projMat.m0, projMat.m1, projMat.m2, projMat.m3, projMat.m4, projMat.m5, projMat.m6, projMat.m7, + projMat.m8, projMat.m9, projMat.m10, projMat.m11, projMat.m12, projMat.m13, projMat.m14, projMat.m15}; + float modelview[16] = { + modelMat.m0, modelMat.m1, modelMat.m2, modelMat.m3, modelMat.m4, modelMat.m5, modelMat.m6, modelMat.m7, + modelMat.m8, modelMat.m9, modelMat.m10, modelMat.m11, modelMat.m12, modelMat.m13, modelMat.m14, modelMat.m15}; ImGuizmo::Manipulate( &modelview[0], @@ -3554,6 +5210,27 @@ void display() } } + ImGui::Separator(); + + // Gradient modes -- declared in ColorScheme since the + // original, but never actually wired to a menu item or + // the renderer there; hooked up here to + // scan_renderer's per-point jet-colormap shader. + if (ImGui::MenuItem("> By intensity (gradient)", nullptr, (csPointCloud == CS_GRAD_INTENS))) + csPointCloud = CS_GRAD_INTENS; + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Per-point jet colormap from LAS/LAZ intensity"); + + if (ImGui::MenuItem("> By height (gradient)", nullptr, (csPointCloud == CS_GRAD_ELEV))) + csPointCloud = CS_GRAD_ELEV; + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Per-point jet colormap from world Z, over the session's [z_min, z_max]"); + + if (ImGui::MenuItem("> By distance (gradient)", nullptr, (csPointCloud == CS_GRAD_DIST))) + csPointCloud = CS_GRAD_DIST; + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Per-point jet colormap from distance to the rotation center"); + ImGui::EndMenu(); } @@ -3799,7 +5476,7 @@ void display() translate_tool.step = TranslateTool::Step::Idle; translate_tool.has_transform = false; translate_tool.transform = Eigen::Affine3d::Identity(); - glutSetCursor(GLUT_CURSOR_INHERIT); + SetMouseCursor(MOUSE_CURSOR_DEFAULT); } cor_window(); @@ -3808,14 +5485,18 @@ void display() draw_translate_preview(); + // 3D drawing is done -- switch to the 2D screen-space projection the + // mini-compass (DrawLineEx/DrawText) and rlImGuiEnd()'s UI render both + // need (see end3DMatrixStack()'s comment). + end3DMatrixStack(); + + if (is_loop_closure_gui) + renderLoopClosureLabels(session.point_clouds_container); + if (compass_ruler) drawMiniCompassWithRuler(); - ImGui::Render(); - ImGui_ImplOpenGL2_RenderDrawData(ImGui::GetDrawData()); - - glutSwapBuffers(); - glutPostRedisplay(); + rlImGuiEnd(); } void draw_translate_preview() @@ -3859,21 +5540,21 @@ void draw_translate_preview() double y_len = x_len * 0.5; double z_len = x_len * 0.25; - glLineWidth(3.0f); - glBegin(GL_LINES); - glColor3f(1.0f, 0.0f, 0.0f); - glVertex3d(O.x(), O.y(), O.z()); - glVertex3d(O.x() + x_n.x() * x_len, O.y() + x_n.y() * x_len, O.z()); - - glColor3f(0.0f, 1.0f, 0.0f); - glVertex3d(O.x(), O.y(), O.z()); - glVertex3d(O.x() + y_n.x() * y_len, O.y() + y_n.y() * y_len, O.z()); - - glColor3f(0.0f, 0.0f, 1.0f); - glVertex3d(O.x(), O.y(), O.z()); - glVertex3d(O.x(), O.y(), O.z() + z_len); - glEnd(); - glLineWidth(1.0f); + rlSetLineWidth(3.0f); + rlBegin(RL_LINES); + rlColor3f(1.0f, 0.0f, 0.0f); + rlVertex3f(O.x(), O.y(), O.z()); + rlVertex3f(O.x() + x_n.x() * x_len, O.y() + x_n.y() * x_len, O.z()); + + rlColor3f(0.0f, 1.0f, 0.0f); + rlVertex3f(O.x(), O.y(), O.z()); + rlVertex3f(O.x() + y_n.x() * y_len, O.y() + y_n.y() * y_len, O.z()); + + rlColor3f(0.0f, 0.0f, 1.0f); + rlVertex3f(O.x(), O.y(), O.z()); + rlVertex3f(O.x(), O.y(), O.z() + z_len); + rlEnd(); + rlSetLineWidth(1.0f); } Eigen::Affine3d compute_translate_matrix(const Eigen::Vector3d& O, const Eigen::Vector3d& X, const Eigen::Vector3d& Y_hint) @@ -3927,7 +5608,7 @@ void translate_gui() new_translate_z = translate_z; camera_transition_active = true; - glutSetCursor(GLUT_CURSOR_CROSSHAIR); + SetMouseCursor(MOUSE_CURSOR_CROSSHAIR); } ImGui::SameLine(); if (ImGui::Button("Reset")) @@ -3935,7 +5616,7 @@ void translate_gui() translate_tool.step = TranslateTool::Step::Idle; translate_tool.has_transform = false; translate_tool.transform = Eigen::Affine3d::Identity(); - glutSetCursor(GLUT_CURSOR_INHERIT); + SetMouseCursor(MOUSE_CURSOR_DEFAULT); } const char* step_text = "Idle"; @@ -3991,7 +5672,7 @@ void translate_gui() translate_tool.step = TranslateTool::Step::Idle; translate_tool.has_transform = false; translate_tool.transform = Eigen::Affine3d::Identity(); - glutSetCursor(GLUT_CURSOR_INHERIT); + SetMouseCursor(MOUSE_CURSOR_DEFAULT); } } ImGui::EndDisabled(); @@ -4017,6 +5698,17 @@ Eigen::Vector3d GLWidgetGetOGLPos(int x, int y, const ObservationPicking& observ return pos; } +// Button/state constants formerly from (matching GLUT's own +// values), so mouse()'s body below -- ported verbatim from GLUT's +// glutMouseFunc callback shape -- needed no changes. Called manually from +// the main loop on raylib button-state transitions (see main() below) +// instead of via glutMouseFunc registration. +constexpr int GLUT_LEFT_BUTTON = 0; +constexpr int GLUT_MIDDLE_BUTTON = 1; +constexpr int GLUT_RIGHT_BUTTON = 2; +constexpr int GLUT_DOWN = 0; +constexpr int GLUT_UP = 1; + void mouse(int glut_button, int state, int x, int y) { ImGuiIO& io = ImGui::GetIO(); @@ -4034,9 +5726,9 @@ void mouse(int glut_button, int state, int x, int y) if (button != -1 && state == GLUT_UP) io.MouseDown[button] = false; - static int glutMajorVersion = glutGet(GLUT_VERSION) / 10000; - if (state == GLUT_DOWN && (glut_button == 3 || glut_button == 4) && glutMajorVersion < 3) - wheel(glut_button, glut_button == 3 ? 1 : -1, x, y); + // The GLUT-version-gated legacy mouse-wheel-as-button-3/4 fallback is + // dropped -- raylib's GetMouseWheelMove() (polled in main()'s loop, + // calling wheel() directly) covers this unconditionally. if (!io.WantCaptureMouse) { @@ -4185,6 +5877,37 @@ void mouse(int glut_button, int state, int x, int y) } } +// Was glutInit/glutInitDisplayMode/glutInitWindowSize/glutCreateWindow + +// ImGui_ImplGLUT_Init/ImGui_ImplOpenGL2_Init + glutDisplayFunc/glutMouseFunc/ +// glutMotionFunc/glutMouseWheelFunc/glutKeyboardFunc/glutKeyboardUpFunc -- +// rewritten with raylib's InitWindow + rlImGuiSetup. Kept as a same-named, +// same-signature function (called the same way from main() below) even +// though the display/mouse function pointers are no longer registered as +// GLUT callbacks -- main()'s own loop calls display()/mouse() directly +// instead (see below), and rlImGuiSetup()/raylib's input polling already +// cover what keyboardDown/keyboardUp/motion/wheel used to need GLUT +// callback registration for. +bool initGL(int* argc, char** argv, const std::string& winTitleArg, void (*)(), void (*)(int, int, int, int)) +{ + (void)argc; + (void)argv; + + SetConfigFlags(FLAG_WINDOW_RESIZABLE | FLAG_MSAA_4X_HINT); + InitWindow(static_cast(window_width), static_cast(window_height), winTitleArg.c_str()); + SetTargetFPS(60); + + rlImGuiSetup(true); + ImGuiIO& io = ImGui::GetIO(); + io.ConfigFlags |= ImGuiConfigFlags_NavEnableKeyboard | ImGuiConfigFlags_NavEnableGamepad | ImGuiConfigFlags_DockingEnable; + io.ConfigDockingWithShift = true; + + scan_renderer.init(); + + reshape(static_cast(window_width), static_cast(window_height)); + + return true; +} + int main(int argc, char* argv[]) { try @@ -4222,11 +5945,44 @@ int main(int argc, char* argv[]) } } - glutMainLoop(); + // Was glutMainLoop() -- which repeatedly invoked the registered + // display/mouse/motion/wheel/keyboard callbacks. Those are called + // directly here instead: mouse() on raylib button-state transitions + // (mirroring glutMouseFunc's fire-on-transition semantics), motion() + // every frame (mirroring glutMotionFunc -- motion() itself only acts + // when mouse_buttons is set, so this is safe unconditionally), wheel() + // when GetMouseWheelMove() is nonzero, and display() once per frame. + while (!WindowShouldClose()) + { + int mx = static_cast(GetMouseX()); + int my = static_cast(GetMouseY()); + + if (IsMouseButtonPressed(MOUSE_BUTTON_LEFT)) + mouse(GLUT_LEFT_BUTTON, GLUT_DOWN, mx, my); + if (IsMouseButtonReleased(MOUSE_BUTTON_LEFT)) + mouse(GLUT_LEFT_BUTTON, GLUT_UP, mx, my); + if (IsMouseButtonPressed(MOUSE_BUTTON_RIGHT)) + mouse(GLUT_RIGHT_BUTTON, GLUT_DOWN, mx, my); + if (IsMouseButtonReleased(MOUSE_BUTTON_RIGHT)) + mouse(GLUT_RIGHT_BUTTON, GLUT_UP, mx, my); + if (IsMouseButtonPressed(MOUSE_BUTTON_MIDDLE)) + mouse(GLUT_MIDDLE_BUTTON, GLUT_DOWN, mx, my); + if (IsMouseButtonReleased(MOUSE_BUTTON_MIDDLE)) + mouse(GLUT_MIDDLE_BUTTON, GLUT_UP, mx, my); + + motion(mx, my); + + float wheelMove = GetMouseWheelMove(); + if (wheelMove != 0.0f) + wheel(0, wheelMove > 0.0f ? 1 : -1, mx, my); + + BeginDrawing(); + display(); + EndDrawing(); + } - ImGui_ImplOpenGL2_Shutdown(); - ImGui_ImplGLUT_Shutdown(); - ImGui::DestroyContext(); + rlImGuiShutdown(); + CloseWindow(); } catch (const std::bad_alloc& e) { spdlog::error("System is out of memory : {}", e.what()); diff --git a/cmake/raylib.cmake b/cmake/raylib.cmake new file mode 100644 index 00000000..752c1b9d --- /dev/null +++ b/cmake/raylib.cmake @@ -0,0 +1,69 @@ +include_guard() + +# raylib + Dear ImGui + rlImGui, for the raylib-based step2 viewer +# (apps/multi_view_tls_registration_raylib). Mirrors the integration pattern +# established in the sibling mandeye-colors project. +# +# NOTE: cmake/imgui.cmake already defines a target named `imgui`, statically +# linked with the GLUT/OpenGL2 backend baked in for the legacy GLUT apps. +# That target cannot be reused here (rlImGui needs a plain ImGui core with no +# backend baked in), so this file builds a separate `imgui_raylib` target. + +include(FetchContent) + +set(BUILD_EXAMPLES OFF CACHE BOOL "" FORCE) +set(BUILD_GAMES OFF CACHE BOOL "" FORCE) +FetchContent_Declare( + raylib + GIT_REPOSITORY https://github.com/raysan5/raylib.git + GIT_TAG 5.5 + GIT_SHALLOW TRUE +) +FetchContent_MakeAvailable(raylib) + +# Built from the already-vendored 3rdparty/imgui tree (same source cmake/ +# imgui.cmake's `imgui` target uses, just without the GLUT/OpenGL2 backend +# files -- rlImGui provides its own backend) rather than fetching a separate +# copy. This matters beyond avoiding a redundant fetch: 3rdparty/imgui is on +# the "docking" branch (has ImGui::DockSpace/DockBuilder*/ +# SetNextWindowViewport, IMGUI_HAS_DOCK) -- gui.cpp's ShowMainDockSpace() +# uses those -- whereas a plain tagged release (e.g. v1.92.8) is built from +# "master", which doesn't include docking/viewports. +set(IMGUI_RAYLIB_DIR ${THIRDPARTY_DIRECTORY}/imgui) +add_library(imgui_raylib STATIC + ${IMGUI_RAYLIB_DIR}/imgui.cpp + ${IMGUI_RAYLIB_DIR}/imgui_draw.cpp + ${IMGUI_RAYLIB_DIR}/imgui_tables.cpp + ${IMGUI_RAYLIB_DIR}/imgui_widgets.cpp + ${IMGUI_RAYLIB_DIR}/imgui_demo.cpp +) +target_include_directories(imgui_raylib PUBLIC ${IMGUI_RAYLIB_DIR}) +# NOTE: deliberately NOT defining IMGUI_DISABLE_OBSOLETE_FUNCTIONS (unlike +# mandeye-colors) -- core's WITH_GUI=1 sources (manual_pose_graph_loop_closure.cpp +# etc, shared unchanged with the legacy GLUT app) call a few of ImGui's +# "obsolete" functions (e.g. SetWindowFontScale), which that flag compiles +# out entirely (declaration included), breaking the link against core.a. + +# Pinned to a specific commit (rather than mandeye-colors' floating `main`) +# for build reproducibility. +FetchContent_Declare( + rlimgui_src + GIT_REPOSITORY https://github.com/raylib-extras/rlImGui.git + GIT_TAG ef129d1858373b6fe332c45f85a1dfc1421bebae +) +FetchContent_MakeAvailable(rlimgui_src) + +add_library(rlimgui STATIC ${rlimgui_src_SOURCE_DIR}/rlImGui.cpp) +target_include_directories(rlimgui PUBLIC ${rlimgui_src_SOURCE_DIR}) +target_link_libraries(rlimgui PUBLIC raylib imgui_raylib) + +# ImGuizmo -- reuses the already-vendored 3rdparty/ImGuizmo sources (pure +# ImGui immediate-mode drawing via ImDrawList, no GL/GLUT calls of its own), +# but built as a SEPARATE static lib against imgui_raylib's headers rather +# than reusing the existing `imguizmo` target (cmake/imguizmo.cmake), which +# is compiled against the vendored 3rdparty/imgui (a different version) -- +# mixing the two ImGui ABIs in one binary would be unsafe. +set(IMGUIZMO_RAYLIB_DIR ${THIRDPARTY_DIRECTORY}/ImGuizmo) +add_library(imguizmo_raylib STATIC ${IMGUIZMO_RAYLIB_DIR}/ImGuizmo.cpp) +target_include_directories(imguizmo_raylib PUBLIC ${IMGUIZMO_RAYLIB_DIR}) +target_link_libraries(imguizmo_raylib PUBLIC imgui_raylib) diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 15793f8d..a13e0ef9 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -79,4 +79,43 @@ target_precompile_headers(core_no_gui ) set_target_properties(core_no_gui PROPERTIES - POSITION_INDEPENDENT_CODE ON) \ No newline at end of file + POSITION_INDEPENDENT_CODE ON) + +# ============================================================================ +# core_raylib -- shared raylib rendering/camera/picking infrastructure +# (ScanRenderer, OrbitCamera, picking, the compass overlay), used by +# apps/multi_view_tls_registration (step2, raylib-based) and any future +# raylib-based HDMapping GUI. This is the raylib-app equivalent of +# core/src/utils.cpp's role for the remaining GLUT apps. Kept as a separate +# target (not merged into `core`/`core_no_gui` above) so that raylib is never +# a dependency of apps that still use GLUT. +# ============================================================================ +add_library(core_raylib STATIC src/raylib_render.cpp) +target_compile_definitions(core_raylib PRIVATE WITH_GUI=1) +target_include_directories(core_raylib PRIVATE + include + ${EIGEN3_INCLUDE_DIR} + ${LASZIP_INCLUDE_DIR}/LASzip/include + ${THIRDPARTY_DIRECTORY}/json/include + ${THIRDPARTY_DIRECTORY}/observation_equations/codes + ${EXTERNAL_LIBRARIES_DIRECTORY}/include + ${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp + ${THIRDPARTY_DIRECTORY}/Fusion/Fusion +) +# PUBLIC: consumers (apps/multi_view_tls_registration) get core's +# Session/PointCloud types and raylib/rlgl/raymath transitively just by +# linking core_raylib. This target only needs raylib itself (rlgl, +# raymath, glad) -- rlImGui/Dear ImGui are used by the app's own panel +# code, not by raylib_render.cpp, so they're linked by the app directly. +# +# FREEGLUT_LIBRARY is linked here (not by the consuming app) so that +# CMake's link-order computation places it immediately alongside `core`: +# core.a's own object files (manual_pose_graph_loop_closure.cpp.o, +# observation_picking.cpp.o) reference glutBitmap*() symbols transitively +# (same translation unit as the Gui()/data methods raylib consumers do +# call, even though the ::Render() methods that use GLUT text are never +# actually invoked at runtime here -- no glutInit/window exists). Linking +# it as a separate PRIVATE entry on the final executable instead let +# CMake's static-library dependency reordering sort libglut.so before +# libcore.a, leaving those symbols unresolved. +target_link_libraries(core_raylib PUBLIC core raylib ${FREEGLUT_LIBRARY}) \ No newline at end of file diff --git a/core/include/Core/raylib_render.hpp b/core/include/Core/raylib_render.hpp new file mode 100644 index 00000000..c0e540e4 --- /dev/null +++ b/core/include/Core/raylib_render.hpp @@ -0,0 +1,174 @@ +#pragma once + +// GPU (rlgl-based) point cloud rendering, shared across any raylib-based +// HDMapping GUI (currently just multi_view_tls_registration, step2 -- +// raylib-based since it was ported off GLUT/legacy-GL). This is the +// raylib-app equivalent of the role core/src/utils.cpp plays for the +// remaining GLUT apps: shared GL-context-specific rendering glue that +// individual tools' GUI code builds on top of. +// +// Core's own PointCloud::render()/PointClouds::render() are legacy +// immediate-mode OpenGL (glBegin/glVertex/...), unavailable under raylib's +// OpenGL 3.3 core-profile context, so ScanRenderer below is new code rather +// than a port of those methods -- it reads the same public PointCloud data +// (points_local, m_pose, render_color, visible, ...) instead. +// +// Coordinates are used as-is (no Y-up/Z-up remap): the one consumer of this +// header drives its own Z-up camera (rlMatrixMode/rlFrustum/rlMultMatrixf +// against Eigen-computed matrices, matching the original GLUT app's +// convention) rather than raylib's native Y-up Camera3D, so points are +// uploaded and drawn in HDMapping's native Z-up world frame directly. +// +// Only built when raylib is fetched (see cmake/raylib.cmake and +// core/CMakeLists.txt's core_raylib target) -- this header is not usable +// from the GLUT apps. + +#include "raylib.h" + +#include +#include + +#include +#include + +#include + +// ============================================================================ +// ScanRenderer -- GPU (rlgl-based) rendering of Core::PointCloud scans. +// ============================================================================ +// Per-point GPU color modes for ScanRenderer::draw() -- jet-colormap +// gradients computed in the shader from data already on the GPU (per-point +// intensity, or world-space position for Elevation/Distance), rather than +// per-scan flat colors. +enum class ScanColorMode +{ + Flat, // pc.render_color, uniform per scan (the original app's only mode) + Intensity, // jet colormap by normalized LAS/LAZ intensity + Elevation, // jet colormap by world-space Z, normalized over [elevationMin, elevationMax] + Distance, // jet colormap by distance from distanceCenter, normalized over [0, distanceMax] +}; + +class ScanRenderer +{ +public: + ScanRenderer() = default; + ~ScanRenderer(); + + ScanRenderer(const ScanRenderer&) = delete; + ScanRenderer& operator=(const ScanRenderer&) = delete; + + // Must be called once, after the raylib window/GL context exists. + void init(); + void shutdown(); + + // Rebuilds the world-space GPU point+intensity buffer for a single scan + // from its current points_local/m_pose/intensities. Call whenever a + // scan's pose or point data changes. + void rebuild(size_t index, const PointCloud& pc); + + // Keeps the internal per-scan GPU buffer list in sync with + // point_clouds_container.point_clouds (adds/removes entries, does not + // rebuild existing ones). + void syncCount(const std::vector& pointClouds); + + // Rebuilds every scan's GPU buffer (e.g. after a session load). + void rebuildAll(const std::vector& pointClouds); + + // Rebuilds only the GPU buffers whose scan's m_pose has changed since + // the last call (a plain 4x4 comparison per scan -- cheap even for + // hundreds of scans). Safety net against pose-mutating call sites that + // forget to call rebuild()/rebuildAll(): call this once per frame, + // right before draw(). + void syncPoses(const std::vector& pointClouds); + + // colorMode switches every scan from its flat render_color to a + // per-point jet-colormap gradient (see ScanColorMode). Intensity is + // normalized per scan at rebuild() time (raw intensity range varies by + // sensor); Elevation/Distance are normalized using the elevationMin/Max + // and distanceCenter/distanceMax parameters, which the caller computes + // once per frame from whatever it considers the current scene bounds + // (e.g. session_dims.z_min/z_max, rotation_center) -- ScanRenderer has + // no notion of session bounds itself. A scan with a mark color set (see + // setMarkColor()) always draws flat in that color instead, regardless + // of colorMode, so it stays visible as a highlight either way. + // decimateStride > 1 draws only every Nth point of each scan (a GPU-side + // vertex fetch stride, not a re-upload -- see the .cpp), for interactive + // framerates while navigating very large sessions; 1 draws every point + // (rebuild()'s default). Uses whatever rlgl projection/modelview + // matrices are currently active (BeginMode3D or a manually-driven + // rlMatrixMode/rlMultMatrixf stack, either works). + void draw( + const std::vector& pointClouds, + float pointSize, + ScanColorMode colorMode, + float elevationMin = 0.f, + float elevationMax = 1.f, + const Eigen::Vector3d& distanceCenter = Eigen::Vector3d::Zero(), + float distanceMax = 1.f, + int decimateStride = 1) const; + + // Overrides a scan's draw() color with a flat mark color -- e.g. to + // highlight the loop-closure source/target scans in red/blue -- drawn + // from the same cached full-resolution GPU buffer as a normal scan + // (just a different uniform), rather than needing a separate ad-hoc + // redraw pass. Persists across frames until cleared; callers that only + // want a highlight for the current frame (e.g. because the highlighted + // set can change every frame) should call clearMarks() first. Silently + // ignored if index is out of range (e.g. before syncCount() has caught + // up with a just-added scan). + void setMarkColor(size_t index, Color color); + void clearMarkColor(size_t index); + void clearMarks(); + + // Ported from PointCloud::render()'s trajectory section (core/src/point_cloud.cpp): + // draws each visible scan's local_trajectory (posed by m_pose) as a line + // strip in its traj_color (skipped if pc.line_width <= 0), decimated by + // reduceRenderedTrajectory (mirrors "viewer_reduce_rendered_trajectory"), + // plus the fuse-inclination-from-IMU quad markers, the fixed-om/fi rings, + // and the show_IMU/show_pose orientation crosses. If visibleImuDiff is + // set, also draws the IMU-vs-LIO angular-difference debug lines. + // Intersection-slab gating (xz/yz/xy) is not implemented so these always + // draw when the relevant per-scan flag is set. + void drawTrajectories(const std::vector& pointClouds, int reduceRenderedTrajectory, bool visibleImuDiff) const; + + // Draws a single already-cached scan (see rebuild()) straight from its + // persistent, full-resolution GPU buffer -- no CPU re-transform or + // re-upload -- displaced by an extra transform on top of the pose it + // was cached at. extraTransform is the delta from the scan's current + // m_pose to the desired preview pose (i.e. previewPose * m_pose.inverse()); + // 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. + void drawCachedWithTransform(size_t index, const Eigen::Affine3d& extraTransform, Color color, float pointSize, bool useIntensityColor) const; + +private: + struct CloudGPU + { + unsigned int vao = 0; + unsigned int vbo = 0; + int count = 0; + Eigen::Affine3d lastPose = Eigen::Affine3d::Identity(); + bool hasPose = false; + bool hasMarkColor = false; + Color markColor = WHITE; + }; + + void unload(CloudGPU& cloud); + + std::vector clouds_; + Shader shader_{}; + bool shaderValid_ = false; + int locMVP_ = -1; + int locPointSize_ = -1; + int locColor_ = -1; + int locColorMode_ = -1; + int locElevMin_ = -1; + int locElevMax_ = -1; + int locDistCenter_ = -1; + int locDistMax_ = -1; +}; diff --git a/core/src/raylib_render.cpp b/core/src/raylib_render.cpp new file mode 100644 index 00000000..f37aa092 --- /dev/null +++ b/core/src/raylib_render.cpp @@ -0,0 +1,580 @@ +#include +#include + +#include "raymath.h" +#include "rlgl.h" +#include "external/glad.h" + +#include +#include +#include +#include + +namespace +{ +// Points/poses are used in HDMapping's native Z-up world frame directly (no +// Y-up remap) -- see the header comment for why. +inline Vector3 toVec3(const Eigen::Vector3d& p) +{ + return Vector3{static_cast(p.x()), static_cast(p.y()), static_cast(p.z())}; +} +} // namespace + +// ============================================================================ +// ScanRenderer +// ============================================================================ +namespace +{ +// vertexIntensity is per-point LAS/LAZ intensity, normalized to [0,1] per +// scan at upload time (see ScanRenderer::rebuild). vertexPosition is +// already world-space (rebuild() applies pc.m_pose before uploading), so +// it doubles as the Elevation/Distance color modes' input with no extra +// per-vertex data needed. colorMode selects between the flat per-scan +// pointColor (0, the original app's only mode) and the ScanColorMode +// gradients (1/2/3), matching the enum's Intensity/Elevation/Distance +// ordering exactly (see ScanRenderer::draw()'s static_cast below). +const char* kPointVS = R"( +#version 330 +in vec3 vertexPosition; +in float vertexIntensity; +uniform mat4 mvp; +uniform float pointSize; +out float fragIntensity; +out vec3 fragWorldPos; +void main() +{ + gl_Position = mvp * vec4(vertexPosition, 1.0); + gl_PointSize = pointSize; + fragIntensity = vertexIntensity; + fragWorldPos = vertexPosition; +} +)"; + +const char* kPointFS = R"( +#version 330 +uniform vec4 pointColor; +uniform int colorMode; +uniform float elevMin; +uniform float elevMax; +uniform vec3 distCenter; +uniform float distMax; +in float fragIntensity; +in vec3 fragWorldPos; +out vec4 finalColor; + +vec3 jet(float t) +{ + t = clamp(t, 0.0, 1.0); + float r = clamp(1.5 - abs(4.0 * t - 3.0), 0.0, 1.0); + float g = clamp(1.5 - abs(4.0 * t - 2.0), 0.0, 1.0); + float b = clamp(1.5 - abs(4.0 * t - 1.0), 0.0, 1.0); + return vec3(r, g, b); +} + +void main() +{ + if (colorMode == 1) + { + finalColor = vec4(jet(fragIntensity), pointColor.a); + } + else if (colorMode == 2) + { + float range = max(elevMax - elevMin, 1e-6); + finalColor = vec4(jet((fragWorldPos.z - elevMin) / range), pointColor.a); + } + else if (colorMode == 3) + { + float d = length(fragWorldPos - distCenter); + finalColor = vec4(jet(d / max(distMax, 1e-6)), pointColor.a); + } + else + { + finalColor = pointColor; + } +} +)"; + +// Bytes per vertex in the uploaded buffer (xyz position + intensity). +constexpr int kVertexStride = 4 * sizeof(float); +} // namespace + +ScanRenderer::~ScanRenderer() +{ + shutdown(); +} + +void ScanRenderer::init() +{ + shader_ = LoadShaderFromMemory(kPointVS, kPointFS); + shaderValid_ = shader_.id > 0; + if (shaderValid_) + { + locMVP_ = rlGetLocationUniform(shader_.id, "mvp"); + locPointSize_ = rlGetLocationUniform(shader_.id, "pointSize"); + locColor_ = rlGetLocationUniform(shader_.id, "pointColor"); + locColorMode_ = rlGetLocationUniform(shader_.id, "colorMode"); + locElevMin_ = rlGetLocationUniform(shader_.id, "elevMin"); + locElevMax_ = rlGetLocationUniform(shader_.id, "elevMax"); + locDistCenter_ = rlGetLocationUniform(shader_.id, "distCenter"); + locDistMax_ = rlGetLocationUniform(shader_.id, "distMax"); + } + else + { + TraceLog(LOG_ERROR, "ScanRenderer: point shader failed to compile"); + } + + // Required for gl_PointSize (set from the vertex shader) to take effect + // under a core-profile context. + glEnable(GL_PROGRAM_POINT_SIZE); +} + +void ScanRenderer::shutdown() +{ + for (auto& c : clouds_) + { + unload(c); + } + clouds_.clear(); + + if (shaderValid_) + { + UnloadShader(shader_); + shaderValid_ = false; + } +} + +void ScanRenderer::unload(CloudGPU& cloud) +{ + if (cloud.vao) + { + rlUnloadVertexArray(cloud.vao); + cloud.vao = 0; + } + if (cloud.vbo) + { + rlUnloadVertexBuffer(cloud.vbo); + cloud.vbo = 0; + } + cloud.count = 0; +} + +void ScanRenderer::syncCount(const std::vector& pointClouds) +{ + if (clouds_.size() > pointClouds.size()) + { + for (size_t i = pointClouds.size(); i < clouds_.size(); ++i) + { + unload(clouds_[i]); + } + } + clouds_.resize(pointClouds.size()); +} + +void ScanRenderer::rebuild(size_t index, const PointCloud& pc) +{ + if (index >= clouds_.size()) + { + return; + } + + CloudGPU& gpu = clouds_[index]; + unload(gpu); + + if (pc.points_local.empty() || !shaderValid_) + { + // Still record the pose so syncPoses() doesn't retry this scan every + // frame -- there's nothing to rebuild until it has points anyway. + gpu.lastPose = pc.m_pose; + gpu.hasPose = true; + return; + } + + // Every point in points_local is uploaded -- no stride/skip here, so + // loading a session always renders it at full point density. + // Session::load's own voxel-bucket decimation, controlled by the + // Settings panel's "Downsample during load" checkbox, is unrelated and + // untouched -- it's shared unchanged with the legacy GLUT app. + const bool hasIntensity = pc.intensities.size() == pc.points_local.size(); + float minIntensity = std::numeric_limits::max(); + float maxIntensity = -std::numeric_limits::max(); + if (hasIntensity) + { + for (unsigned short raw : pc.intensities) + { + float v = static_cast(raw); + minIntensity = std::min(minIntensity, v); + maxIntensity = std::max(maxIntensity, v); + } + } + const float intensityRange = (hasIntensity && maxIntensity > minIntensity) ? (maxIntensity - minIntensity) : 1.0f; + + std::vector data; + data.reserve(pc.points_local.size() * 4); + for (size_t i = 0; i < pc.points_local.size(); ++i) + { + Eigen::Vector3d world = pc.m_pose * pc.points_local[i]; + Vector3 rp = toVec3(world); + data.push_back(rp.x); + data.push_back(rp.y); + data.push_back(rp.z); + data.push_back(hasIntensity ? (static_cast(pc.intensities[i]) - minIntensity) / intensityRange : 0.0f); + } + + if (data.empty()) + { + return; + } + + gpu.vao = rlLoadVertexArray(); + rlEnableVertexArray(gpu.vao); + gpu.vbo = rlLoadVertexBuffer(data.data(), static_cast(data.size() * sizeof(float)), false); + rlSetVertexAttribute(0, 3, RL_FLOAT, false, kVertexStride, 0); + rlEnableVertexAttribute(0); + rlSetVertexAttribute(1, 1, RL_FLOAT, false, kVertexStride, 3 * sizeof(float)); + rlEnableVertexAttribute(1); + rlDisableVertexArray(); + + gpu.count = static_cast(data.size() / 4); + gpu.lastPose = pc.m_pose; + gpu.hasPose = true; +} + +void ScanRenderer::rebuildAll(const std::vector& pointClouds) +{ + syncCount(pointClouds); + for (size_t i = 0; i < pointClouds.size(); ++i) + { + rebuild(i, pointClouds[i]); + } +} + +void ScanRenderer::syncPoses(const std::vector& pointClouds) +{ + syncCount(pointClouds); + for (size_t i = 0; i < pointClouds.size(); ++i) + { + const CloudGPU& gpu = clouds_[i]; + if (!gpu.hasPose || !gpu.lastPose.isApprox(pointClouds[i].m_pose, 1e-9)) + { + rebuild(i, pointClouds[i]); + } + } +} + +void ScanRenderer::draw( + const std::vector& pointClouds, + float pointSize, + ScanColorMode colorMode, + float elevationMin, + float elevationMax, + const Eigen::Vector3d& distanceCenter, + float distanceMax, + int decimateStride) const +{ + if (!shaderValid_) + { + return; + } + + // Flush raylib's own pending immediate-mode batch (grid/lines) before + // issuing raw draw calls, so draw order stays correct. + rlDrawRenderBatchActive(); + + Matrix mvp = MatrixMultiply(rlGetMatrixModelview(), rlGetMatrixProjection()); + + rlEnableShader(shader_.id); + rlSetUniformMatrix(locMVP_, mvp); + rlSetUniform(locPointSize_, &pointSize, RL_SHADER_UNIFORM_FLOAT, 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); + + // decimateStride > 1 skips points by widening the vertex attribute + // stride the GPU fetches from (e.g. stride=10 reads every 10th vertex), + // rather than by re-uploading a thinned-out buffer -- so it's a pure + // per-draw-call setting with no CPU cost and no ScanRenderer::rebuild() + // needed when the user changes it. Mirrors the legacy GLUT app's + // "Points render downsampling" (viewer_decimate_point_cloud) stride + // skip, ported here as a GPU-side fetch stride instead of a CPU-side + // point-array stride since points are already resident on the GPU. + const int stride = std::max(1, decimateStride); + const int byteStride = kVertexStride * stride; + + for (size_t i = 0; i < clouds_.size() && i < pointClouds.size(); ++i) + { + const PointCloud& pc = pointClouds[i]; + const CloudGPU& gpu = clouds_[i]; + if (!pc.visible || gpu.count == 0) + { + continue; + } + + float color[4]; + int colorModeInt; + if (gpu.hasMarkColor) + { + color[0] = gpu.markColor.r / 255.f; + color[1] = gpu.markColor.g / 255.f; + color[2] = gpu.markColor.b / 255.f; + color[3] = gpu.markColor.a / 255.f; + colorModeInt = 0; + } + else + { + color[0] = pc.render_color[0]; + color[1] = pc.render_color[1]; + color[2] = pc.render_color[2]; + color[3] = 1.0f; + // Matches the shader's colorMode branches (0=flat, 1=intensity, + // 2=elevation, 3=distance) exactly, since ScanColorMode's + // enumerator order was chosen to match. + colorModeInt = static_cast(colorMode); + } + rlSetUniform(locColor_, color, RL_SHADER_UNIFORM_VEC4, 1); + rlSetUniform(locColorMode_, &colorModeInt, RL_SHADER_UNIFORM_INT, 1); + + rlEnableVertexArray(gpu.vao); + // Re-specifies both attributes' stride against the VAO's cached VBO + // binding every draw (cheap: a handful of GL calls per visible + // scan), since the VAO otherwise keeps whatever stride rebuild() + // baked in and there's no persistent "decimated" VAO to switch to. + rlEnableVertexBuffer(gpu.vbo); + rlSetVertexAttribute(0, 3, RL_FLOAT, false, byteStride, 0); + rlEnableVertexAttribute(0); + rlSetVertexAttribute(1, 1, RL_FLOAT, false, byteStride, 3 * sizeof(float)); + rlEnableVertexAttribute(1); + + const int drawCount = (gpu.count + stride - 1) / stride; + glDrawArrays(GL_POINTS, 0, drawCount); + rlDisableVertexArray(); + } + + rlDisableShader(); +} + +void ScanRenderer::setMarkColor(size_t index, Color color) +{ + if (index < clouds_.size()) + { + clouds_[index].hasMarkColor = true; + clouds_[index].markColor = color; + } +} + +void ScanRenderer::clearMarkColor(size_t index) +{ + if (index < clouds_.size()) + { + clouds_[index].hasMarkColor = false; + } +} + +void ScanRenderer::clearMarks() +{ + for (auto& c : clouds_) + { + c.hasMarkColor = false; + } +} + +namespace +{ +// Converts an Eigen::Affine3d (in HDMapping's native Z-up world frame, used +// as-is -- see the header comment) to the equivalent raylib Matrix. No +// coordinate remap needed (unlike an earlier version of this function), +// since points/poses are no longer converted to a separate Y-up space. +Matrix toRaylibMatrix(const Eigen::Affine3d& t) +{ + const Eigen::Matrix3d& r = t.linear(); + const Eigen::Vector3d& tr = t.translation(); + + Matrix m{}; + m.m0 = static_cast(r(0, 0)); + m.m4 = static_cast(r(0, 1)); + m.m8 = static_cast(r(0, 2)); + m.m12 = static_cast(tr.x()); + m.m1 = static_cast(r(1, 0)); + m.m5 = static_cast(r(1, 1)); + m.m9 = static_cast(r(1, 2)); + m.m13 = static_cast(tr.y()); + m.m2 = static_cast(r(2, 0)); + m.m6 = static_cast(r(2, 1)); + m.m10 = static_cast(r(2, 2)); + m.m14 = static_cast(tr.z()); + m.m3 = 0.f; + m.m7 = 0.f; + m.m11 = 0.f; + m.m15 = 1.f; + return m; +} +} // namespace + +void ScanRenderer::drawCachedWithTransform(size_t index, const Eigen::Affine3d& extraTransform, Color color, float pointSize, bool useIntensityColor) const +{ + if (!shaderValid_ || index >= clouds_.size()) + { + return; + } + + const CloudGPU& gpu = clouds_[index]; + if (gpu.count == 0) + { + return; + } + + rlDrawRenderBatchActive(); + + Matrix mvpBase = MatrixMultiply(rlGetMatrixModelview(), rlGetMatrixProjection()); + // MatrixMultiply(A, B) composes as "apply A, then B" (see draw()'s own + // modelview-then-projection use above) -- so this applies the delta + // transform to the cached points first, then the normal view/projection. + 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; + + 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); + + rlEnableVertexArray(gpu.vao); + // Explicitly re-specified (not just inherited from the VAO's last + // binding) since draw() rebinds this same VAO's attributes with a + // decimation-widened stride every frame -- this draw always wants every + // cached point, independent of the current "sparse drawing" setting. + rlEnableVertexBuffer(gpu.vbo); + rlSetVertexAttribute(0, 3, RL_FLOAT, false, kVertexStride, 0); + rlEnableVertexAttribute(0); + rlSetVertexAttribute(1, 1, RL_FLOAT, false, kVertexStride, 3 * sizeof(float)); + rlEnableVertexAttribute(1); + + glDrawArrays(GL_POINTS, 0, gpu.count); + rlDisableVertexArray(); + rlDisableShader(); +} + +namespace +{ +constexpr double RAD_TO_DEG = 180.0 / M_PI; + +// Ported from the "fuse_inclination_from_IMU" / "fixed_om && fixed_fi" / +// "show_IMU" / "show_pose" quad-and-cross markers in +// core/src/point_cloud.cpp's PointCloud::render(). +void drawSquareOutline(const Eigen::Affine3d& m, double half, Color color) +{ + Eigen::Vector3d a1 = m * Eigen::Vector3d(-half, -half, 0); + Eigen::Vector3d a2 = m * Eigen::Vector3d(half, -half, 0); + Eigen::Vector3d a3 = m * Eigen::Vector3d(half, half, 0); + Eigen::Vector3d a4 = m * Eigen::Vector3d(-half, half, 0); + + Vector3 p1 = toVec3(a1); + Vector3 p2 = toVec3(a2); + Vector3 p3 = toVec3(a3); + Vector3 p4 = toVec3(a4); + + DrawLine3D(p1, p2, color); + DrawLine3D(p2, p3, color); + DrawLine3D(p3, p4, color); + DrawLine3D(p4, p1, color); +} + +void drawOrientationCross(const Eigen::Affine3d& m) +{ + Vector3 origin = toVec3(m.translation()); + DrawLine3D(origin, toVec3(m.translation() + m.linear().col(0)), RED); + DrawLine3D(origin, toVec3(m.translation() + m.linear().col(1)), GREEN); + DrawLine3D(origin, toVec3(m.translation() + m.linear().col(2)), BLUE); +} + +// The IMU-fused orientation (used by both the fuse-inclination quad marker +// and show_IMU) mirrors the local pose's translation with orientation taken +// from the first local_trajectory node's raw IMU om/fi/ka instead of the +// LIO-optimized pose. +Eigen::Affine3d imuOrientationAtPose(const PointCloud& pc) +{ + TaitBryanPose tb; + tb.px = pc.m_pose(0, 3); + tb.py = pc.m_pose(1, 3); + tb.pz = pc.m_pose(2, 3); + tb.om = pc.local_trajectory[0].imu_om_fi_ka.x(); + tb.fi = pc.local_trajectory[0].imu_om_fi_ka.y(); + tb.ka = pc.local_trajectory[0].imu_om_fi_ka.z(); + return affine_matrix_from_pose_tait_bryan(tb); +} +} // namespace + +void ScanRenderer::drawTrajectories(const std::vector& pointClouds, int reduceRenderedTrajectory, bool visibleImuDiff) const +{ + int stride = reduceRenderedTrajectory < 1 ? 1 : reduceRenderedTrajectory; + + for (const auto& pc : pointClouds) + { + if (!pc.visible || pc.local_trajectory.empty()) + { + continue; + } + + if (visibleImuDiff) + { + for (size_t i = 1; i < pc.local_trajectory.size(); ++i) + { + Eigen::Affine3d m = pc.m_pose * pc.local_trajectory[i].m_pose; + Vector3 origin = toVec3(m.translation()); + const auto& diff = pc.local_trajectory[i].imu_diff_angle_om_fi_ka_deg; + DrawLine3D(origin, toVec3(m.translation() + Eigen::Vector3d(diff.x() * 10, 0, 0)), RED); + DrawLine3D(origin, toVec3(m.translation() + Eigen::Vector3d(0, diff.y() * 10, 0)), GREEN); + DrawLine3D(origin, toVec3(m.translation() + Eigen::Vector3d(0, 0, diff.z() * 10)), BLUE); + } + } + + if (pc.line_width > 0 && pc.local_trajectory.size() >= 2) + { + Color c = Color{ + static_cast(pc.traj_color[0] * 255.f), + static_cast(pc.traj_color[1] * 255.f), + static_cast(pc.traj_color[2] * 255.f), + 255}; + + Vector3 prev = toVec3((pc.m_pose * pc.local_trajectory[0].m_pose).translation()); + for (size_t i = stride; i < pc.local_trajectory.size(); i += stride) + { + Vector3 cur = toVec3((pc.m_pose * pc.local_trajectory[i].m_pose).translation()); + DrawLine3D(prev, cur, c); + prev = cur; + } + } + + if (pc.fuse_inclination_from_IMU) + { + drawSquareOutline(pc.m_pose, 0.2, GREEN); + drawSquareOutline(imuOrientationAtPose(pc), 0.2, RED); + } + + if (pc.fixed_om && pc.fixed_fi) + { + for (double x = 0.4; x <= 1.0; x += 0.1) + { + drawSquareOutline(pc.m_pose, x, RED); + } + } + + if (pc.show_IMU) + { + drawOrientationCross(imuOrientationAtPose(pc)); + } + + if (pc.show_pose) + { + drawOrientationCross(pc.m_pose); + } + } +} + +// Mini compass + ruler: ported to gui.cpp's own drawMiniCompassWithRuler() +// (it needs this app's own viewLocal/translate_z globals, not a Camera3D), +// so no longer lives here. From 9e3db087a490d364e8e2200187db3cfedf1287ec Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Sat, 25 Jul 2026 03:31:32 +0200 Subject: [PATCH 02/17] Split raylib-based camera/picking/misc-widget code into rl_utils.cpp/.h Moves the ~1300 lines of camera/picking/mini-compass/misc-ImGui-widget code (this app's raylib-based replacement for the shared core/src/utils.cpp API) out of multi_view_tls_registration_gui.cpp into their own rl_utils.cpp/rl_utils.h files, mirroring how core/src/utils.cpp / Core/utils.hpp were structured for the GLUT apps. Co-Authored-By: Claude Sonnet 5 Claude-Session: https://claude.ai/code/session_01XgxCVLnEEAftKKRvZHPUB9 --- .../CMakeLists.txt | 16 +- .../multi_view_tls_registration_gui.cpp | 1367 +---------------- apps/multi_view_tls_registration/rl_utils.cpp | 1260 +++++++++++++++ apps/multi_view_tls_registration/rl_utils.h | 167 ++ 4 files changed, 1493 insertions(+), 1317 deletions(-) create mode 100644 apps/multi_view_tls_registration/rl_utils.cpp create mode 100644 apps/multi_view_tls_registration/rl_utils.h diff --git a/apps/multi_view_tls_registration/CMakeLists.txt b/apps/multi_view_tls_registration/CMakeLists.txt index 5e523051..58e7e19e 100644 --- a/apps/multi_view_tls_registration/CMakeLists.txt +++ b/apps/multi_view_tls_registration/CMakeLists.txt @@ -4,13 +4,16 @@ project(multi_view_tls_registration_step_2) # Source files # -# multi_view_tls_registration_gui.cpp is raylib-based: camera/picking/mini- -# compass/GLUT-window-and-input-loop code that used to come from +# multi_view_tls_registration_gui.cpp is raylib-based: the GLUT-window-and- +# input-loop and point-cloud/loop-closure rendering code that used to go +# through core's own legacy-GL PointCloud::render()/PointClouds::render() +# now uses Core/raylib_render.hpp's ScanRenderer instead. +# +# rl_utils.cpp/rl_utils.h are this app's own raylib-based replacement for +# the camera/picking/mini-compass/misc-ImGui-widget API it used to get from # core/src/utils.cpp (shared with the remaining GLUT apps, so it can't be -# changed) is now implemented locally in the .cpp itself, using rlgl's rl*() -# legacy-GL-emulation API and Core/raylib_render.hpp's ScanRenderer for the -# point-cloud GPU rendering that used to go through core's own legacy-GL -# PointCloud::render()/PointClouds::render(). +# changed), using rlgl's rl*() legacy-GL-emulation API instead of real +# gl*()/glu*()/glut*() calls -- see rl_utils.h's top comment. # # perform_experiment.cpp still #includes GLUT/glew/imgui-GLUT-backend headers, # but never calls anything from them (verified: no glut*/GL_*/ImGui_Impl* @@ -19,6 +22,7 @@ project(multi_view_tls_registration_step_2) set(SOURCES multi_view_tls_registration.cpp perform_experiment.cpp multi_view_tls_registration_gui.cpp multi_view_tls_registration.h + rl_utils.cpp rl_utils.h ../lidar_odometry_step_1/lidar_odometry_utils.cpp ) 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 d5b583ce..5cbba2b7 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 @@ -73,114 +73,12 @@ #endif -/////////////////////////////////////////////////////////////////////////////////// -// Formerly 's constants/enum/extern globals -- local now (see the -// big comment at the top of this file for why). -/////////////////////////////////////////////////////////////////////////////////// - -const float DEG_TO_RAD = M_PI / 180.0f; -const float RAD_TO_DEG = 180.0f / M_PI; - -const ImVec4 orangeBorder(1.0f, 0.5f, 0.0f, 1.0f); - -const std::string out_fn = "Output file name"; - -constexpr float ImGuiNumberWidth = 120.0f; -constexpr const char* omText = "Roll (left/right)"; -constexpr const char* fiText = "Pitch (up/down)"; -constexpr const char* kaText = "Yaw (turning left/right)"; -constexpr const char* xText = "Longitudinal (forward/backward)"; -constexpr const char* yText = "Lateral (left/right)"; -constexpr const char* zText = "Vertical (up/down)"; - -const uint32_t window_width = 1600; -const uint32_t window_height = 900; - -const float camera_transition_speed = 1.0f; // higher = faster - -enum CameraPreset -{ - CAMERA_FRONT, - CAMERA_BACK, - CAMERA_LEFT, - CAMERA_RIGHT, - CAMERA_TOP, - CAMERA_BOTTOM, - CAMERA_ISO, - CAMERA_RESET -}; - -enum ColorScheme -{ - CS_SOLID, // fixed color - CS_RANDOM, // random - CS_GRAD_INTENS, // gradient based on intensity - CS_GRAD_ELEV, // gradient based on elevation - CS_GRAD_DIST, // gradient based on distance from rotation center - CS_FOLLOW // valid for trajectory -}; - -int viewer_decimate_point_cloud = 1000; - -int mouse_old_x, mouse_old_y; -int mouse_buttons = 0; -float mouse_sensitivity = 1.0; - -bool is_ortho = false; -bool lock_z = false; -bool show_axes = true; -ImVec4 bg_color = ImVec4(0.65f, 0.65f, 0.65f, 1.00f); -int point_size = 1; - -bool info_gui = false; -bool compass_ruler = true; - -Eigen::Affine3f viewLocal; - -Eigen::Vector3f rotation_center = Eigen::Vector3f::Zero(); -float rotate_x = -35.264f, rotate_y = 135.0f; -float translate_x, translate_y = 0.0; -float translate_z = -50.0; - -double camera_ortho_xy_view_zoom = 10; -double camera_ortho_xy_view_shift_x = 0.0; -double camera_ortho_xy_view_shift_y = 0.0; -double camera_mode_ortho_z_center_h = 0.0; - -// Target camera state for smooth transitions -Eigen::Vector3f new_rotation_center = rotation_center; -float new_rotate_x = rotate_x; -float new_rotate_y = rotate_y; -float new_translate_x = translate_x; -float new_translate_y = translate_y; -float new_translate_z = translate_z; - -bool cor_gui = false; - -// Transition timing -bool camera_transition_active = false; - -bool scroll_hint_enabled = true; -bool scroll_hint_active = false; -int scroll_hint_count = 0; -float scroll_hint_accu = 0.0f; -double scroll_hint_lastT = 0.0; - -bool show_about = false; - -// Unlike the original (which probed GL_LINE_WIDTH_RANGE), rlgl's line -// width support is uniform enough here not to need a runtime check. -bool glLineWidthSupport = true; +// Camera/picking/mini-compass/misc-ImGui-widget API this app used to get +// from (see rl_utils.h's top comment for why it's now a +// local header instead). +#include "rl_utils.h" -float m_ortho_projection[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; -float m_ortho_gizmo_view[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; - -struct ShortcutEntry -{ - std::string type; - std::string shortcut; - std::string description; -}; +/////////////////////////////////////////////////////////////////////////////////// // GPU (rlgl-based) point cloud renderer -- replaces core's legacy-GL // PointCloud::render()/PointClouds::render() (see Core/raylib_render.hpp). @@ -198,34 +96,10 @@ ScanRenderer scan_renderer; // each 3D point landed on screen). Matrix frame_mvp_3d{}; -// Forward declarations for the functions defined near display() below -// (formerly declared in , plus a few new ones for this -// port) -- panel functions earlier in this file call several of these. -std::string truncPath(const std::string& fullPath); -void wheel(int button, int dir, int x, int y); -void reshape(int w, int h); -void motion(int x, int y); -void ShowMainDockSpace(); -void showAxes(); -void updateCameraTransition(); -void breakCameraTransition(); -void setCameraPreset(CameraPreset preset); -void camMenu(); -void view_kbd_shortcuts(); -void cor_window(); -void ImGuiHyperlink(const char* url, ImVec4 color = ImVec4(0.2f, 0.4f, 0.8f, 1.0f)); -void ShowShortcutsTable(const std::vector appShortcuts); -void info_window(const std::vector& infoLines, const std::vector& appShortcuts); -void drawMiniCompassWithRuler(); -float distanceToPlane(const RegistrationPlaneFeature::Plane& plane, const Eigen::Vector3d& p); -Eigen::Vector3d rayIntersection(const LaserBeam& laser_beam, const RegistrationPlaneFeature::Plane& plane); -LaserBeam GetLaserBeam(int x, int y); -double distance_point_to_line(const Eigen::Vector3d& point, const LaserBeam& line); -void getClosestTrajectoryPoint(Session& session_, int x, int y, bool gcpPicking, int& picked_index); -void setNewRotationCenter(int x, int y); -bool checkClHelp(int argc, char** argv); -void updateOrthoView(); -void end3DMatrixStack(); +// Forward declarations for this file's own functions defined near +// display() below (everything else that used to be here is now declared +// by rl_utils.h, included above) -- panel functions earlier in this file +// call some of these. void observationPickingRender(const ObservationPicking& observation_picking); void renderLoopClosure(PointClouds& point_clouds_container, int index_loop_closure_source, int index_loop_closure_target, int before, int after); void renderLoopClosureLabels(PointClouds& point_clouds_container); @@ -406,7 +280,7 @@ int index_loop_closure_target = 0; int index_begin = 0; int index_end = 0; -ColorScheme csPointCloud = CS_SOLID; +ColorScheme csPointCloud = CS_GRAD_INTENS; ColorScheme csTrajectory = CS_SOLID; @@ -2419,1173 +2293,13 @@ void settings_gui() ImGui::End(); } -// ============================================================================ -// Formerly core/src/utils.cpp -- local now (see the big comment at the top -// of this file for why). Everywhere the original was pure ImGui/Eigen/GLM -// (no gl*/glu*/glut* calls), it's copied verbatim. Everywhere it touched -// legacy GL, it's reimplemented with rlgl's rl*() legacy-emulation API -// (a software matrix stack + immediate-mode layer that mirrors gl*()'s -// call shape but works under a core-profile context), or with raylib/ -// raymath equivalents (gluUnProject -> Vector3Unproject, glutBitmapCharacter -// -> DrawText). Function names/signatures/globals are unchanged so every -// call site elsewhere in this file (display(), mouse(), the panel -// functions, ...) needed no changes. -// ============================================================================ - -std::string truncPath(const std::string& fullPath) -{ - namespace fspath = std::filesystem; - fspath::path path(fullPath); - - auto parent1 = path.parent_path().filename().string(); - auto parent2 = path.parent_path().parent_path().filename().string(); // second to last folder - auto filename = path.filename().string(); - - return "..\\" + parent2 + "\\" + parent1 + "\\" + filename; -} - -void wheel(int button, int dir, int x, int y) -{ - ImGuiIO& io = ImGui::GetIO(); - io.MouseWheel += dir; // or direction * 1.0f depending on your setup - - if (!ImGui::IsWindowHovered(ImGuiHoveredFlags_AnyWindow)) - { - if (dir > 0) - { - if (is_ortho) - { - camera_ortho_xy_view_zoom -= 0.1f * camera_ortho_xy_view_zoom; - - if (camera_ortho_xy_view_zoom < 0.1) - { - camera_ortho_xy_view_zoom = 0.1; - } - } - else - { - if (io.KeyShift) - translate_z += 5.0f; - else - translate_z += 1.0f; - } - } - else - { - if (is_ortho) - camera_ortho_xy_view_zoom += 0.1 * camera_ortho_xy_view_zoom; - else - { - if (io.KeyShift) - translate_z -= 5.0f; - else - translate_z -= 1.0f; - } - } - - mouse_sensitivity = fabs(translate_z) / 100; // 1 for translate_z 50 (default zoom) - camera_transition_active = false; - - if (scroll_hint_enabled) - { - if (!scroll_hint_active) - { - scroll_hint_accu += fabs(dir); - - if (scroll_hint_accu > 30.0f) // tweak threshold - { - scroll_hint_accu = 0.0f; - scroll_hint_active = true; - scroll_hint_count++; - } - } - - if (scroll_hint_active) - scroll_hint_lastT = ImGui::GetTime(); - - // Reset and disable hint if Shift is pressed while scrolling - if (io.KeyShift || scroll_hint_count > 3) - { - scroll_hint_active = false; - scroll_hint_enabled = false; - } - } - } -} - -// Was glMatrixMode/glLoadIdentity/gluPerspective/glOrtho -- rewritten with -// rlgl's software matrix-stack API (RL_PROJECTION/RL_MODELVIEW), which -// works under raylib's core-profile context. gluPerspective(fovy, aspect, -// near, far) has no rl* equivalent, so it's expanded to the equivalent -// rlFrustum() call by hand (standard fovy -> frustum-bounds formula). -void reshape(int w, int h) -{ - rlViewport(0, 0, (int)w, (int)h); - rlMatrixMode(RL_PROJECTION); - rlLoadIdentity(); - if (!is_ortho) - { - const double fovy = 60.0; - const double aspect = (double)w / (double)h; - const double nearP = 0.01, farP = 10000.0; - const double top = nearP * tan(fovy * 0.5 * M_PI / 180.0); - const double bottom = -top; - const double right = top * aspect; - const double left = -right; - rlFrustum(left, right, bottom, top, nearP, farP); - } - else - { - ImGuiIO& io = ImGui::GetIO(); - float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); - - rlOrtho( - -camera_ortho_xy_view_zoom, - camera_ortho_xy_view_zoom, - -camera_ortho_xy_view_zoom / ratio, - camera_ortho_xy_view_zoom / ratio, - -100000, - 100000); - } - rlMatrixMode(RL_MODELVIEW); - rlLoadIdentity(); -} - -// GL-free -- copied verbatim, minus the trailing glutPostRedisplay() (a -// no-op here: this app's main loop already redraws every frame). -void motion(int x, int y) -{ - ImGuiIO& io = ImGui::GetIO(); - io.MousePos = ImVec2((float)x, (float)y); - - if (!io.WantCaptureMouse) - { - float dx, dy; - dx = (float)(x - mouse_old_x); - dy = (float)(y - mouse_old_y); - - if (mouse_buttons & 1) // left button - { - rotate_x += dy * 0.2f; - rotate_y += dx * 0.2f; - breakCameraTransition(); - } - - if (mouse_buttons & 4) // right button - { - if (is_ortho) - { - float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); - Eigen::Vector3d v( - dx * (camera_ortho_xy_view_zoom / (float)io.DisplaySize.x * 2), - dy * (camera_ortho_xy_view_zoom / (float)io.DisplaySize.y * 2 / ratio), - 0); - TaitBryanPose pose_tb; - pose_tb.px = 0.0; - pose_tb.py = 0.0; - pose_tb.pz = 0.0; - pose_tb.om = 0.0; - pose_tb.fi = 0.0; - pose_tb.ka = (rotate_x + rotate_y) * M_PI / 180.0; - auto m = affine_matrix_from_pose_tait_bryan(pose_tb); - Eigen::Vector3d v_t = m * v; - camera_ortho_xy_view_shift_x += v_t.x(); - camera_ortho_xy_view_shift_y += v_t.y(); - } - else - { - translate_x += dx * 0.1f * mouse_sensitivity; - translate_y -= dy * 0.1f * mouse_sensitivity; - breakCameraTransition(); - } - } - - mouse_old_x = x; - mouse_old_y = y; - } -} - -// GL-free -- copied verbatim. -static bool first_time = true; - -void ShowMainDockSpace() -{ - ImGuiWindowFlags window_flags = ImGuiWindowFlags_NoDocking | ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_NoCollapse | - ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoBringToFrontOnFocus | ImGuiWindowFlags_NoNavFocus | - ImGuiWindowFlags_NoBackground | ImGuiWindowFlags_NoInputs; - - ImGuiViewport* viewport = ImGui::GetMainViewport(); - ImGui::SetNextWindowPos(viewport->WorkPos); - ImGui::SetNextWindowSize(viewport->WorkSize); - ImGui::SetNextWindowViewport(viewport->ID); - - ImGui::PushStyleVar(ImGuiStyleVar_WindowRounding, 0.0f); - ImGui::PushStyleVar(ImGuiStyleVar_WindowBorderSize, 0.0f); - - ImGui::Begin("MainDockSpace", nullptr, window_flags); - - ImGui::PopStyleVar(2); - - // This is the dockspace! - ImGuiID dockspace_id = ImGui::GetID("MyDockSpace"); - ImGui::DockSpace(dockspace_id, ImVec2(0, 0), ImGuiDockNodeFlags_PassthruCentralNode | ImGuiDockNodeFlags_NoDockingInCentralNode); - - if (first_time) - { - first_time = false; - - auto dock_id_left = ImGui::DockBuilderSplitNode(dockspace_id, ImGuiDir_Left, 0.2f, nullptr, &dockspace_id); - auto dock_id_bottom = ImGui::DockBuilderSplitNode(dockspace_id, ImGuiDir_Down, 0.2f, nullptr, &dockspace_id); - - ImGui::DockBuilderDockWindow("Console", dock_id_bottom); - ImGui::DockBuilderFinish(dockspace_id); - } - - ImGui::End(); -} - -// Was glBegin(GL_LINES)/glColor3f/glVertex3f/glEnd -- rl* rename. -void showAxes() -{ - if (show_axes || ImGui::GetIO().KeyCtrl) // rotation center axes - { - rlBegin(RL_LINES); - rlColor3f(1.f, 1.f, 1.f); - rlVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z()); - rlVertex3f(rotation_center.x() + 1.f, rotation_center.y(), rotation_center.z()); - rlVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z()); - rlVertex3f(rotation_center.x() - 1.f, rotation_center.y(), rotation_center.z()); - rlVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z()); - rlVertex3f(rotation_center.x(), rotation_center.y() - 1.f, rotation_center.z()); - rlVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z()); - rlVertex3f(rotation_center.x(), rotation_center.y() + 1.f, rotation_center.z()); - rlVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z()); - rlVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z() - 1.f); - rlVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z()); - rlVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z() + 1.f); - rlEnd(); - } - - if (show_axes || ImGui::GetIO().KeyCtrl) // origin axes - { - rlBegin(RL_LINES); - rlColor3f(1.0f, 0.0f, 0.0f); - rlVertex3f(0.0f, 0.0f, 0.0f); - rlVertex3f(100, 0.0f, 0.0f); - - rlColor3f(0.0f, 1.0f, 0.0f); - rlVertex3f(0.0f, 0.0f, 0.0f); - rlVertex3f(0.0f, 100, 0.0f); - - rlColor3f(0.0f, 0.0f, 1.0f); - rlVertex3f(0.0f, 0.0f, 0.0f); - rlVertex3f(0.0f, 0.0f, 100); - rlEnd(); - } -} - -// GL-free -- copied verbatim. -void updateCameraTransition() -{ - if (!camera_transition_active) - return; - - float t = 1.0f - powf(1.0f - std::min(ImGui::GetIO().DeltaTime * camera_transition_speed, 1.0f), 3.0f); - - bool doneXrc = fabs(new_rotation_center.x() - rotation_center.x()) < 0.01f; - bool doneYrc = fabs(new_rotation_center.y() - rotation_center.y()) < 0.01f; - bool doneZrc = fabs(new_rotation_center.z() - rotation_center.z()) < 0.01f; - bool doneXr = fabs(new_rotate_x - rotate_x) < 0.01f; - bool doneYr = fabs(new_rotate_y - rotate_y) < 0.01f; - bool doneXt = fabs(new_translate_x - translate_x) < 0.01f; - bool doneYt = fabs(new_translate_y - translate_y) < 0.01f; - bool doneZt = fabs(new_translate_z - translate_z) < 0.01f; - - if (!doneXrc) - rotation_center.x() += (new_rotation_center.x() - rotation_center.x()) * t; - if (!doneYrc) - rotation_center.y() += (new_rotation_center.y() - rotation_center.y()) * t; - if (!doneZrc) - rotation_center.z() += (new_rotation_center.z() - rotation_center.z()) * t; - if (!doneXr) - rotate_x += (new_rotate_x - rotate_x) * t; - if (!doneYr) - rotate_y += (new_rotate_y - rotate_y) * t; - if (!doneXt) - translate_x += (new_translate_x - translate_x) * t; - if (!doneYt) - translate_y += (new_translate_y - translate_y) * t; - if (!doneZt) - translate_z += (new_translate_z - translate_z) * t; - - camera_transition_active = !(doneXrc && doneYrc && doneZrc && doneXr && doneYr && doneXt && doneYt && doneZt); - - if (!camera_transition_active) - { - rotation_center = new_rotation_center; - rotate_x = new_rotate_x; - rotate_y = new_rotate_y; - translate_x = new_translate_x; - translate_y = new_translate_y; - translate_z = new_translate_z; - } -} - -// GL-free -- copied verbatim. -void breakCameraTransition() -{ - if (camera_transition_active == false) - return; - rotation_center = new_rotation_center; - camera_transition_active = false; -} - -// GL-free -- copied verbatim. -void setCameraPreset(CameraPreset preset) -{ - bool triggered = false; - - switch (preset) - { - case CAMERA_FRONT: - new_rotate_x = -90.0f; - new_rotate_y = +90.0f; - triggered = true; - break; - case CAMERA_BACK: - new_rotate_x = -90.0f; - new_rotate_y = -90.0f; - triggered = true; - break; - case CAMERA_LEFT: - new_rotate_x = -90.0f; - new_rotate_y = 180.0f; - triggered = true; - break; - case CAMERA_RIGHT: - new_rotate_x = -90.0f; - new_rotate_y = 0.0f; - triggered = true; - break; - case CAMERA_TOP: - new_rotate_x = 0.0f; - new_rotate_y = 90.0f; - triggered = true; - break; - case CAMERA_BOTTOM: - new_rotate_x = 180.0f; - new_rotate_y = -90.0f; - triggered = true; - break; - case CAMERA_ISO: - new_rotate_x = -35.264f; - new_rotate_y = 135.0f; - triggered = true; - break; - case CAMERA_RESET: - new_rotation_center = Eigen::Vector3f::Zero(); - new_rotate_x = 0; - new_rotate_y = 0; - new_translate_x = 0; - new_translate_y = 0; - new_translate_z = -50.0f; - mouse_sensitivity = fabs(translate_z) / 100; - - camera_ortho_xy_view_zoom = 10; - camera_ortho_xy_view_shift_x = 0.0; - camera_ortho_xy_view_shift_y = 0.0; - camera_mode_ortho_z_center_h = 0.0; - - viewer_decimate_point_cloud = 1000; - triggered = false; - break; - } - - if (triggered) - { - new_rotation_center = rotation_center; - new_translate_x = translate_x; - new_translate_y = translate_y; - new_translate_z = translate_z; - } - - camera_transition_active = true; -} - -// GL-free -- copied verbatim. -void camMenu() -{ - if (ImGui::BeginMenu("Camera")) - { - if (ImGui::MenuItem("Front (yz view)", "key F")) - setCameraPreset(CAMERA_FRONT); - if (ImGui::MenuItem("Back", "key B")) - setCameraPreset(CAMERA_BACK); - if (ImGui::MenuItem("Left (xz view)", "key L")) - setCameraPreset(CAMERA_LEFT); - if (ImGui::MenuItem("Right", "key R")) - setCameraPreset(CAMERA_RIGHT); - if (ImGui::MenuItem("Top (xy view)", "key T")) - setCameraPreset(CAMERA_TOP); - if (ImGui::MenuItem("Bottom", "key U")) - setCameraPreset(CAMERA_BOTTOM); - if (ImGui::MenuItem("Isometric", "key I")) - setCameraPreset(CAMERA_ISO); - ImGui::Separator(); - if (ImGui::MenuItem("Reset", "key Z")) - setCameraPreset(CAMERA_RESET); - - ImGui::EndMenu(); - } - if (ImGui::IsItemHovered()) - { - ImGui::BeginTooltip(); - ImGui::Text("Change camera view to fixed positions"); - ImGui::Separator(); - ImGui::Text("Metrics:"); - if (ImGui::BeginTable("Metrics", 4)) - { - ImGui::TableSetupColumn("Coord"); - ImGui::TableSetupColumn("rotate"); - ImGui::TableSetupColumn("translate"); - ImGui::TableSetupColumn("rot center"); - ImGui::TableHeadersRow(); - - ImGui::TableNextRow(); - ImGui::TableSetColumnIndex(0); - - std::string text = "X"; - float centered = ImGui::GetColumnWidth() - ImGui::CalcTextSize(text.c_str()).x; - ImGui::SetCursorPosX(ImGui::GetCursorPosX() + centered * 0.5f); - ImGui::Text("X"); - - ImGui::TableSetColumnIndex(1); - ImGui::Text("%.3f", rotate_x); - ImGui::TableSetColumnIndex(2); - ImGui::Text("%.3f", translate_x); - ImGui::TableSetColumnIndex(3); - ImGui::Text("%.3f", rotation_center.x()); - - ImGui::TableNextRow(); - ImGui::TableSetColumnIndex(0); - ImGui::SetCursorPosX(ImGui::GetCursorPosX() + centered * 0.5f); - ImGui::Text("Y"); - - ImGui::TableSetColumnIndex(1); - ImGui::Text("%.3f", rotate_y); - ImGui::TableSetColumnIndex(2); - ImGui::Text("%.3f", translate_y); - ImGui::TableSetColumnIndex(3); - ImGui::Text("%.3f", rotation_center.y()); - - ImGui::TableNextRow(); - ImGui::TableSetColumnIndex(0); - ImGui::SetCursorPosX(ImGui::GetCursorPosX() + centered * 0.5f); - ImGui::Text("Z"); - - ImGui::TableSetColumnIndex(2); - ImGui::Text("%.3f", translate_z); - ImGui::TableSetColumnIndex(3); - ImGui::Text("%.3f", rotation_center.y()); - - ImGui::EndTable(); - } - ImGui::Text("Mouse sensitivity: %.4f", mouse_sensitivity); - - ImGui::EndTooltip(); - } - - if (scroll_hint_active) - { - ImVec2 mousePos = ImGui::GetMousePos(); - ImGui::SetNextWindowPos(ImVec2(mousePos.x + 20, mousePos.y - 40)); - ImGui::SetNextWindowBgAlpha(0.7f); - ImGui::BeginTooltip(); - ImGui::Text("Tip: To accelerate hold Shift + scroll"); - ImGui::EndTooltip(); - - if (ImGui::GetTime() - scroll_hint_lastT > 1) - scroll_hint_active = false; - } -} - -// GL-free -- copied verbatim. -void view_kbd_shortcuts() -{ - ImGuiIO& io = ImGui::GetIO(); - - if (io.WantCaptureKeyboard) - return; - - if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_RightArrow, true)) - { - translate_x += 0.5f * mouse_sensitivity; - breakCameraTransition(); - } - if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_LeftArrow, true)) - { - translate_x -= 0.5f * mouse_sensitivity; - breakCameraTransition(); - } - - if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_UpArrow, true)) - { - translate_y += 0.5f * mouse_sensitivity; - breakCameraTransition(); - } - if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_DownArrow, true)) - { - translate_y -= 0.5f * mouse_sensitivity; - breakCameraTransition(); - } - - if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_RightArrow, true)) - { - rotate_y -= 0.6; - breakCameraTransition(); - } - if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_LeftArrow, true)) - { - rotate_y += 0.6; - breakCameraTransition(); - } - - if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_UpArrow, true)) - { - rotate_x -= 0.6; - breakCameraTransition(); - } - if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_DownArrow, true)) - { - rotate_x += 0.6; - breakCameraTransition(); - } - - if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_R, false)) - cor_gui = true; - - if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_Z, false) && !is_ortho) - lock_z = !lock_z; - - if (io.KeyCtrl || io.KeyAlt || io.KeyShift) - return; - - if (ImGui::IsKeyPressed(ImGuiKey_B)) - setCameraPreset(CAMERA_BACK); - if (ImGui::IsKeyPressed(ImGuiKey_F)) - setCameraPreset(CAMERA_FRONT); - if (ImGui::IsKeyPressed(ImGuiKey_I)) - setCameraPreset(CAMERA_ISO); - if (ImGui::IsKeyPressed(ImGuiKey_L)) - setCameraPreset(CAMERA_LEFT); - if (ImGui::IsKeyPressed(ImGuiKey_R)) - setCameraPreset(CAMERA_RIGHT); - if (ImGui::IsKeyPressed(ImGuiKey_T)) - setCameraPreset(CAMERA_TOP); - if (ImGui::IsKeyPressed(ImGuiKey_U)) - setCameraPreset(CAMERA_BOTTOM); - if (ImGui::IsKeyPressed(ImGuiKey_Z)) - setCameraPreset(CAMERA_RESET); - - if (ImGui::IsKeyPressed(ImGuiKey_C, false)) - compass_ruler = !compass_ruler; - if (ImGui::IsKeyPressed(ImGuiKey_O, false)) - is_ortho = !is_ortho; - if (ImGui::IsKeyPressed(ImGuiKey_X, false)) - show_axes = !show_axes; - - if (ImGui::IsKeyPressed(ImGuiKey_1)) - point_size = 1; - if (ImGui::IsKeyPressed(ImGuiKey_2)) - point_size = 2; - if (ImGui::IsKeyPressed(ImGuiKey_3)) - point_size = 3; - if (ImGui::IsKeyPressed(ImGuiKey_4)) - point_size = 4; - if (ImGui::IsKeyPressed(ImGuiKey_5)) - point_size = 5; - if (ImGui::IsKeyPressed(ImGuiKey_6)) - point_size = 6; - if (ImGui::IsKeyPressed(ImGuiKey_7)) - point_size = 7; - if (ImGui::IsKeyPressed(ImGuiKey_8)) - point_size = 8; - if (ImGui::IsKeyPressed(ImGuiKey_9)) - point_size = 9; -} - -// GL-free -- copied verbatim. -void cor_window() -{ - if (cor_gui) - { - ImGui::OpenPopup("Center of rotation"); - cor_gui = false; - } - - if (ImGui::BeginPopupModal("Center of rotation", NULL, ImGuiWindowFlags_AlwaysAutoResize)) - { - ImGui::Text("Select new center of rotation [m]:"); - ImGui::PushItemWidth(ImGuiNumberWidth); - ImGui::InputFloat("X", &new_rotation_center.x(), 0.0, 0.0, "%.3f"); - if (ImGui::IsItemHovered()) - ImGui::SetTooltip(xText); - ImGui::SameLine(); - ImGui::InputFloat("Y", &new_rotation_center.y(), 0.0, 0.0, "%.3f"); - if (ImGui::IsItemHovered()) - ImGui::SetTooltip(yText); - ImGui::SameLine(); - ImGui::InputFloat("Z", &new_rotation_center.z(), 0.0, 0.0, "%.3f"); - if (ImGui::IsItemHovered()) - ImGui::SetTooltip(zText); - ImGui::PopItemWidth(); - - ImGui::Separator(); - - if (ImGui::Button("Set")) - { - new_rotate_x = rotate_x; - new_rotate_y = rotate_y; - new_translate_x = -new_rotation_center.x(); - new_translate_y = -new_rotation_center.y(); - new_translate_z = translate_z; - - camera_transition_active = true; - - ImGui::CloseCurrentPopup(); - } - - ImGui::SameLine(); - if (ImGui::Button("Cancel")) - { - ImGui::CloseCurrentPopup(); - } - - ImGui::EndPopup(); - } -} - -// GL-free -- copied verbatim. -void ImGuiHyperlink(const char* url, ImVec4 color) -{ - ImGui::PushStyleColor(ImGuiCol_Text, color); - ImGui::TextUnformatted(url); - ImGui::PopStyleColor(); - - ImVec2 pos = ImGui::GetItemRectMin(); - ImVec2 size = ImGui::GetItemRectSize(); - - if (ImGui::IsItemHovered()) - ImGui::SetMouseCursor(ImGuiMouseCursor_Hand); - - if (ImGui::IsItemHovered()) - { - ImDrawList* draw_list = ImGui::GetWindowDrawList(); - draw_list->AddLine(ImVec2(pos.x, pos.y + size.y), ImVec2(pos.x + size.x, pos.y + size.y), ImColor(color)); - } - - if (ImGui::IsItemClicked()) - { -#ifdef _WIN32 - ShellExecuteA(0, "open", url, 0, 0, SW_SHOWNORMAL); -#elif __APPLE__ - std::string cmd = std::string("open ") + url; - system(cmd.c_str()); -#else - std::string cmd = std::string("xdg-open ") + url; - system(cmd.c_str()); -#endif - } -} - -// General shortcuts applicable to any app -- GL-free, copied verbatim. -static const std::vector shortcuts = { { "Normal keys", "A", "" }, - { "", "Ctrl+A", "" }, - { "", "B", "camera Back" }, - { "", "Ctrl+B", "" }, - { "", "C", "Compass/ruler" }, - { "", "Ctrl+C", "" }, - { "", "D", "" }, - { "", "Ctrl+D", "" }, - { "", "E", "" }, - { "", "Ctrl+E", "" }, - { "", "F", "camera Front" }, - { "", "Ctrl+F", "" }, - { "", "G", "" }, - { "", "Ctrl+G", "" }, - { "", "H", "" }, - { "", "Ctrl+H", "" }, - { "", "I", "camera Isometric" }, - { "", "Ctrl+I", "" }, - { "", "J", "" }, - { "", "Ctrl+J", "" }, - { "", "K", "" }, - { "", "Ctrl+K", "" }, - { "", "L", "camera Left" }, - { "", "Ctrl+L", "" }, - { "", "M", "" }, - { "", "Ctrl+M", "" }, - { "", "N", "" }, - { "", "Ctrl+N", "" }, - { "", "O", "Ortographic view" }, - { "", "Ctrl+O", "Open/load session/data" }, - { "", "P", "" }, - { "", "Ctrl+P", "" }, - { "", "Q", "" }, - { "", "Ctrl+Q", "" }, - { "", "R", "camera Right" }, - { "", "Ctrl+R", "" }, - { "", "Shift+R", "Rotation center" }, - { "", "S", "" }, - { "", "Ctrl+S", "" }, - { "", "Ctrl+Shift+S", "" }, - { "", "T", "camera Top" }, - { "", "Ctrl+T", "" }, - { "", "U", "camera bottom (Under)" }, - { "", "Ctrl+U", "" }, - { "", "V", "" }, - { "", "Ctrl+V", "" }, - { "", "W", "" }, - { "", "Ctrl+W", "" }, - { "", "X", "show aXes" }, - { "", "Ctrl+X", "" }, - { "", "Y", "" }, - { "", "Ctrl+Y", "" }, - { "", "Z", "camera reset" }, - { "", "Ctrl+Z", "" }, - { "", "Shift+Z", "Lock Z" }, - { "", "1-9", "point size" }, - { "Special keys", "Up arrow", "" }, - { "", "Shift + up arrow", "camera translate Up" }, - { "", "Ctrl + up arrow", "" }, - { "", "Down arrow", "" }, - { "", "Shift + down arrow", "camera translate Down" }, - { "", "Ctrl + down arrow", "" }, - { "", "Left arrow", "" }, - { "", "Shift + left arrow", "camera translate Left" }, - { "", "Ctrl + left arrow", "" }, - { "", "Right arrow", "" }, - { "", "Shift + right arrow", "camera translate Right" }, - { "", "Ctrl + right arrow", "" }, - { "", "Pg down", "" }, - { "", "Pg up", "" }, - { "", "- key", "" }, - { "", "+ key", "" }, - { "Mouse related", "Left click + drag", "camera rotate" }, - { "", "Right click + drag", "camera pan" }, - { "", "Scroll", "camera zoom" }, - { "", "Shift + scroll", "camera 5x zoom" }, - { "", "Shift + drag", "Dock window to screen edges" }, - { "", "Ctrl + left click", "" }, - { "", "Ctrl + right click", "change center of rotation" }, - { "", "Ctrl + middle click", "change center of rotation (if no CP GUI active)" } }; - -// GL-free -- copied verbatim. -void ShowShortcutsTable(const std::vector appShortcuts) -{ - if (ImGui::BeginTable( - "ShortcutsTable", 2, ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg | ImGuiTableFlags_ScrollY, ImVec2(-FLT_MIN, 200))) - { - ImGui::TableSetupScrollFreeze(0, 1); - ImGui::TableSetupColumn("Shortcut", ImGuiTableColumnFlags_WidthFixed, 120); - ImGui::TableSetupColumn("Description"); - ImGui::TableHeadersRow(); - - std::string lastType; - - for (size_t i = 0; i < shortcuts.size(); ++i) - { - const auto& s = shortcuts[i]; - - if (!s.type.empty() && s.type != lastType) - { - lastType = s.type; - ImGui::TableNextRow(); - - ImGui::TableSetBgColor(ImGuiTableBgTarget_RowBg0, IM_COL32(70, 70, 140, 255)); - - ImGui::TableSetColumnIndex(0); - ImGui::TextColored(ImVec4(0.8f, 0.8f, 1.0f, 1.0f), "%s", lastType.c_str()); - ImGui::TableSetColumnIndex(1); - ImGui::TextUnformatted(""); - } - - auto description = s.description; - - if (description.empty()) - description = appShortcuts[i].description; - - if (!description.empty()) - { - ImGui::TableNextRow(); - ImGui::TableSetColumnIndex(0); - ImGui::TextUnformatted(s.shortcut.c_str()); - ImGui::TableSetColumnIndex(1); - ImGui::TextUnformatted(description.c_str()); - } - } - - ImGui::EndTable(); - } -} - -// GL-free -- copied verbatim (glGetString(GL_RENDERER/...) is a plain -// string query, still valid under a core-profile context). -void info_window(const std::vector& infoLines, const std::vector& appShortcuts) -{ - if (!info_gui) - return; - - if (ImGui::Begin( - "Info", - &info_gui, - ImGuiWindowFlags_NoResize | ImGuiWindowFlags_AlwaysAutoResize | ImGuiWindowFlags_NoDocking | ImGuiWindowFlags_NoCollapse)) - { - bool firstLine = true; - for (const auto& line : infoLines) - { - if (line.empty()) - ImGui::NewLine(); - else if (line.rfind("https://", 0) == 0) - ImGuiHyperlink(line.c_str()); - else - ImGui::Text(line.c_str()); - - if (firstLine) - { - ImGui::SameLine( - ImGui::GetWindowWidth() - ImGui::CalcTextSize("ImGui").x - ImGui::GetStyle().ItemSpacing.x * 2 - - ImGui::GetStyle().FramePadding.x * 2); - if (ImGui::Button("ImGui")) - show_about = true; - if (ImGui::IsItemHovered()) - { - ImGui::BeginTooltip(); - const GLubyte* renderer = glGetString(GL_RENDERER); - const GLubyte* version = glGetString(GL_VERSION); - const GLubyte* glslVersion = glGetString(GL_SHADING_LANGUAGE_VERSION); - - ImGui::Text("Renderer: %s", renderer); - ImGui::Text("OpenGL version supported: %s", version); - ImGui::Text("GLSL version: %s", glslVersion); - ImGui::EndTooltip(); - } - - firstLine = false; - } - } - - ImGui::NewLine(); - ImGui::Text("Author: Janusz Bedkowski & contributors"); - ImGui::NewLine(); - ImGui::Text("Part of HDMapping software suite"); - ImGui::Text("Version: %s (%s)", HDMAPPING_VERSION_STRING, __DATE__); - ImGui::Text("Project page: "); - ImGui::SameLine(); - ImGuiHyperlink("https://github.com/MapsHD/HDMapping"); - - ImGui::NewLine(); - ImGui::Separator(); - ImGui::NewLine(); - - ShowShortcutsTable(appShortcuts); - - if (show_about) - ImGui::ShowAboutWindow(&show_about); - } - - ImGui::End(); -} - -// Was a dedicated 200x200 GL sub-viewport with its own glOrtho projection, -// rotation-only modelview (viewLocal's rotation), and GLUT bitmap-font text -// (glRasterPos + glutBitmapCharacter). Reimplemented as a pure 2D -// screen-space overlay instead: project each world axis direction through -// viewLocal's rotation to get eye-space X/Y (screen right/up), and draw -// with raylib's DrawLineEx/DrawText -- same bottom-left placement, same -// "nice number" ruler tied to zoom (translate_z), no sub-viewport or GLUT -// font needed. -void drawMiniCompassWithRuler() -{ - const float compassSize = 200.0f; - const float originX = compassSize * 0.5f; - const float originY = static_cast(GetScreenHeight()) - compassSize * 0.5f; - const float axisPixelLength = compassSize * 0.35f; - - struct Axis - { - Eigen::Vector3f dir; - const char* label; - Color color; - }; - const Axis axes[3] = { - {Eigen::Vector3f::UnitX(), "X (long.)", RED}, - {Eigen::Vector3f::UnitY(), "Y (lat.)", GREEN}, - {Eigen::Vector3f::UnitZ(), "Z (vert.)", BLUE}, - }; - - for (const auto& axis : axes) - { - Eigen::Vector3f eyeDir = viewLocal.rotation() * axis.dir; - Vector2 tip = {originX + eyeDir.x() * axisPixelLength, originY - eyeDir.y() * axisPixelLength}; - DrawLineEx(Vector2{originX, originY}, tip, 2.f, axis.color); - DrawText(axis.label, (int)tip.x + 4, (int)tip.y - 6, 12, axis.color); - } - - // Ruler: "nice" (1/2/5 x 10^n) length, mirroring the original's - // 0.1 * fabs(translate_z) heuristic (translate_z is this app's - // zoom/dolly distance). - float rawUnit = std::max(0.001f, 0.1f * fabsf(translate_z)); - float base = powf(10.0f, floorf(log10f(rawUnit))); - float normalized = rawUnit / base; - float niceUnit = normalized < 2.0f ? 1.0f : (normalized < 5.0f ? 2.0f : 5.0f); - float worldLength = niceUnit * base; - - char label[32]; - if (worldLength >= 1000.0f) - snprintf(label, sizeof(label), "%.0f [km]", worldLength / 1000.0f); - else if (worldLength >= 1.0f) - snprintf(label, sizeof(label), "%.0f [m]", worldLength); - else if (worldLength >= 0.01f) - snprintf(label, sizeof(label), "%.0f [cm]", worldLength * 100.0f); - else - snprintf(label, sizeof(label), "<1 [cm]"); - - float rulerY = originY + compassSize * 0.45f; - Color rulerColor = ColorFromNormalized(Vector4{1.0f - bg_color.x, 1.0f - bg_color.y, 1.0f - bg_color.z, 1.0f}); - DrawLineEx(Vector2{originX - 40.f, rulerY}, Vector2{originX + 40.f, rulerY}, 2.f, rulerColor); - DrawLineEx(Vector2{originX - 40.f, rulerY - 5.f}, Vector2{originX - 40.f, rulerY + 5.f}, 2.f, rulerColor); - DrawLineEx(Vector2{originX + 40.f, rulerY - 5.f}, Vector2{originX + 40.f, rulerY + 5.f}, 2.f, rulerColor); - DrawText(label, (int)originX - 20, (int)rulerY + 6, 14, rulerColor); -} - -// GL-free -- copied verbatim. -float distanceToPlane(const RegistrationPlaneFeature::Plane& plane, const Eigen::Vector3d& p) -{ - return (plane.a * p.x() + plane.b * p.y() + plane.c * p.z() + plane.d); -} - -// GL-free -- copied verbatim. -Eigen::Vector3d rayIntersection(const LaserBeam& laser_beam, const RegistrationPlaneFeature::Plane& plane) -{ - float TOLERANCE = 0.0001; - Eigen::Vector3d out_point; - out_point.x() = laser_beam.position.x(); - out_point.y() = laser_beam.position.y(); - out_point.z() = laser_beam.position.z(); - - float a = plane.a * laser_beam.direction.x() + plane.b * laser_beam.direction.y() + plane.c * laser_beam.direction.z(); - - if (a > -TOLERANCE && a < TOLERANCE) - { - return out_point; - } - - float distance = distanceToPlane(plane, out_point); - - out_point.x() = laser_beam.position.x() - laser_beam.direction.x() * (distance / a); - out_point.y() = laser_beam.position.y() - laser_beam.direction.y() * (distance / a); - out_point.z() = laser_beam.position.z() - laser_beam.direction.z() * (distance / a); - - return out_point; -} - -// Was gluUnProject(winX, winY, winZ, modelview, projection, viewport, ...) -// against glGetDoublev(GL_MODELVIEW/PROJECTION_MATRIX) -- rewritten with -// raymath's Vector3Unproject against rlgl's current matrix stack -// (rlGetMatrixModelview/Projection), following the same NDC-space -// conversion raylib's own GetScreenToWorldRayEx uses. The original's -// far point used winZ=-1000 (an out-of-range hack to get a point far along -// the ray, since gluUnProject doesn't clamp); using the actual far-plane -// NDC z=1 here is equally valid for the same purpose (only direction, not -// magnitude, of laser_beam.direction matters to callers). -LaserBeam GetLaserBeam(int x, int y) -{ - int width = GetScreenWidth(); - int height = GetScreenHeight(); - - float ndcX = (2.0f * (float)x) / (float)width - 1.0f; - float ndcY = 1.0f - (2.0f * (float)y) / (float)height; - - Matrix matView = rlGetMatrixModelview(); - Matrix matProj = rlGetMatrixProjection(); - - Vector3 nearPoint = Vector3Unproject(Vector3{ndcX, ndcY, 0.0f}, matProj, matView); - Vector3 farPoint = Vector3Unproject(Vector3{ndcX, ndcY, 1.0f}, matProj, matView); - - LaserBeam laser_beam; - laser_beam.position = Eigen::Vector3d(nearPoint.x, nearPoint.y, nearPoint.z); - laser_beam.direction = Eigen::Vector3d(farPoint.x - nearPoint.x, farPoint.y - nearPoint.y, farPoint.z - nearPoint.z); - return laser_beam; -} - -// GL-free -- copied verbatim. -double distance_point_to_line(const Eigen::Vector3d& point, const LaserBeam& line) -{ - Eigen::Vector3d AP = point - line.position; - return (AP.cross(line.direction)).norm(); -} - -// GL-free -- copied verbatim. -void getClosestTrajectoryPoint(Session& session_, int x, int y, bool gcpPicking, int& picked_index) -{ - picked_index = -1; - - const auto laser_beam = GetLaserBeam(x, y); - double min_distance = std::numeric_limits::max(); - int index_i = -1; - int index_j = -1; - - for (int i = 0; i < session_.point_clouds_container.point_clouds.size(); i++) - { - for (int j = 0; j < session_.point_clouds_container.point_clouds[i].local_trajectory.size(); j++) - { - const auto& p = session_.point_clouds_container.point_clouds[i].local_trajectory[j].m_pose.translation(); - Eigen::Vector3d vp = session_.point_clouds_container.point_clouds[i].m_pose * p; - - double dist = distance_point_to_line(vp, laser_beam); - - if (dist < min_distance) - { - min_distance = dist; - index_i = i; - index_j = j; - - new_rotation_center.x() = static_cast(vp.x()); - new_rotation_center.y() = static_cast(vp.y()); - new_rotation_center.z() = static_cast(vp.z()); - - if (gcpPicking) - { - session_.ground_control_points.picking_mode_index_to_node_inner = index_i; - session_.ground_control_points.picking_mode_index_to_node_outer = index_j; - } - - picked_index = index_i; - } - } - } - - new_rotate_x = rotate_x; - new_rotate_y = rotate_y; - new_translate_x = -new_rotation_center.x(); - new_translate_y = -new_rotation_center.y(); - new_translate_z = translate_z; - camera_transition_active = true; -} - -// GL-free -- copied verbatim. -void setNewRotationCenter(int x, int y) -{ - const auto laser_beam = GetLaserBeam(x, y); - - RegistrationPlaneFeature::Plane pl; - - pl.a = 0; - pl.b = 0; - pl.c = 1; - pl.d = 0; - new_rotation_center = rayIntersection(laser_beam, pl).cast(); - - std::cout << "Setting new rotation center to:\n" << new_rotation_center << std::endl; - - new_rotate_x = rotate_x; - new_rotate_y = rotate_y; - new_translate_x = -new_rotation_center.x(); - new_translate_y = -new_rotation_center.y(); - new_translate_z = translate_z; - - camera_transition_active = true; -} - -// GL-free -- copied verbatim. -bool checkClHelp(int argc, char** argv) -{ - for (int i = 1; i < argc; ++i) - { - std::string arg(argv[i]); - - if (arg == "-h" || arg == "/h" || arg == "--help" || arg == "/?") - { - return true; - } - } - return false; -} - -// Was glOrtho + gluLookAt (folded into GL_PROJECTION, matching the -// original's call order -- gluLookAt ran before the GL_MODELVIEW switch -// below) -- rewritten as rlOrtho + rlMultMatrixf with the same lookAt -// matrix already computed via GLM for m_ortho_gizmo_view just above it. -void updateOrthoView() -{ - // still updating viewLocal for compass - viewLocal.rotate(Eigen::AngleAxisf((rotate_x + rotate_y) * DEG_TO_RAD, Eigen::Vector3f::UnitZ())); - - ImGuiIO& io = ImGui::GetIO(); - float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); - - rlOrtho( - -camera_ortho_xy_view_zoom, - camera_ortho_xy_view_zoom, - -camera_ortho_xy_view_zoom / ratio, - camera_ortho_xy_view_zoom / ratio, - -100000, - 100000); - - glm::mat4 proj = glm::orthoLH_ZO( - -camera_ortho_xy_view_zoom, - camera_ortho_xy_view_zoom, - -camera_ortho_xy_view_zoom / ratio, - camera_ortho_xy_view_zoom / ratio, - -100, - 100); - - std::copy(&proj[0][0], &proj[3][3], m_ortho_projection); - - Eigen::Vector3d v_eye_t(-camera_ortho_xy_view_shift_x, camera_ortho_xy_view_shift_y, camera_mode_ortho_z_center_h + 10); - Eigen::Vector3d v_center_t(-camera_ortho_xy_view_shift_x, camera_ortho_xy_view_shift_y, camera_mode_ortho_z_center_h); - Eigen::Vector3d v(0, 1, 0); - - TaitBryanPose pose_tb; - pose_tb.px = 0.0; - pose_tb.py = 0.0; - pose_tb.pz = 0.0; - pose_tb.om = 0.0; - pose_tb.fi = 0.0; - pose_tb.ka = -(rotate_x + rotate_y) * DEG_TO_RAD; - auto m = affine_matrix_from_pose_tait_bryan(pose_tb); - - Eigen::Vector3d v_t = m * v; - - glm::mat4 lookat = glm::lookAt( - glm::vec3(v_eye_t.x(), v_eye_t.y(), v_eye_t.z()), - glm::vec3(v_center_t.x(), v_center_t.y(), v_center_t.z()), - glm::vec3(v_t.x(), v_t.y(), v_t.z())); - std::copy(&lookat[0][0], &lookat[3][3], m_ortho_gizmo_view); - - rlMultMatrixf(&lookat[0][0]); - - rlMatrixMode(RL_MODELVIEW); - rlLoadIdentity(); -} - -// Restores rlgl's default 2D screen-space projection (matches what -// raylib's own EndMode3D() does), since this app drives the rlgl matrix -// stack manually (rlMatrixMode/rlFrustum/rlMultMatrixf in reshape()/ -// display() above) instead of using raylib's BeginMode3D/EndMode3D -// wrapper. Must be called after all 3D drawing and before any 2D drawing -// (the mini-compass, ImGui) each frame. -void end3DMatrixStack() -{ - rlDrawRenderBatchActive(); - rlMatrixMode(RL_PROJECTION); - rlLoadIdentity(); - rlOrtho(0, GetScreenWidth(), GetScreenHeight(), 0, 0.0f, 1.0f); - rlMatrixMode(RL_MODELVIEW); - rlLoadIdentity(); - rlDisableDepthTest(); -} +// Camera/picking/mini-compass/misc-ImGui-widget functions (truncPath, +// wheel/reshape/motion, showAxes, updateCameraTransition/breakCameraTransition/ +// setCameraPreset, camMenu/view_kbd_shortcuts/cor_window/ImGuiHyperlink/ +// ShowShortcutsTable/info_window, drawMiniCompassWithRuler, rayIntersection/ +// GetLaserBeam/distance_point_to_line/getClosestTrajectoryPoint/ +// setNewRotationCenter, checkClHelp, updateOrthoView, end3DMatrixStack) now +// live in rl_utils.cpp/rl_utils.h -- see rl_utils.h's top comment. // Was ObservationPicking::render() (core/src/observation_picking.cpp) -- // legacy-GL, compiled once into `core`, shared with the remaining GLUT @@ -3746,7 +2460,7 @@ void renderLoopClosure(PointClouds& point_clouds_container, int index_loop_closu // Pose-sequence trail across the whole session, as a chain of thick // green cylinders (sphere at each joint), sized relative to the current // zoom (translate_z) so it stays visible next to the point cloud. - const float tubeRadius = std::max(0.05f, fabsf(translate_z) * 0.01f); + const float tubeRadius = std::max(0.005f, fabsf(translate_z) * 0.001f); bool first = true; Vector3 prev{}; for (const auto& pc : pointClouds) @@ -3797,11 +2511,43 @@ void renderLoopClosure(PointClouds& point_clouds_container, int index_loop_closu // in display() while the 3D projection/modelview was still active -- see // its declaration) and draws with DrawText instead. Must run after // end3DMatrixStack() (2D screen-space drawing). +namespace +{ +// Plain DrawText at a point sitting exactly on top of a same-size, often +// same-color trajectory marker is easy to lose visually -- outlined in +// black and nudged up-right of the anchor so it reads clearly regardless +// of what's directly underneath. +void drawOutlinedText(const char* text, Vector2 anchor, int fontSize, Color color) +{ + int x = static_cast(anchor.x) + 6; + int y = static_cast(anchor.y) - fontSize - 6; + for (int dx = -1; dx <= 1; ++dx) + { + for (int dy = -1; dy <= 1; ++dy) + { + if (dx != 0 || dy != 0) + { + DrawText(text, x + dx, y + dy, fontSize, BLACK); + } + } + } + DrawText(text, x, y, fontSize, color); +} +} // namespace + void renderLoopClosureLabels(PointClouds& point_clouds_container) { auto& pointClouds = point_clouds_container.point_clouds; - auto worldToScreen = [](const Eigen::Vector3d& world) -> Vector2 + // io.DisplaySize, not GetScreenWidth()/GetScreenHeight(): must match + // whatever reshape() last set the actual GL viewport to (see + // end3DMatrixStack()'s comment) -- the two can differ under DPI + // scaling, which would silently throw this off-screen. + ImGuiIO& io = ImGui::GetIO(); + float screenW = io.DisplaySize.x; + float screenH = io.DisplaySize.y; + + auto worldToScreen = [screenW, screenH](const Eigen::Vector3d& world) -> Vector2 { // Manual clip-space transform (mat * [x,y,z,1]^T) -- raymath's // Vector3Transform computes the same x/y/z but drops w, which the @@ -3819,14 +2565,13 @@ void renderLoopClosureLabels(PointClouds& point_clouds_container) } float ndcX = clipX / clipW; float ndcY = clipY / clipW; - return Vector2{ - (ndcX * 0.5f + 0.5f) * static_cast(GetScreenWidth()), (1.0f - (ndcY * 0.5f + 0.5f)) * static_cast(GetScreenHeight())}; + return Vector2{(ndcX * 0.5f + 0.5f) * screenW, (1.0f - (ndcY * 0.5f + 0.5f)) * screenH}; }; for (size_t i = 0; i < pointClouds.size(); ++i) { Vector2 screen = worldToScreen(pointClouds[i].m_pose.translation()); - DrawText(TextFormat("%d", static_cast(i)), static_cast(screen.x), static_cast(screen.y), 18, GREEN); + drawOutlinedText(TextFormat("%d", static_cast(i)), screen, 20, WHITE); } for (size_t i = 0; i < session.pose_graph_loop_closure.edges.size(); ++i) @@ -3843,8 +2588,8 @@ void renderLoopClosureLabels(PointClouds& point_clouds_container) Eigen::Vector3d midUp = (worldSrc + worldTrg) * 0.5 + Eigen::Vector3d(0, 0, 10); Vector2 screen = worldToScreen(midUp); - Color c = (static_cast(i) == session.pose_graph_loop_closure.index_active_edge) ? RED : BLUE; - DrawText(TextFormat("%d", static_cast(i)), static_cast(screen.x), static_cast(screen.y), 20, c); + Color c = (static_cast(i) == session.pose_graph_loop_closure.index_active_edge) ? RED : SKYBLUE; + drawOutlinedText(TextFormat("%d", static_cast(i)), screen, 22, c); } } @@ -5400,7 +4145,7 @@ void display() ImGui::SameLine(); ImGui::SetNextItemWidth(ImGuiNumberWidth); - ImGui::InputInt("Points render downsampling", &viewer_decimate_point_cloud, 10, 100); + ImGui::InputInt("Points render downsampling", &viewer_decimate_point_cloud, 2, 10); if (ImGui::IsItemHovered()) ImGui::SetTooltip("increase for better performance, decrease for rendering more points"); ImGui::SameLine(); diff --git a/apps/multi_view_tls_registration/rl_utils.cpp b/apps/multi_view_tls_registration/rl_utils.cpp new file mode 100644 index 00000000..492ec99b --- /dev/null +++ b/apps/multi_view_tls_registration/rl_utils.cpp @@ -0,0 +1,1260 @@ +#include "rl_utils.h" + +#include "raylib.h" +#include "rlgl.h" +#include "raymath.h" +#include "external/glad.h" + +#include + +#include + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +#ifdef _WIN32 +#include +#include +#endif + +/////////////////////////////////////////////////////////////////////////////////// +// Formerly 's extern globals -- defined here now (see +// rl_utils.h's top comment for why). +/////////////////////////////////////////////////////////////////////////////////// + +int viewer_decimate_point_cloud = 2; + +int mouse_old_x, mouse_old_y; +int mouse_buttons = 0; +float mouse_sensitivity = 1.0; + +bool is_ortho = false; +bool lock_z = false; +bool show_axes = true; +ImVec4 bg_color = ImVec4(0.65f, 0.65f, 0.65f, 1.00f); +int point_size = 1; + +bool info_gui = false; +bool compass_ruler = true; + +Eigen::Affine3f viewLocal; + +Eigen::Vector3f rotation_center = Eigen::Vector3f::Zero(); +float rotate_x = -35.264f, rotate_y = 135.0f; +float translate_x, translate_y = 0.0; +float translate_z = -50.0; + +double camera_ortho_xy_view_zoom = 10; +double camera_ortho_xy_view_shift_x = 0.0; +double camera_ortho_xy_view_shift_y = 0.0; +double camera_mode_ortho_z_center_h = 0.0; + +// Target camera state for smooth transitions +Eigen::Vector3f new_rotation_center = rotation_center; +float new_rotate_x = rotate_x; +float new_rotate_y = rotate_y; +float new_translate_x = translate_x; +float new_translate_y = translate_y; +float new_translate_z = translate_z; + +bool cor_gui = false; + +// Transition timing +bool camera_transition_active = false; + +bool scroll_hint_enabled = true; +bool scroll_hint_active = false; +int scroll_hint_count = 0; +float scroll_hint_accu = 0.0f; +double scroll_hint_lastT = 0.0; + +bool show_about = false; + +bool glLineWidthSupport = true; + +float m_ortho_projection[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; +float m_ortho_gizmo_view[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; + +// ============================================================================ +// Formerly core/src/utils.cpp -- local now (see the big comment at the top +// of this file for why). Everywhere the original was pure ImGui/Eigen/GLM +// (no gl*/glu*/glut* calls), it's copied verbatim. Everywhere it touched +// legacy GL, it's reimplemented with rlgl's rl*() legacy-emulation API +// (a software matrix stack + immediate-mode layer that mirrors gl*()'s +// call shape but works under a core-profile context), or with raylib/ +// raymath equivalents (gluUnProject -> Vector3Unproject, glutBitmapCharacter +// -> DrawText). Function names/signatures/globals are unchanged so every +// call site elsewhere in this file (display(), mouse(), the panel +// functions, ...) needed no changes. +// ============================================================================ + +std::string truncPath(const std::string& fullPath) +{ + namespace fspath = std::filesystem; + fspath::path path(fullPath); + + auto parent1 = path.parent_path().filename().string(); + auto parent2 = path.parent_path().parent_path().filename().string(); // second to last folder + auto filename = path.filename().string(); + + return "..\\" + parent2 + "\\" + parent1 + "\\" + filename; +} + +void wheel(int button, int dir, int x, int y) +{ + ImGuiIO& io = ImGui::GetIO(); + io.MouseWheel += dir; // or direction * 1.0f depending on your setup + + if (!ImGui::IsWindowHovered(ImGuiHoveredFlags_AnyWindow)) + { + if (dir > 0) + { + if (is_ortho) + { + camera_ortho_xy_view_zoom -= 0.1f * camera_ortho_xy_view_zoom; + + if (camera_ortho_xy_view_zoom < 0.1) + { + camera_ortho_xy_view_zoom = 0.1; + } + } + else + { + if (io.KeyShift) + translate_z += 5.0f; + else + translate_z += 1.0f; + } + } + else + { + if (is_ortho) + camera_ortho_xy_view_zoom += 0.1 * camera_ortho_xy_view_zoom; + else + { + if (io.KeyShift) + translate_z -= 5.0f; + else + translate_z -= 1.0f; + } + } + + mouse_sensitivity = fabs(translate_z) / 100; // 1 for translate_z 50 (default zoom) + camera_transition_active = false; + + if (scroll_hint_enabled) + { + if (!scroll_hint_active) + { + scroll_hint_accu += fabs(dir); + + if (scroll_hint_accu > 30.0f) // tweak threshold + { + scroll_hint_accu = 0.0f; + scroll_hint_active = true; + scroll_hint_count++; + } + } + + if (scroll_hint_active) + scroll_hint_lastT = ImGui::GetTime(); + + // Reset and disable hint if Shift is pressed while scrolling + if (io.KeyShift || scroll_hint_count > 3) + { + scroll_hint_active = false; + scroll_hint_enabled = false; + } + } + } +} + +// Was glMatrixMode/glLoadIdentity/gluPerspective/glOrtho -- rewritten with +// rlgl's software matrix-stack API (RL_PROJECTION/RL_MODELVIEW), which +// works under raylib's core-profile context. gluPerspective(fovy, aspect, +// near, far) has no rl* equivalent, so it's expanded to the equivalent +// rlFrustum() call by hand (standard fovy -> frustum-bounds formula). +void reshape(int w, int h) +{ + rlViewport(0, 0, (int)w, (int)h); + rlMatrixMode(RL_PROJECTION); + rlLoadIdentity(); + if (!is_ortho) + { + const double fovy = 60.0; + const double aspect = (double)w / (double)h; + const double nearP = 0.01, farP = 10000.0; + const double top = nearP * tan(fovy * 0.5 * M_PI / 180.0); + const double bottom = -top; + const double right = top * aspect; + const double left = -right; + rlFrustum(left, right, bottom, top, nearP, farP); + } + else + { + ImGuiIO& io = ImGui::GetIO(); + float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); + + rlOrtho( + -camera_ortho_xy_view_zoom, + camera_ortho_xy_view_zoom, + -camera_ortho_xy_view_zoom / ratio, + camera_ortho_xy_view_zoom / ratio, + -100000, + 100000); + } + rlMatrixMode(RL_MODELVIEW); + rlLoadIdentity(); +} + +// GL-free -- copied verbatim, minus the trailing glutPostRedisplay() (a +// no-op here: this app's main loop already redraws every frame). +void motion(int x, int y) +{ + ImGuiIO& io = ImGui::GetIO(); + io.MousePos = ImVec2((float)x, (float)y); + + if (!io.WantCaptureMouse) + { + float dx, dy; + dx = (float)(x - mouse_old_x); + dy = (float)(y - mouse_old_y); + + if (mouse_buttons & 1) // left button + { + rotate_x += dy * 0.2f; + rotate_y += dx * 0.2f; + breakCameraTransition(); + } + + if (mouse_buttons & 4) // right button + { + if (is_ortho) + { + float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); + Eigen::Vector3d v( + dx * (camera_ortho_xy_view_zoom / (float)io.DisplaySize.x * 2), + dy * (camera_ortho_xy_view_zoom / (float)io.DisplaySize.y * 2 / ratio), + 0); + TaitBryanPose pose_tb; + pose_tb.px = 0.0; + pose_tb.py = 0.0; + pose_tb.pz = 0.0; + pose_tb.om = 0.0; + pose_tb.fi = 0.0; + pose_tb.ka = (rotate_x + rotate_y) * M_PI / 180.0; + auto m = affine_matrix_from_pose_tait_bryan(pose_tb); + Eigen::Vector3d v_t = m * v; + camera_ortho_xy_view_shift_x += v_t.x(); + camera_ortho_xy_view_shift_y += v_t.y(); + } + else + { + translate_x += dx * 0.1f * mouse_sensitivity; + translate_y -= dy * 0.1f * mouse_sensitivity; + breakCameraTransition(); + } + } + + mouse_old_x = x; + mouse_old_y = y; + } +} + +// GL-free -- copied verbatim. +static bool first_time = true; + +void ShowMainDockSpace() +{ + ImGuiWindowFlags window_flags = ImGuiWindowFlags_NoDocking | ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_NoCollapse | + ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoBringToFrontOnFocus | ImGuiWindowFlags_NoNavFocus | + ImGuiWindowFlags_NoBackground | ImGuiWindowFlags_NoInputs; + + ImGuiViewport* viewport = ImGui::GetMainViewport(); + ImGui::SetNextWindowPos(viewport->WorkPos); + ImGui::SetNextWindowSize(viewport->WorkSize); + ImGui::SetNextWindowViewport(viewport->ID); + + ImGui::PushStyleVar(ImGuiStyleVar_WindowRounding, 0.0f); + ImGui::PushStyleVar(ImGuiStyleVar_WindowBorderSize, 0.0f); + + ImGui::Begin("MainDockSpace", nullptr, window_flags); + + ImGui::PopStyleVar(2); + + // This is the dockspace! + ImGuiID dockspace_id = ImGui::GetID("MyDockSpace"); + ImGui::DockSpace(dockspace_id, ImVec2(0, 0), ImGuiDockNodeFlags_PassthruCentralNode | ImGuiDockNodeFlags_NoDockingInCentralNode); + + if (first_time) + { + first_time = false; + + auto dock_id_left = ImGui::DockBuilderSplitNode(dockspace_id, ImGuiDir_Left, 0.2f, nullptr, &dockspace_id); + auto dock_id_bottom = ImGui::DockBuilderSplitNode(dockspace_id, ImGuiDir_Down, 0.2f, nullptr, &dockspace_id); + + ImGui::DockBuilderDockWindow("Console", dock_id_bottom); + ImGui::DockBuilderFinish(dockspace_id); + } + + ImGui::End(); +} + +// Was glBegin(GL_LINES)/glColor3f/glVertex3f/glEnd -- rl* rename. +void showAxes() +{ + if (show_axes || ImGui::GetIO().KeyCtrl) // rotation center axes + { + rlBegin(RL_LINES); + rlColor3f(1.f, 1.f, 1.f); + rlVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z()); + rlVertex3f(rotation_center.x() + 1.f, rotation_center.y(), rotation_center.z()); + rlVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z()); + rlVertex3f(rotation_center.x() - 1.f, rotation_center.y(), rotation_center.z()); + rlVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z()); + rlVertex3f(rotation_center.x(), rotation_center.y() - 1.f, rotation_center.z()); + rlVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z()); + rlVertex3f(rotation_center.x(), rotation_center.y() + 1.f, rotation_center.z()); + rlVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z()); + rlVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z() - 1.f); + rlVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z()); + rlVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z() + 1.f); + rlEnd(); + } + + if (show_axes || ImGui::GetIO().KeyCtrl) // origin axes + { + rlBegin(RL_LINES); + rlColor3f(1.0f, 0.0f, 0.0f); + rlVertex3f(0.0f, 0.0f, 0.0f); + rlVertex3f(100, 0.0f, 0.0f); + + rlColor3f(0.0f, 1.0f, 0.0f); + rlVertex3f(0.0f, 0.0f, 0.0f); + rlVertex3f(0.0f, 100, 0.0f); + + rlColor3f(0.0f, 0.0f, 1.0f); + rlVertex3f(0.0f, 0.0f, 0.0f); + rlVertex3f(0.0f, 0.0f, 100); + rlEnd(); + } +} + +// GL-free -- copied verbatim. +void updateCameraTransition() +{ + if (!camera_transition_active) + return; + + float t = 1.0f - powf(1.0f - std::min(ImGui::GetIO().DeltaTime * camera_transition_speed, 1.0f), 3.0f); + + bool doneXrc = fabs(new_rotation_center.x() - rotation_center.x()) < 0.01f; + bool doneYrc = fabs(new_rotation_center.y() - rotation_center.y()) < 0.01f; + bool doneZrc = fabs(new_rotation_center.z() - rotation_center.z()) < 0.01f; + bool doneXr = fabs(new_rotate_x - rotate_x) < 0.01f; + bool doneYr = fabs(new_rotate_y - rotate_y) < 0.01f; + bool doneXt = fabs(new_translate_x - translate_x) < 0.01f; + bool doneYt = fabs(new_translate_y - translate_y) < 0.01f; + bool doneZt = fabs(new_translate_z - translate_z) < 0.01f; + + if (!doneXrc) + rotation_center.x() += (new_rotation_center.x() - rotation_center.x()) * t; + if (!doneYrc) + rotation_center.y() += (new_rotation_center.y() - rotation_center.y()) * t; + if (!doneZrc) + rotation_center.z() += (new_rotation_center.z() - rotation_center.z()) * t; + if (!doneXr) + rotate_x += (new_rotate_x - rotate_x) * t; + if (!doneYr) + rotate_y += (new_rotate_y - rotate_y) * t; + if (!doneXt) + translate_x += (new_translate_x - translate_x) * t; + if (!doneYt) + translate_y += (new_translate_y - translate_y) * t; + if (!doneZt) + translate_z += (new_translate_z - translate_z) * t; + + camera_transition_active = !(doneXrc && doneYrc && doneZrc && doneXr && doneYr && doneXt && doneYt && doneZt); + + if (!camera_transition_active) + { + rotation_center = new_rotation_center; + rotate_x = new_rotate_x; + rotate_y = new_rotate_y; + translate_x = new_translate_x; + translate_y = new_translate_y; + translate_z = new_translate_z; + } +} + +// GL-free -- copied verbatim. +void breakCameraTransition() +{ + if (camera_transition_active == false) + return; + rotation_center = new_rotation_center; + camera_transition_active = false; +} + +// GL-free -- copied verbatim. +void setCameraPreset(CameraPreset preset) +{ + bool triggered = false; + + switch (preset) + { + case CAMERA_FRONT: + new_rotate_x = -90.0f; + new_rotate_y = +90.0f; + triggered = true; + break; + case CAMERA_BACK: + new_rotate_x = -90.0f; + new_rotate_y = -90.0f; + triggered = true; + break; + case CAMERA_LEFT: + new_rotate_x = -90.0f; + new_rotate_y = 180.0f; + triggered = true; + break; + case CAMERA_RIGHT: + new_rotate_x = -90.0f; + new_rotate_y = 0.0f; + triggered = true; + break; + case CAMERA_TOP: + new_rotate_x = 0.0f; + new_rotate_y = 90.0f; + triggered = true; + break; + case CAMERA_BOTTOM: + new_rotate_x = 180.0f; + new_rotate_y = -90.0f; + triggered = true; + break; + case CAMERA_ISO: + new_rotate_x = -35.264f; + new_rotate_y = 135.0f; + triggered = true; + break; + case CAMERA_RESET: + new_rotation_center = Eigen::Vector3f::Zero(); + new_rotate_x = 0; + new_rotate_y = 0; + new_translate_x = 0; + new_translate_y = 0; + new_translate_z = -50.0f; + mouse_sensitivity = fabs(translate_z) / 100; + + camera_ortho_xy_view_zoom = 10; + camera_ortho_xy_view_shift_x = 0.0; + camera_ortho_xy_view_shift_y = 0.0; + camera_mode_ortho_z_center_h = 0.0; + + viewer_decimate_point_cloud = 1000; + triggered = false; + break; + } + + if (triggered) + { + new_rotation_center = rotation_center; + new_translate_x = translate_x; + new_translate_y = translate_y; + new_translate_z = translate_z; + } + + camera_transition_active = true; +} + +// GL-free -- copied verbatim. +void camMenu() +{ + if (ImGui::BeginMenu("Camera")) + { + if (ImGui::MenuItem("Front (yz view)", "key F")) + setCameraPreset(CAMERA_FRONT); + if (ImGui::MenuItem("Back", "key B")) + setCameraPreset(CAMERA_BACK); + if (ImGui::MenuItem("Left (xz view)", "key L")) + setCameraPreset(CAMERA_LEFT); + if (ImGui::MenuItem("Right", "key R")) + setCameraPreset(CAMERA_RIGHT); + if (ImGui::MenuItem("Top (xy view)", "key T")) + setCameraPreset(CAMERA_TOP); + if (ImGui::MenuItem("Bottom", "key U")) + setCameraPreset(CAMERA_BOTTOM); + if (ImGui::MenuItem("Isometric", "key I")) + setCameraPreset(CAMERA_ISO); + ImGui::Separator(); + if (ImGui::MenuItem("Reset", "key Z")) + setCameraPreset(CAMERA_RESET); + + ImGui::EndMenu(); + } + if (ImGui::IsItemHovered()) + { + ImGui::BeginTooltip(); + ImGui::Text("Change camera view to fixed positions"); + ImGui::Separator(); + ImGui::Text("Metrics:"); + if (ImGui::BeginTable("Metrics", 4)) + { + ImGui::TableSetupColumn("Coord"); + ImGui::TableSetupColumn("rotate"); + ImGui::TableSetupColumn("translate"); + ImGui::TableSetupColumn("rot center"); + ImGui::TableHeadersRow(); + + ImGui::TableNextRow(); + ImGui::TableSetColumnIndex(0); + + std::string text = "X"; + float centered = ImGui::GetColumnWidth() - ImGui::CalcTextSize(text.c_str()).x; + ImGui::SetCursorPosX(ImGui::GetCursorPosX() + centered * 0.5f); + ImGui::Text("X"); + + ImGui::TableSetColumnIndex(1); + ImGui::Text("%.3f", rotate_x); + ImGui::TableSetColumnIndex(2); + ImGui::Text("%.3f", translate_x); + ImGui::TableSetColumnIndex(3); + ImGui::Text("%.3f", rotation_center.x()); + + ImGui::TableNextRow(); + ImGui::TableSetColumnIndex(0); + ImGui::SetCursorPosX(ImGui::GetCursorPosX() + centered * 0.5f); + ImGui::Text("Y"); + + ImGui::TableSetColumnIndex(1); + ImGui::Text("%.3f", rotate_y); + ImGui::TableSetColumnIndex(2); + ImGui::Text("%.3f", translate_y); + ImGui::TableSetColumnIndex(3); + ImGui::Text("%.3f", rotation_center.y()); + + ImGui::TableNextRow(); + ImGui::TableSetColumnIndex(0); + ImGui::SetCursorPosX(ImGui::GetCursorPosX() + centered * 0.5f); + ImGui::Text("Z"); + + ImGui::TableSetColumnIndex(2); + ImGui::Text("%.3f", translate_z); + ImGui::TableSetColumnIndex(3); + ImGui::Text("%.3f", rotation_center.y()); + + ImGui::EndTable(); + } + ImGui::Text("Mouse sensitivity: %.4f", mouse_sensitivity); + + ImGui::EndTooltip(); + } + + if (scroll_hint_active) + { + ImVec2 mousePos = ImGui::GetMousePos(); + ImGui::SetNextWindowPos(ImVec2(mousePos.x + 20, mousePos.y - 40)); + ImGui::SetNextWindowBgAlpha(0.7f); + ImGui::BeginTooltip(); + ImGui::Text("Tip: To accelerate hold Shift + scroll"); + ImGui::EndTooltip(); + + if (ImGui::GetTime() - scroll_hint_lastT > 1) + scroll_hint_active = false; + } +} + +// GL-free -- copied verbatim. +void view_kbd_shortcuts() +{ + ImGuiIO& io = ImGui::GetIO(); + + if (io.WantCaptureKeyboard) + return; + + if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_RightArrow, true)) + { + translate_x += 0.5f * mouse_sensitivity; + breakCameraTransition(); + } + if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_LeftArrow, true)) + { + translate_x -= 0.5f * mouse_sensitivity; + breakCameraTransition(); + } + + if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_UpArrow, true)) + { + translate_y += 0.5f * mouse_sensitivity; + breakCameraTransition(); + } + if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_DownArrow, true)) + { + translate_y -= 0.5f * mouse_sensitivity; + breakCameraTransition(); + } + + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_RightArrow, true)) + { + rotate_y -= 0.6; + breakCameraTransition(); + } + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_LeftArrow, true)) + { + rotate_y += 0.6; + breakCameraTransition(); + } + + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_UpArrow, true)) + { + rotate_x -= 0.6; + breakCameraTransition(); + } + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_DownArrow, true)) + { + rotate_x += 0.6; + breakCameraTransition(); + } + + if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_R, false)) + cor_gui = true; + + if (io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_Z, false) && !is_ortho) + lock_z = !lock_z; + + if (io.KeyCtrl || io.KeyAlt || io.KeyShift) + return; + + if (ImGui::IsKeyPressed(ImGuiKey_B)) + setCameraPreset(CAMERA_BACK); + if (ImGui::IsKeyPressed(ImGuiKey_F)) + setCameraPreset(CAMERA_FRONT); + if (ImGui::IsKeyPressed(ImGuiKey_I)) + setCameraPreset(CAMERA_ISO); + if (ImGui::IsKeyPressed(ImGuiKey_L)) + setCameraPreset(CAMERA_LEFT); + if (ImGui::IsKeyPressed(ImGuiKey_R)) + setCameraPreset(CAMERA_RIGHT); + if (ImGui::IsKeyPressed(ImGuiKey_T)) + setCameraPreset(CAMERA_TOP); + if (ImGui::IsKeyPressed(ImGuiKey_U)) + setCameraPreset(CAMERA_BOTTOM); + if (ImGui::IsKeyPressed(ImGuiKey_Z)) + setCameraPreset(CAMERA_RESET); + + if (ImGui::IsKeyPressed(ImGuiKey_C, false)) + compass_ruler = !compass_ruler; + if (ImGui::IsKeyPressed(ImGuiKey_O, false)) + is_ortho = !is_ortho; + if (ImGui::IsKeyPressed(ImGuiKey_X, false)) + show_axes = !show_axes; + + if (ImGui::IsKeyPressed(ImGuiKey_1)) + point_size = 1; + if (ImGui::IsKeyPressed(ImGuiKey_2)) + point_size = 2; + if (ImGui::IsKeyPressed(ImGuiKey_3)) + point_size = 3; + if (ImGui::IsKeyPressed(ImGuiKey_4)) + point_size = 4; + if (ImGui::IsKeyPressed(ImGuiKey_5)) + point_size = 5; + if (ImGui::IsKeyPressed(ImGuiKey_6)) + point_size = 6; + if (ImGui::IsKeyPressed(ImGuiKey_7)) + point_size = 7; + if (ImGui::IsKeyPressed(ImGuiKey_8)) + point_size = 8; + if (ImGui::IsKeyPressed(ImGuiKey_9)) + point_size = 9; +} + +// GL-free -- copied verbatim. +void cor_window() +{ + if (cor_gui) + { + ImGui::OpenPopup("Center of rotation"); + cor_gui = false; + } + + if (ImGui::BeginPopupModal("Center of rotation", NULL, ImGuiWindowFlags_AlwaysAutoResize)) + { + ImGui::Text("Select new center of rotation [m]:"); + ImGui::PushItemWidth(ImGuiNumberWidth); + ImGui::InputFloat("X", &new_rotation_center.x(), 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(xText); + ImGui::SameLine(); + ImGui::InputFloat("Y", &new_rotation_center.y(), 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(yText); + ImGui::SameLine(); + ImGui::InputFloat("Z", &new_rotation_center.z(), 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(zText); + ImGui::PopItemWidth(); + + ImGui::Separator(); + + if (ImGui::Button("Set")) + { + new_rotate_x = rotate_x; + new_rotate_y = rotate_y; + new_translate_x = -new_rotation_center.x(); + new_translate_y = -new_rotation_center.y(); + new_translate_z = translate_z; + + camera_transition_active = true; + + ImGui::CloseCurrentPopup(); + } + + ImGui::SameLine(); + if (ImGui::Button("Cancel")) + { + ImGui::CloseCurrentPopup(); + } + + ImGui::EndPopup(); + } +} + +// GL-free -- copied verbatim. +void ImGuiHyperlink(const char* url, ImVec4 color) +{ + ImGui::PushStyleColor(ImGuiCol_Text, color); + ImGui::TextUnformatted(url); + ImGui::PopStyleColor(); + + ImVec2 pos = ImGui::GetItemRectMin(); + ImVec2 size = ImGui::GetItemRectSize(); + + if (ImGui::IsItemHovered()) + ImGui::SetMouseCursor(ImGuiMouseCursor_Hand); + + if (ImGui::IsItemHovered()) + { + ImDrawList* draw_list = ImGui::GetWindowDrawList(); + draw_list->AddLine(ImVec2(pos.x, pos.y + size.y), ImVec2(pos.x + size.x, pos.y + size.y), ImColor(color)); + } + + if (ImGui::IsItemClicked()) + { +#ifdef _WIN32 + ShellExecuteA(0, "open", url, 0, 0, SW_SHOWNORMAL); +#elif __APPLE__ + std::string cmd = std::string("open ") + url; + system(cmd.c_str()); +#else + std::string cmd = std::string("xdg-open ") + url; + system(cmd.c_str()); +#endif + } +} + +// General shortcuts applicable to any app -- GL-free, copied verbatim. +static const std::vector shortcuts = { { "Normal keys", "A", "" }, + { "", "Ctrl+A", "" }, + { "", "B", "camera Back" }, + { "", "Ctrl+B", "" }, + { "", "C", "Compass/ruler" }, + { "", "Ctrl+C", "" }, + { "", "D", "" }, + { "", "Ctrl+D", "" }, + { "", "E", "" }, + { "", "Ctrl+E", "" }, + { "", "F", "camera Front" }, + { "", "Ctrl+F", "" }, + { "", "G", "" }, + { "", "Ctrl+G", "" }, + { "", "H", "" }, + { "", "Ctrl+H", "" }, + { "", "I", "camera Isometric" }, + { "", "Ctrl+I", "" }, + { "", "J", "" }, + { "", "Ctrl+J", "" }, + { "", "K", "" }, + { "", "Ctrl+K", "" }, + { "", "L", "camera Left" }, + { "", "Ctrl+L", "" }, + { "", "M", "" }, + { "", "Ctrl+M", "" }, + { "", "N", "" }, + { "", "Ctrl+N", "" }, + { "", "O", "Ortographic view" }, + { "", "Ctrl+O", "Open/load session/data" }, + { "", "P", "" }, + { "", "Ctrl+P", "" }, + { "", "Q", "" }, + { "", "Ctrl+Q", "" }, + { "", "R", "camera Right" }, + { "", "Ctrl+R", "" }, + { "", "Shift+R", "Rotation center" }, + { "", "S", "" }, + { "", "Ctrl+S", "" }, + { "", "Ctrl+Shift+S", "" }, + { "", "T", "camera Top" }, + { "", "Ctrl+T", "" }, + { "", "U", "camera bottom (Under)" }, + { "", "Ctrl+U", "" }, + { "", "V", "" }, + { "", "Ctrl+V", "" }, + { "", "W", "" }, + { "", "Ctrl+W", "" }, + { "", "X", "show aXes" }, + { "", "Ctrl+X", "" }, + { "", "Y", "" }, + { "", "Ctrl+Y", "" }, + { "", "Z", "camera reset" }, + { "", "Ctrl+Z", "" }, + { "", "Shift+Z", "Lock Z" }, + { "", "1-9", "point size" }, + { "Special keys", "Up arrow", "" }, + { "", "Shift + up arrow", "camera translate Up" }, + { "", "Ctrl + up arrow", "" }, + { "", "Down arrow", "" }, + { "", "Shift + down arrow", "camera translate Down" }, + { "", "Ctrl + down arrow", "" }, + { "", "Left arrow", "" }, + { "", "Shift + left arrow", "camera translate Left" }, + { "", "Ctrl + left arrow", "" }, + { "", "Right arrow", "" }, + { "", "Shift + right arrow", "camera translate Right" }, + { "", "Ctrl + right arrow", "" }, + { "", "Pg down", "" }, + { "", "Pg up", "" }, + { "", "- key", "" }, + { "", "+ key", "" }, + { "Mouse related", "Left click + drag", "camera rotate" }, + { "", "Right click + drag", "camera pan" }, + { "", "Scroll", "camera zoom" }, + { "", "Shift + scroll", "camera 5x zoom" }, + { "", "Shift + drag", "Dock window to screen edges" }, + { "", "Ctrl + left click", "" }, + { "", "Ctrl + right click", "change center of rotation" }, + { "", "Ctrl + middle click", "change center of rotation (if no CP GUI active)" } }; + +// GL-free -- copied verbatim. +void ShowShortcutsTable(const std::vector appShortcuts) +{ + if (ImGui::BeginTable( + "ShortcutsTable", 2, ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg | ImGuiTableFlags_ScrollY, ImVec2(-FLT_MIN, 200))) + { + ImGui::TableSetupScrollFreeze(0, 1); + ImGui::TableSetupColumn("Shortcut", ImGuiTableColumnFlags_WidthFixed, 120); + ImGui::TableSetupColumn("Description"); + ImGui::TableHeadersRow(); + + std::string lastType; + + for (size_t i = 0; i < shortcuts.size(); ++i) + { + const auto& s = shortcuts[i]; + + if (!s.type.empty() && s.type != lastType) + { + lastType = s.type; + ImGui::TableNextRow(); + + ImGui::TableSetBgColor(ImGuiTableBgTarget_RowBg0, IM_COL32(70, 70, 140, 255)); + + ImGui::TableSetColumnIndex(0); + ImGui::TextColored(ImVec4(0.8f, 0.8f, 1.0f, 1.0f), "%s", lastType.c_str()); + ImGui::TableSetColumnIndex(1); + ImGui::TextUnformatted(""); + } + + auto description = s.description; + + if (description.empty()) + description = appShortcuts[i].description; + + if (!description.empty()) + { + ImGui::TableNextRow(); + ImGui::TableSetColumnIndex(0); + ImGui::TextUnformatted(s.shortcut.c_str()); + ImGui::TableSetColumnIndex(1); + ImGui::TextUnformatted(description.c_str()); + } + } + + ImGui::EndTable(); + } +} + +// GL-free -- copied verbatim (glGetString(GL_RENDERER/...) is a plain +// string query, still valid under a core-profile context). +void info_window(const std::vector& infoLines, const std::vector& appShortcuts) +{ + if (!info_gui) + return; + + if (ImGui::Begin( + "Info", + &info_gui, + ImGuiWindowFlags_NoResize | ImGuiWindowFlags_AlwaysAutoResize | ImGuiWindowFlags_NoDocking | ImGuiWindowFlags_NoCollapse)) + { + bool firstLine = true; + for (const auto& line : infoLines) + { + if (line.empty()) + ImGui::NewLine(); + else if (line.rfind("https://", 0) == 0) + ImGuiHyperlink(line.c_str()); + else + ImGui::Text(line.c_str()); + + if (firstLine) + { + ImGui::SameLine( + ImGui::GetWindowWidth() - ImGui::CalcTextSize("ImGui").x - ImGui::GetStyle().ItemSpacing.x * 2 - + ImGui::GetStyle().FramePadding.x * 2); + if (ImGui::Button("ImGui")) + show_about = true; + if (ImGui::IsItemHovered()) + { + ImGui::BeginTooltip(); + const GLubyte* renderer = glGetString(GL_RENDERER); + const GLubyte* version = glGetString(GL_VERSION); + const GLubyte* glslVersion = glGetString(GL_SHADING_LANGUAGE_VERSION); + + ImGui::Text("Renderer: %s", renderer); + ImGui::Text("OpenGL version supported: %s", version); + ImGui::Text("GLSL version: %s", glslVersion); + ImGui::EndTooltip(); + } + + firstLine = false; + } + } + + ImGui::NewLine(); + ImGui::Text("Author: Janusz Bedkowski & contributors"); + ImGui::NewLine(); + ImGui::Text("Part of HDMapping software suite"); + ImGui::Text("Version: %s (%s)", HDMAPPING_VERSION_STRING, __DATE__); + ImGui::Text("Project page: "); + ImGui::SameLine(); + ImGuiHyperlink("https://github.com/MapsHD/HDMapping"); + + ImGui::NewLine(); + ImGui::Separator(); + ImGui::NewLine(); + + ShowShortcutsTable(appShortcuts); + + if (show_about) + ImGui::ShowAboutWindow(&show_about); + } + + ImGui::End(); +} + +// Was a dedicated 200x200 GL sub-viewport with its own glOrtho projection, +// rotation-only modelview (viewLocal's rotation), and GLUT bitmap-font text +// (glRasterPos + glutBitmapCharacter). Reimplemented as a pure 2D +// screen-space overlay instead: project each world axis direction through +// viewLocal's rotation to get eye-space X/Y (screen right/up), and draw +// with raylib's DrawLineEx/DrawText -- same bottom-left placement, same +// "nice number" ruler tied to zoom (translate_z), no sub-viewport or GLUT +// font needed. +void drawMiniCompassWithRuler() +{ + const float compassSize = 200.0f; + const float originX = compassSize * 0.5f; + const float originY = static_cast(GetScreenHeight()) - compassSize * 0.5f; + const float axisPixelLength = compassSize * 0.35f; + + struct Axis + { + Eigen::Vector3f dir; + const char* label; + Color color; + }; + const Axis axes[3] = { + {Eigen::Vector3f::UnitX(), "X (long.)", RED}, + {Eigen::Vector3f::UnitY(), "Y (lat.)", GREEN}, + {Eigen::Vector3f::UnitZ(), "Z (vert.)", BLUE}, + }; + + for (const auto& axis : axes) + { + Eigen::Vector3f eyeDir = viewLocal.rotation() * axis.dir; + Vector2 tip = {originX + eyeDir.x() * axisPixelLength, originY - eyeDir.y() * axisPixelLength}; + DrawLineEx(Vector2{originX, originY}, tip, 2.f, axis.color); + DrawText(axis.label, (int)tip.x + 4, (int)tip.y - 6, 12, axis.color); + } + + // Ruler: "nice" (1/2/5 x 10^n) length, mirroring the original's + // 0.1 * fabs(translate_z) heuristic (translate_z is this app's + // zoom/dolly distance). + float rawUnit = std::max(0.001f, 0.1f * fabsf(translate_z)); + float base = powf(10.0f, floorf(log10f(rawUnit))); + float normalized = rawUnit / base; + float niceUnit = normalized < 2.0f ? 1.0f : (normalized < 5.0f ? 2.0f : 5.0f); + float worldLength = niceUnit * base; + + char label[32]; + if (worldLength >= 1000.0f) + snprintf(label, sizeof(label), "%.0f [km]", worldLength / 1000.0f); + else if (worldLength >= 1.0f) + snprintf(label, sizeof(label), "%.0f [m]", worldLength); + else if (worldLength >= 0.01f) + snprintf(label, sizeof(label), "%.0f [cm]", worldLength * 100.0f); + else + snprintf(label, sizeof(label), "<1 [cm]"); + + float rulerY = originY + compassSize * 0.45f; + Color rulerColor = ColorFromNormalized(Vector4{1.0f - bg_color.x, 1.0f - bg_color.y, 1.0f - bg_color.z, 1.0f}); + DrawLineEx(Vector2{originX - 40.f, rulerY}, Vector2{originX + 40.f, rulerY}, 2.f, rulerColor); + DrawLineEx(Vector2{originX - 40.f, rulerY - 5.f}, Vector2{originX - 40.f, rulerY + 5.f}, 2.f, rulerColor); + DrawLineEx(Vector2{originX + 40.f, rulerY - 5.f}, Vector2{originX + 40.f, rulerY + 5.f}, 2.f, rulerColor); + DrawText(label, (int)originX - 20, (int)rulerY + 6, 14, rulerColor); +} + +// GL-free -- copied verbatim. +float distanceToPlane(const RegistrationPlaneFeature::Plane& plane, const Eigen::Vector3d& p) +{ + return (plane.a * p.x() + plane.b * p.y() + plane.c * p.z() + plane.d); +} + +// GL-free -- copied verbatim. +Eigen::Vector3d rayIntersection(const LaserBeam& laser_beam, const RegistrationPlaneFeature::Plane& plane) +{ + float TOLERANCE = 0.0001; + Eigen::Vector3d out_point; + out_point.x() = laser_beam.position.x(); + out_point.y() = laser_beam.position.y(); + out_point.z() = laser_beam.position.z(); + + float a = plane.a * laser_beam.direction.x() + plane.b * laser_beam.direction.y() + plane.c * laser_beam.direction.z(); + + if (a > -TOLERANCE && a < TOLERANCE) + { + return out_point; + } + + float distance = distanceToPlane(plane, out_point); + + out_point.x() = laser_beam.position.x() - laser_beam.direction.x() * (distance / a); + out_point.y() = laser_beam.position.y() - laser_beam.direction.y() * (distance / a); + out_point.z() = laser_beam.position.z() - laser_beam.direction.z() * (distance / a); + + return out_point; +} + +// Was gluUnProject(winX, winY, winZ, modelview, projection, viewport, ...) +// against glGetDoublev(GL_MODELVIEW/PROJECTION_MATRIX) -- rewritten with +// raymath's Vector3Unproject against rlgl's current matrix stack +// (rlGetMatrixModelview/Projection), following the same NDC-space +// conversion raylib's own GetScreenToWorldRayEx uses. The original's +// far point used winZ=-1000 (an out-of-range hack to get a point far along +// the ray, since gluUnProject doesn't clamp); using the actual far-plane +// NDC z=1 here is equally valid for the same purpose (only direction, not +// magnitude, of laser_beam.direction matters to callers). +LaserBeam GetLaserBeam(int x, int y) +{ + int width = GetScreenWidth(); + int height = GetScreenHeight(); + + float ndcX = (2.0f * (float)x) / (float)width - 1.0f; + float ndcY = 1.0f - (2.0f * (float)y) / (float)height; + + Matrix matView = rlGetMatrixModelview(); + Matrix matProj = rlGetMatrixProjection(); + + Vector3 nearPoint = Vector3Unproject(Vector3{ndcX, ndcY, 0.0f}, matProj, matView); + Vector3 farPoint = Vector3Unproject(Vector3{ndcX, ndcY, 1.0f}, matProj, matView); + + LaserBeam laser_beam; + laser_beam.position = Eigen::Vector3d(nearPoint.x, nearPoint.y, nearPoint.z); + laser_beam.direction = Eigen::Vector3d(farPoint.x - nearPoint.x, farPoint.y - nearPoint.y, farPoint.z - nearPoint.z); + return laser_beam; +} + +// GL-free -- copied verbatim. +double distance_point_to_line(const Eigen::Vector3d& point, const LaserBeam& line) +{ + Eigen::Vector3d AP = point - line.position; + return (AP.cross(line.direction)).norm(); +} + +// GL-free -- copied verbatim. +void getClosestTrajectoryPoint(Session& session_, int x, int y, bool gcpPicking, int& picked_index) +{ + picked_index = -1; + + const auto laser_beam = GetLaserBeam(x, y); + double min_distance = std::numeric_limits::max(); + int index_i = -1; + int index_j = -1; + + for (int i = 0; i < session_.point_clouds_container.point_clouds.size(); i++) + { + for (int j = 0; j < session_.point_clouds_container.point_clouds[i].local_trajectory.size(); j++) + { + const auto& p = session_.point_clouds_container.point_clouds[i].local_trajectory[j].m_pose.translation(); + Eigen::Vector3d vp = session_.point_clouds_container.point_clouds[i].m_pose * p; + + double dist = distance_point_to_line(vp, laser_beam); + + if (dist < min_distance) + { + min_distance = dist; + index_i = i; + index_j = j; + + new_rotation_center.x() = static_cast(vp.x()); + new_rotation_center.y() = static_cast(vp.y()); + new_rotation_center.z() = static_cast(vp.z()); + + if (gcpPicking) + { + session_.ground_control_points.picking_mode_index_to_node_inner = index_i; + session_.ground_control_points.picking_mode_index_to_node_outer = index_j; + } + + picked_index = index_i; + } + } + } + + new_rotate_x = rotate_x; + new_rotate_y = rotate_y; + new_translate_x = -new_rotation_center.x(); + new_translate_y = -new_rotation_center.y(); + new_translate_z = translate_z; + camera_transition_active = true; +} + +// GL-free -- copied verbatim. +void setNewRotationCenter(int x, int y) +{ + const auto laser_beam = GetLaserBeam(x, y); + + RegistrationPlaneFeature::Plane pl; + + pl.a = 0; + pl.b = 0; + pl.c = 1; + pl.d = 0; + new_rotation_center = rayIntersection(laser_beam, pl).cast(); + + std::cout << "Setting new rotation center to:\n" << new_rotation_center << std::endl; + + new_rotate_x = rotate_x; + new_rotate_y = rotate_y; + new_translate_x = -new_rotation_center.x(); + new_translate_y = -new_rotation_center.y(); + new_translate_z = translate_z; + + camera_transition_active = true; +} + +// GL-free -- copied verbatim. +bool checkClHelp(int argc, char** argv) +{ + for (int i = 1; i < argc; ++i) + { + std::string arg(argv[i]); + + if (arg == "-h" || arg == "/h" || arg == "--help" || arg == "/?") + { + return true; + } + } + return false; +} + +// Was glOrtho + gluLookAt (folded into GL_PROJECTION, matching the +// original's call order -- gluLookAt ran before the GL_MODELVIEW switch +// below) -- rewritten as rlOrtho + rlMultMatrixf with the same lookAt +// matrix already computed via GLM for m_ortho_gizmo_view just above it. +void updateOrthoView() +{ + // still updating viewLocal for compass + viewLocal.rotate(Eigen::AngleAxisf((rotate_x + rotate_y) * DEG_TO_RAD, Eigen::Vector3f::UnitZ())); + + ImGuiIO& io = ImGui::GetIO(); + float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); + + rlOrtho( + -camera_ortho_xy_view_zoom, + camera_ortho_xy_view_zoom, + -camera_ortho_xy_view_zoom / ratio, + camera_ortho_xy_view_zoom / ratio, + -100000, + 100000); + + glm::mat4 proj = glm::orthoLH_ZO( + -camera_ortho_xy_view_zoom, + camera_ortho_xy_view_zoom, + -camera_ortho_xy_view_zoom / ratio, + camera_ortho_xy_view_zoom / ratio, + -100, + 100); + + std::copy(&proj[0][0], &proj[3][3], m_ortho_projection); + + Eigen::Vector3d v_eye_t(-camera_ortho_xy_view_shift_x, camera_ortho_xy_view_shift_y, camera_mode_ortho_z_center_h + 10); + Eigen::Vector3d v_center_t(-camera_ortho_xy_view_shift_x, camera_ortho_xy_view_shift_y, camera_mode_ortho_z_center_h); + Eigen::Vector3d v(0, 1, 0); + + TaitBryanPose pose_tb; + pose_tb.px = 0.0; + pose_tb.py = 0.0; + pose_tb.pz = 0.0; + pose_tb.om = 0.0; + pose_tb.fi = 0.0; + pose_tb.ka = -(rotate_x + rotate_y) * DEG_TO_RAD; + auto m = affine_matrix_from_pose_tait_bryan(pose_tb); + + Eigen::Vector3d v_t = m * v; + + glm::mat4 lookat = glm::lookAt( + glm::vec3(v_eye_t.x(), v_eye_t.y(), v_eye_t.z()), + glm::vec3(v_center_t.x(), v_center_t.y(), v_center_t.z()), + glm::vec3(v_t.x(), v_t.y(), v_t.z())); + std::copy(&lookat[0][0], &lookat[3][3], m_ortho_gizmo_view); + + rlMultMatrixf(&lookat[0][0]); + + rlMatrixMode(RL_MODELVIEW); + rlLoadIdentity(); +} + +// Restores rlgl's default 2D screen-space projection (matches what +// raylib's own EndMode3D() does), since this app drives the rlgl matrix +// stack manually (rlMatrixMode/rlFrustum/rlMultMatrixf in reshape()/ +// display() above) instead of using raylib's BeginMode3D/EndMode3D +// wrapper. Must be called after all 3D drawing and before any 2D drawing +// (the mini-compass, ImGui) each frame. +void end3DMatrixStack() +{ + rlDrawRenderBatchActive(); + rlMatrixMode(RL_PROJECTION); + rlLoadIdentity(); + // io.DisplaySize, not GetScreenWidth()/GetScreenHeight(): reshape() + // sets the actual GL viewport from io.DisplaySize (see display()'s call + // to it), and the two can differ under DPI scaling -- this has to + // match the viewport currently in effect, or 2D screen-space math done + // against it (e.g. renderLoopClosureLabels()'s world-to-pixel + // projection) lands off by the mismatch. + ImGuiIO& io = ImGui::GetIO(); + rlOrtho(0, io.DisplaySize.x, io.DisplaySize.y, 0, 0.0f, 1.0f); + rlMatrixMode(RL_MODELVIEW); + rlLoadIdentity(); + rlDisableDepthTest(); +} diff --git a/apps/multi_view_tls_registration/rl_utils.h b/apps/multi_view_tls_registration/rl_utils.h new file mode 100644 index 00000000..f483f3af --- /dev/null +++ b/apps/multi_view_tls_registration/rl_utils.h @@ -0,0 +1,167 @@ +#pragma once + +// raylib-based replacement for the app-agnostic camera/picking/mini-compass/ +// misc-ImGui-widget API that this app used to get from +// (core/src/utils.cpp). That file is shared by several other GLUT apps and +// can't be changed, and raylib's context here is OpenGL 3.3 core profile (no +// fixed-function pipeline), so this header/its .cpp are a from-scratch +// reimplementation of the same API surface -- same names, same call shape -- +// backed by rlgl's rl*() legacy-GL-emulation API (a software matrix stack + +// immediate-mode layer that mirrors gl*()'s call shape but works under core +// profile) instead of real gl*()/glu*()/glut*() calls. See rl_utils.cpp's +// top comment for the function-by-function porting notes. +// +// Deliberately not shared with any other app (unlike Core/utils.hpp): this +// is multi_view_tls_registration's own local header, analogous to how +// core/src/utils.cpp served the same role for the GLUT apps. + +#include + +#include +#include +#include + +#include + +#include +#include + +/////////////////////////////////////////////////////////////////////////////////// + +const float DEG_TO_RAD = M_PI / 180.0f; +const float RAD_TO_DEG = 180.0f / M_PI; + +const ImVec4 orangeBorder(1.0f, 0.5f, 0.0f, 1.0f); + +const std::string out_fn = "Output file name"; + +constexpr float ImGuiNumberWidth = 120.0f; +constexpr const char* omText = "Roll (left/right)"; +constexpr const char* fiText = "Pitch (up/down)"; +constexpr const char* kaText = "Yaw (turning left/right)"; +constexpr const char* xText = "Longitudinal (forward/backward)"; +constexpr const char* yText = "Lateral (left/right)"; +constexpr const char* zText = "Vertical (up/down)"; + +const uint32_t window_width = 1600; +const uint32_t window_height = 900; + +const float camera_transition_speed = 1.0f; // higher = faster + +enum CameraPreset +{ + CAMERA_FRONT, + CAMERA_BACK, + CAMERA_LEFT, + CAMERA_RIGHT, + CAMERA_TOP, + CAMERA_BOTTOM, + CAMERA_ISO, + CAMERA_RESET +}; + +enum ColorScheme +{ + CS_SOLID, // fixed color + CS_RANDOM, // random + CS_GRAD_INTENS, // gradient based on intensity + CS_GRAD_ELEV, // gradient based on elevation + CS_GRAD_DIST, // gradient based on distance from rotation center + CS_FOLLOW // valid for trajectory +}; + +/////////////////////////////////////////////////////////////////////////////////// + +extern int viewer_decimate_point_cloud; + +extern int mouse_old_x, mouse_old_y; +extern int mouse_buttons; +extern float mouse_sensitivity; + +extern bool is_ortho; +extern bool lock_z; +extern bool show_axes; +extern ImVec4 bg_color; +extern int point_size; + +extern bool info_gui; +extern bool compass_ruler; + +extern Eigen::Affine3f viewLocal; + +extern Eigen::Vector3f rotation_center; +extern float rotate_x, rotate_y; +extern float translate_x, translate_y, translate_z; + +extern double camera_ortho_xy_view_zoom; +extern double camera_ortho_xy_view_shift_x; +extern double camera_ortho_xy_view_shift_y; +extern double camera_mode_ortho_z_center_h; + +// Target camera state for smooth transitions +extern Eigen::Vector3f new_rotation_center; +extern float new_rotate_x; +extern float new_rotate_y; +extern float new_translate_x; +extern float new_translate_y; +extern float new_translate_z; + +// Transition timing +extern bool camera_transition_active; + +// Unlike the original (which probed GL_LINE_WIDTH_RANGE), rlgl's line width +// support is uniform enough here not to need a runtime check -- always true. +extern bool glLineWidthSupport; + +extern float m_ortho_projection[]; +extern float m_ortho_gizmo_view[]; + +struct ShortcutEntry +{ + std::string type; + std::string shortcut; + std::string description; +}; + +/////////////////////////////////////////////////////////////////////////////////// + +std::string truncPath(const std::string& fullPath); + +void wheel(int button, int dir, int x, int y); +void reshape(int w, int h); +void motion(int x, int y); +void ShowMainDockSpace(); + +void showAxes(); +void updateCameraTransition(); +void breakCameraTransition(); +void setCameraPreset(CameraPreset preset); +void camMenu(); +void view_kbd_shortcuts(); +void cor_window(); + +void ImGuiHyperlink(const char* url, ImVec4 color = ImVec4(0.2f, 0.4f, 0.8f, 1.0f)); +void ShowShortcutsTable(const std::vector appShortcuts); +void info_window(const std::vector& infoLines, const std::vector& appShortcuts); + +void drawMiniCompassWithRuler(); + +float distanceToPlane(const RegistrationPlaneFeature::Plane& plane, const Eigen::Vector3d& p); +Eigen::Vector3d rayIntersection(const LaserBeam& laser_beam, const RegistrationPlaneFeature::Plane& plane); +LaserBeam GetLaserBeam(int x, int y); +double distance_point_to_line(const Eigen::Vector3d& point, const LaserBeam& line); +void getClosestTrajectoryPoint(Session& session_, int x, int y, bool gcpPicking, int& picked_index); + +void setNewRotationCenter(int x, int y); + +bool checkClHelp(int argc, char** argv); + +void updateOrthoView(); + +// New (no equivalent in the original ): restores rlgl's +// default 2D screen-space projection (matches what raylib's own +// EndMode3D() does), since this app drives the rlgl matrix stack manually +// (rlMatrixMode/rlFrustum/rlMultMatrixf in reshape()/display()) instead of +// using raylib's BeginMode3D/EndMode3D wrapper. Must be called after all 3D +// drawing and before any 2D drawing (the mini-compass, ImGui) each frame. +void end3DMatrixStack(); From f63e2797c2c6a1d5bf02b24a7c6b9ef005744e58 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Sat, 25 Jul 2026 14:57:05 +0200 Subject: [PATCH 03/17] Apply clang-format to raylib port files Co-Authored-By: Claude Sonnet 5 Claude-Session: https://claude.ai/code/session_01XgxCVLnEEAftKKRvZHPUB9 --- .../multi_view_tls_registration_gui.cpp | 137 ++++++------ apps/multi_view_tls_registration/rl_utils.cpp | 26 +-- core/include/Core/raylib_render.hpp | 7 +- core/src/raylib_render.cpp | 199 +++++++++--------- 4 files changed, 195 insertions(+), 174 deletions(-) diff --git a/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp b/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp index 5cbba2b7..dcf11107 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 @@ -16,11 +16,11 @@ // shared with GLUT apps, so they can't be changed either) are replaced with // Core/raylib_render.hpp's ScanRenderer instead. See each local // function/global below for what it replaces. +#include "external/glad.h" #include "raylib.h" +#include "raymath.h" #include "rlImGui.h" #include "rlgl.h" -#include "raymath.h" -#include "external/glad.h" #include #include @@ -101,7 +101,8 @@ Matrix frame_mvp_3d{}; // by rl_utils.h, included above) -- panel functions earlier in this file // call some of these. void observationPickingRender(const ObservationPicking& observation_picking); -void renderLoopClosure(PointClouds& point_clouds_container, int index_loop_closure_source, int index_loop_closure_target, int before, int after); +void renderLoopClosure( + PointClouds& point_clouds_container, int index_loop_closure_source, int index_loop_closure_target, int before, int after); void renderLoopClosureLabels(PointClouds& point_clouds_container); void display(); void mouse(int glut_button, int state, int x, int y); @@ -283,7 +284,6 @@ int index_end = 0; ColorScheme csPointCloud = CS_GRAD_INTENS; ColorScheme csTrajectory = CS_SOLID; - // New (not in the original GLUT app): CS_GRAD_INTENS/CS_GRAD_ELEV/CS_GRAD_DIST // were declared in the original's ColorScheme enum but never actually wired // to a menu item or the renderer -- this hooks them up to scan_renderer's @@ -2344,7 +2344,7 @@ void observationPickingRender(const ObservationPicking& observation_picking) // point-mode immediate drawing), so point markers use small spheres. for (const auto& [key, value] : observation_picking.current_observation) { - DrawSphere(Vector3{static_cast(value.x()), static_cast(value.y()), static_cast(value.z())}, 0.05f, WHITE); + DrawSphere(Vector3{ static_cast(value.x()), static_cast(value.y()), static_cast(value.z()) }, 0.05f, WHITE); } rlColor3f(1.0f, 0.2f, 0.2f); @@ -2372,7 +2372,8 @@ void observationPickingRender(const ObservationPicking& observation_picking) // work against whatever rlgl projection/modelview is currently active, same // as this app's own manually-driven matrix stack -- no BeginMode3D needed). // Coordinates are used directly (Z-up, no remap -- see raylib_render.hpp). -void renderLoopClosure(PointClouds& point_clouds_container, int index_loop_closure_source, int index_loop_closure_target, int before, int after) +void renderLoopClosure( + PointClouds& point_clouds_container, int index_loop_closure_source, int index_loop_closure_target, int before, int after) { auto& pointClouds = point_clouds_container.point_clouds; if (pointClouds.empty()) @@ -2441,11 +2442,12 @@ void renderLoopClosure(PointClouds& point_clouds_container, int index_loop_closu Eigen::Affine3d delta = mTrg * pointClouds[idx].m_pose.inverse(); Color c = session.pose_graph_loop_closure.render_source_as_red_target_as_blue ? BLUE - : Color{static_cast(pointClouds[idx].render_color[0] * 255.f), - static_cast(pointClouds[idx].render_color[1] * 255.f), - 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); + : Color{ static_cast(pointClouds[idx].render_color[0] * 255.f), + static_cast(pointClouds[idx].render_color[1] * 255.f), + 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); } } } @@ -2453,9 +2455,14 @@ void renderLoopClosure(PointClouds& point_clouds_container, int index_loop_closu // The marked/visible-restricted range set above (source/target or // active-edge scans, at their normal stored pose). scan_renderer.draw( - pointClouds, static_cast(point_size), scanColorModeFromScheme(csPointCloud), static_cast(session_dims.z_min), - static_cast(session_dims.z_max), Eigen::Vector3d(rotation_center.x(), rotation_center.y(), rotation_center.z()), - static_cast(std::max({session_dims.length, session_dims.width, session_dims.height, 1.0})), 1); + pointClouds, + static_cast(point_size), + scanColorModeFromScheme(csPointCloud), + static_cast(session_dims.z_min), + static_cast(session_dims.z_max), + Eigen::Vector3d(rotation_center.x(), rotation_center.y(), rotation_center.z()), + static_cast(std::max({ session_dims.length, session_dims.width, session_dims.height, 1.0 })), + 1); // Pose-sequence trail across the whole session, as a chain of thick // green cylinders (sphere at each joint), sized relative to the current @@ -2465,9 +2472,9 @@ void renderLoopClosure(PointClouds& point_clouds_container, int index_loop_closu Vector3 prev{}; for (const auto& pc : pointClouds) { - Vector3 p = Vector3{ - static_cast(pc.m_pose.translation().x()), static_cast(pc.m_pose.translation().y()), - static_cast(pc.m_pose.translation().z())}; + Vector3 p = Vector3{ static_cast(pc.m_pose.translation().x()), + static_cast(pc.m_pose.translation().y()), + static_cast(pc.m_pose.translation().z()) }; DrawSphere(p, tubeRadius, GREEN); if (!first) { @@ -2491,14 +2498,14 @@ void renderLoopClosure(PointClouds& point_clouds_container, int index_loop_closu Eigen::Vector3d worldSrc = pointClouds[edge.index_from].m_pose.translation(); Eigen::Vector3d worldTrg = pointClouds[edge.index_to].m_pose.translation(); - Vector3 pSrc = Vector3{static_cast(worldSrc.x()), static_cast(worldSrc.y()), static_cast(worldSrc.z())}; - Vector3 pTrg = Vector3{static_cast(worldTrg.x()), static_cast(worldTrg.y()), static_cast(worldTrg.z())}; + Vector3 pSrc = Vector3{ static_cast(worldSrc.x()), static_cast(worldSrc.y()), static_cast(worldSrc.z()) }; + Vector3 pTrg = Vector3{ static_cast(worldTrg.x()), static_cast(worldTrg.y()), static_cast(worldTrg.z()) }; DrawCylinderEx(pSrc, pTrg, tubeRadius, tubeRadius, 8, c); Eigen::Vector3d mid = (worldSrc + worldTrg) * 0.5; Eigen::Vector3d midUp = mid + Eigen::Vector3d(0, 0, 10); - Vector3 pMid = Vector3{static_cast(mid.x()), static_cast(mid.y()), static_cast(mid.z())}; - Vector3 pMidUp = Vector3{static_cast(midUp.x()), static_cast(midUp.y()), static_cast(midUp.z())}; + Vector3 pMid = Vector3{ static_cast(mid.x()), static_cast(mid.y()), static_cast(mid.z()) }; + Vector3 pMidUp = Vector3{ static_cast(midUp.x()), static_cast(midUp.y()), static_cast(midUp.z()) }; DrawCylinderEx(pMid, pMidUp, tubeRadius, tubeRadius, 8, c); } } @@ -2513,26 +2520,26 @@ void renderLoopClosure(PointClouds& point_clouds_container, int index_loop_closu // end3DMatrixStack() (2D screen-space drawing). namespace { -// Plain DrawText at a point sitting exactly on top of a same-size, often -// same-color trajectory marker is easy to lose visually -- outlined in -// black and nudged up-right of the anchor so it reads clearly regardless -// of what's directly underneath. -void drawOutlinedText(const char* text, Vector2 anchor, int fontSize, Color color) -{ - int x = static_cast(anchor.x) + 6; - int y = static_cast(anchor.y) - fontSize - 6; - for (int dx = -1; dx <= 1; ++dx) + // Plain DrawText at a point sitting exactly on top of a same-size, often + // same-color trajectory marker is easy to lose visually -- outlined in + // black and nudged up-right of the anchor so it reads clearly regardless + // of what's directly underneath. + void drawOutlinedText(const char* text, Vector2 anchor, int fontSize, Color color) { - for (int dy = -1; dy <= 1; ++dy) + int x = static_cast(anchor.x) + 6; + int y = static_cast(anchor.y) - fontSize - 6; + for (int dx = -1; dx <= 1; ++dx) { - if (dx != 0 || dy != 0) + for (int dy = -1; dy <= 1; ++dy) { - DrawText(text, x + dx, y + dy, fontSize, BLACK); + if (dx != 0 || dy != 0) + { + DrawText(text, x + dx, y + dy, fontSize, BLACK); + } } } + DrawText(text, x, y, fontSize, color); } - DrawText(text, x, y, fontSize, color); -} } // namespace void renderLoopClosureLabels(PointClouds& point_clouds_container) @@ -2561,11 +2568,11 @@ void renderLoopClosureLabels(PointClouds& point_clouds_container) float clipW = m.m3 * x + m.m7 * y + m.m11 * z + m.m15; if (fabsf(clipW) < 1e-6f) { - return Vector2{-1000.f, -1000.f}; + return Vector2{ -1000.f, -1000.f }; } float ndcX = clipX / clipW; float ndcY = clipY / clipW; - return Vector2{(ndcX * 0.5f + 0.5f) * screenW, (1.0f - (ndcY * 0.5f + 0.5f)) * screenH}; + return Vector2{ (ndcX * 0.5f + 0.5f) * screenW, (1.0f - (ndcY * 0.5f + 0.5f)) * screenH }; }; for (size_t i = 0; i < pointClouds.size(); ++i) @@ -2605,8 +2612,7 @@ void display() ImGuiIO& io = ImGui::GetIO(); rlViewport(0, 0, (int)io.DisplaySize.x, (int)io.DisplaySize.y); - ClearBackground(ColorFromNormalized( - Vector4{bg_color.x * bg_color.w, bg_color.y * bg_color.w, bg_color.z * bg_color.w, bg_color.w})); + ClearBackground(ColorFromNormalized(Vector4{ bg_color.x * bg_color.w, bg_color.y * bg_color.w, bg_color.z * bg_color.w, bg_color.w })); rlEnableDepthTest(); rlMatrixMode(RL_PROJECTION); @@ -2709,7 +2715,10 @@ void display() { if (is_loop_closure_gui) renderLoopClosure( - session.point_clouds_container, index_loop_closure_source, index_loop_closure_target, num_edge_extended_before, + session.point_clouds_container, + index_loop_closure_source, + index_loop_closure_target, + num_edge_extended_before, num_edge_extended_after); } @@ -2784,12 +2793,12 @@ void display() // m0..m15 column-major numbering its names imply. Matrix projMat = rlGetMatrixProjection(); Matrix modelMat = rlGetMatrixModelview(); - float projection[16] = { - projMat.m0, projMat.m1, projMat.m2, projMat.m3, projMat.m4, projMat.m5, projMat.m6, projMat.m7, - projMat.m8, projMat.m9, projMat.m10, projMat.m11, projMat.m12, projMat.m13, projMat.m14, projMat.m15}; - float modelview[16] = { - modelMat.m0, modelMat.m1, modelMat.m2, modelMat.m3, modelMat.m4, modelMat.m5, modelMat.m6, modelMat.m7, - modelMat.m8, modelMat.m9, modelMat.m10, modelMat.m11, modelMat.m12, modelMat.m13, modelMat.m14, modelMat.m15}; + float projection[16] = { projMat.m0, projMat.m1, projMat.m2, projMat.m3, projMat.m4, projMat.m5, + projMat.m6, projMat.m7, projMat.m8, projMat.m9, projMat.m10, projMat.m11, + projMat.m12, projMat.m13, projMat.m14, projMat.m15 }; + float modelview[16] = { modelMat.m0, modelMat.m1, modelMat.m2, modelMat.m3, modelMat.m4, modelMat.m5, + modelMat.m6, modelMat.m7, modelMat.m8, modelMat.m9, modelMat.m10, modelMat.m11, + modelMat.m12, modelMat.m13, modelMat.m14, modelMat.m15 }; ImGuizmo::Manipulate( &modelview[0], @@ -2874,12 +2883,16 @@ void display() // sync via rebuildAll()/syncPoses() at session-load/pose-change // sites and each frame (see main()/loadSession() below). scan_renderer.draw( - session.point_clouds_container.point_clouds, static_cast(point_size), scanColorModeFromScheme(csPointCloud), - static_cast(session_dims.z_min), static_cast(session_dims.z_max), + session.point_clouds_container.point_clouds, + static_cast(point_size), + scanColorModeFromScheme(csPointCloud), + static_cast(session_dims.z_min), + static_cast(session_dims.z_max), Eigen::Vector3d(rotation_center.x(), rotation_center.y(), rotation_center.z()), - static_cast(std::max({session_dims.length, session_dims.width, session_dims.height, 1.0})), + static_cast(std::max({ session_dims.length, session_dims.width, session_dims.height, 1.0 })), viewer_decimate_point_cloud); - scan_renderer.drawTrajectories(session.point_clouds_container.point_clouds, 1, session.point_clouds_container.show_imu_to_lio_diff); + scan_renderer.drawTrajectories( + session.point_clouds_container.point_clouds, 1, session.point_clouds_container.show_imu_to_lio_diff); observationPickingRender(observation_picking); @@ -2902,8 +2915,14 @@ void display() p1 = session.point_clouds_container.point_clouds[key1].m_pose * value1; p2 = session.point_clouds_container.point_clouds[key2].m_pose * value2; } - DrawSphere(Vector3{static_cast(p1.x()), static_cast(p1.y()), static_cast(p1.z())}, 0.05f, GREEN); - DrawSphere(Vector3{static_cast(p2.x()), static_cast(p2.y()), static_cast(p2.z())}, 0.05f, GREEN); + DrawSphere( + Vector3{ static_cast(p1.x()), static_cast(p1.y()), static_cast(p1.z()) }, + 0.05f, + GREEN); + DrawSphere( + Vector3{ static_cast(p2.x()), static_cast(p2.y()), static_cast(p2.z()) }, + 0.05f, + GREEN); rlColor3f(1, 0, 0); rlBegin(RL_LINES); rlVertex3f(p1.x(), p1.y(), p1.z()); @@ -2946,7 +2965,7 @@ void display() for (auto p : picked_points) { - DrawSphere(Vector3{static_cast(p.x()), static_cast(p.y()), static_cast(p.z())}, 0.05f, MAGENTA); + DrawSphere(Vector3{ static_cast(p.x()), static_cast(p.y()), static_cast(p.z()) }, 0.05f, MAGENTA); } } else @@ -2962,12 +2981,12 @@ void display() { Matrix projMat = rlGetMatrixProjection(); Matrix modelMat = rlGetMatrixModelview(); - float projection[16] = { - projMat.m0, projMat.m1, projMat.m2, projMat.m3, projMat.m4, projMat.m5, projMat.m6, projMat.m7, - projMat.m8, projMat.m9, projMat.m10, projMat.m11, projMat.m12, projMat.m13, projMat.m14, projMat.m15}; - float modelview[16] = { - modelMat.m0, modelMat.m1, modelMat.m2, modelMat.m3, modelMat.m4, modelMat.m5, modelMat.m6, modelMat.m7, - modelMat.m8, modelMat.m9, modelMat.m10, modelMat.m11, modelMat.m12, modelMat.m13, modelMat.m14, modelMat.m15}; + float projection[16] = { projMat.m0, projMat.m1, projMat.m2, projMat.m3, projMat.m4, projMat.m5, + projMat.m6, projMat.m7, projMat.m8, projMat.m9, projMat.m10, projMat.m11, + projMat.m12, projMat.m13, projMat.m14, projMat.m15 }; + float modelview[16] = { modelMat.m0, modelMat.m1, modelMat.m2, modelMat.m3, modelMat.m4, modelMat.m5, + modelMat.m6, modelMat.m7, modelMat.m8, modelMat.m9, modelMat.m10, modelMat.m11, + modelMat.m12, modelMat.m13, modelMat.m14, modelMat.m15 }; ImGuizmo::Manipulate( &modelview[0], diff --git a/apps/multi_view_tls_registration/rl_utils.cpp b/apps/multi_view_tls_registration/rl_utils.cpp index 492ec99b..217c577e 100644 --- a/apps/multi_view_tls_registration/rl_utils.cpp +++ b/apps/multi_view_tls_registration/rl_utils.cpp @@ -1,9 +1,9 @@ #include "rl_utils.h" +#include "external/glad.h" #include "raylib.h" -#include "rlgl.h" #include "raymath.h" -#include "external/glad.h" +#include "rlgl.h" #include @@ -984,16 +984,16 @@ void drawMiniCompassWithRuler() Color color; }; const Axis axes[3] = { - {Eigen::Vector3f::UnitX(), "X (long.)", RED}, - {Eigen::Vector3f::UnitY(), "Y (lat.)", GREEN}, - {Eigen::Vector3f::UnitZ(), "Z (vert.)", BLUE}, + { Eigen::Vector3f::UnitX(), "X (long.)", RED }, + { Eigen::Vector3f::UnitY(), "Y (lat.)", GREEN }, + { Eigen::Vector3f::UnitZ(), "Z (vert.)", BLUE }, }; for (const auto& axis : axes) { Eigen::Vector3f eyeDir = viewLocal.rotation() * axis.dir; - Vector2 tip = {originX + eyeDir.x() * axisPixelLength, originY - eyeDir.y() * axisPixelLength}; - DrawLineEx(Vector2{originX, originY}, tip, 2.f, axis.color); + Vector2 tip = { originX + eyeDir.x() * axisPixelLength, originY - eyeDir.y() * axisPixelLength }; + DrawLineEx(Vector2{ originX, originY }, tip, 2.f, axis.color); DrawText(axis.label, (int)tip.x + 4, (int)tip.y - 6, 12, axis.color); } @@ -1017,10 +1017,10 @@ void drawMiniCompassWithRuler() snprintf(label, sizeof(label), "<1 [cm]"); float rulerY = originY + compassSize * 0.45f; - Color rulerColor = ColorFromNormalized(Vector4{1.0f - bg_color.x, 1.0f - bg_color.y, 1.0f - bg_color.z, 1.0f}); - DrawLineEx(Vector2{originX - 40.f, rulerY}, Vector2{originX + 40.f, rulerY}, 2.f, rulerColor); - DrawLineEx(Vector2{originX - 40.f, rulerY - 5.f}, Vector2{originX - 40.f, rulerY + 5.f}, 2.f, rulerColor); - DrawLineEx(Vector2{originX + 40.f, rulerY - 5.f}, Vector2{originX + 40.f, rulerY + 5.f}, 2.f, rulerColor); + Color rulerColor = ColorFromNormalized(Vector4{ 1.0f - bg_color.x, 1.0f - bg_color.y, 1.0f - bg_color.z, 1.0f }); + DrawLineEx(Vector2{ originX - 40.f, rulerY }, Vector2{ originX + 40.f, rulerY }, 2.f, rulerColor); + DrawLineEx(Vector2{ originX - 40.f, rulerY - 5.f }, Vector2{ originX - 40.f, rulerY + 5.f }, 2.f, rulerColor); + DrawLineEx(Vector2{ originX + 40.f, rulerY - 5.f }, Vector2{ originX + 40.f, rulerY + 5.f }, 2.f, rulerColor); DrawText(label, (int)originX - 20, (int)rulerY + 6, 14, rulerColor); } @@ -1075,8 +1075,8 @@ LaserBeam GetLaserBeam(int x, int y) Matrix matView = rlGetMatrixModelview(); Matrix matProj = rlGetMatrixProjection(); - Vector3 nearPoint = Vector3Unproject(Vector3{ndcX, ndcY, 0.0f}, matProj, matView); - Vector3 farPoint = Vector3Unproject(Vector3{ndcX, ndcY, 1.0f}, matProj, matView); + Vector3 nearPoint = Vector3Unproject(Vector3{ ndcX, ndcY, 0.0f }, matProj, matView); + Vector3 farPoint = Vector3Unproject(Vector3{ ndcX, ndcY, 1.0f }, matProj, matView); LaserBeam laser_beam; laser_beam.position = Eigen::Vector3d(nearPoint.x, nearPoint.y, nearPoint.z); diff --git a/core/include/Core/raylib_render.hpp b/core/include/Core/raylib_render.hpp index c0e540e4..2b940628 100644 --- a/core/include/Core/raylib_render.hpp +++ b/core/include/Core/raylib_render.hpp @@ -42,10 +42,10 @@ // per-scan flat colors. enum class ScanColorMode { - Flat, // pc.render_color, uniform per scan (the original app's only mode) + Flat, // pc.render_color, uniform per scan (the original app's only mode) Intensity, // jet colormap by normalized LAS/LAZ intensity Elevation, // jet colormap by world-space Z, normalized over [elevationMin, elevationMax] - Distance, // jet colormap by distance from distanceCenter, normalized over [0, distanceMax] + Distance, // jet colormap by distance from distanceCenter, normalized over [0, distanceMax] }; class ScanRenderer @@ -144,7 +144,8 @@ class ScanRenderer // 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. - void drawCachedWithTransform(size_t index, const Eigen::Affine3d& extraTransform, Color color, float pointSize, bool useIntensityColor) const; + void drawCachedWithTransform( + size_t index, const Eigen::Affine3d& extraTransform, Color color, float pointSize, bool useIntensityColor) const; private: struct CloudGPU diff --git a/core/src/raylib_render.cpp b/core/src/raylib_render.cpp index f37aa092..57f74d1c 100644 --- a/core/src/raylib_render.cpp +++ b/core/src/raylib_render.cpp @@ -1,9 +1,9 @@ #include #include +#include "external/glad.h" #include "raymath.h" #include "rlgl.h" -#include "external/glad.h" #include #include @@ -12,12 +12,12 @@ namespace { -// Points/poses are used in HDMapping's native Z-up world frame directly (no -// Y-up remap) -- see the header comment for why. -inline Vector3 toVec3(const Eigen::Vector3d& p) -{ - return Vector3{static_cast(p.x()), static_cast(p.y()), static_cast(p.z())}; -} + // Points/poses are used in HDMapping's native Z-up world frame directly (no + // Y-up remap) -- see the header comment for why. + inline Vector3 toVec3(const Eigen::Vector3d& p) + { + return Vector3{ static_cast(p.x()), static_cast(p.y()), static_cast(p.z()) }; + } } // namespace // ============================================================================ @@ -25,15 +25,15 @@ inline Vector3 toVec3(const Eigen::Vector3d& p) // ============================================================================ namespace { -// vertexIntensity is per-point LAS/LAZ intensity, normalized to [0,1] per -// scan at upload time (see ScanRenderer::rebuild). vertexPosition is -// already world-space (rebuild() applies pc.m_pose before uploading), so -// it doubles as the Elevation/Distance color modes' input with no extra -// per-vertex data needed. colorMode selects between the flat per-scan -// pointColor (0, the original app's only mode) and the ScanColorMode -// gradients (1/2/3), matching the enum's Intensity/Elevation/Distance -// ordering exactly (see ScanRenderer::draw()'s static_cast below). -const char* kPointVS = R"( + // vertexIntensity is per-point LAS/LAZ intensity, normalized to [0,1] per + // scan at upload time (see ScanRenderer::rebuild). vertexPosition is + // already world-space (rebuild() applies pc.m_pose before uploading), so + // it doubles as the Elevation/Distance color modes' input with no extra + // per-vertex data needed. colorMode selects between the flat per-scan + // pointColor (0, the original app's only mode) and the ScanColorMode + // gradients (1/2/3), matching the enum's Intensity/Elevation/Distance + // ordering exactly (see ScanRenderer::draw()'s static_cast below). + const char* kPointVS = R"( #version 330 in vec3 vertexPosition; in float vertexIntensity; @@ -50,7 +50,7 @@ void main() } )"; -const char* kPointFS = R"( + const char* kPointFS = R"( #version 330 uniform vec4 pointColor; uniform int colorMode; @@ -94,8 +94,8 @@ void main() } )"; -// Bytes per vertex in the uploaded buffer (xyz position + intensity). -constexpr int kVertexStride = 4 * sizeof(float); + // Bytes per vertex in the uploaded buffer (xyz position + intensity). + constexpr int kVertexStride = 4 * sizeof(float); } // namespace ScanRenderer::~ScanRenderer() @@ -287,8 +287,9 @@ void ScanRenderer::draw( rlSetUniform(locPointSize_, &pointSize, RL_SHADER_UNIFORM_FLOAT, 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())}; + 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); @@ -382,37 +383,38 @@ void ScanRenderer::clearMarks() namespace { -// Converts an Eigen::Affine3d (in HDMapping's native Z-up world frame, used -// as-is -- see the header comment) to the equivalent raylib Matrix. No -// coordinate remap needed (unlike an earlier version of this function), -// since points/poses are no longer converted to a separate Y-up space. -Matrix toRaylibMatrix(const Eigen::Affine3d& t) -{ - const Eigen::Matrix3d& r = t.linear(); - const Eigen::Vector3d& tr = t.translation(); - - Matrix m{}; - m.m0 = static_cast(r(0, 0)); - m.m4 = static_cast(r(0, 1)); - m.m8 = static_cast(r(0, 2)); - m.m12 = static_cast(tr.x()); - m.m1 = static_cast(r(1, 0)); - m.m5 = static_cast(r(1, 1)); - m.m9 = static_cast(r(1, 2)); - m.m13 = static_cast(tr.y()); - m.m2 = static_cast(r(2, 0)); - m.m6 = static_cast(r(2, 1)); - m.m10 = static_cast(r(2, 2)); - m.m14 = static_cast(tr.z()); - m.m3 = 0.f; - m.m7 = 0.f; - m.m11 = 0.f; - m.m15 = 1.f; - return m; -} + // Converts an Eigen::Affine3d (in HDMapping's native Z-up world frame, used + // as-is -- see the header comment) to the equivalent raylib Matrix. No + // coordinate remap needed (unlike an earlier version of this function), + // since points/poses are no longer converted to a separate Y-up space. + Matrix toRaylibMatrix(const Eigen::Affine3d& t) + { + const Eigen::Matrix3d& r = t.linear(); + const Eigen::Vector3d& tr = t.translation(); + + Matrix m{}; + m.m0 = static_cast(r(0, 0)); + m.m4 = static_cast(r(0, 1)); + m.m8 = static_cast(r(0, 2)); + m.m12 = static_cast(tr.x()); + m.m1 = static_cast(r(1, 0)); + m.m5 = static_cast(r(1, 1)); + m.m9 = static_cast(r(1, 2)); + m.m13 = static_cast(tr.y()); + m.m2 = static_cast(r(2, 0)); + m.m6 = static_cast(r(2, 1)); + m.m10 = static_cast(r(2, 2)); + m.m14 = static_cast(tr.z()); + m.m3 = 0.f; + m.m7 = 0.f; + m.m11 = 0.f; + m.m15 = 1.f; + return m; + } } // namespace -void ScanRenderer::drawCachedWithTransform(size_t index, const Eigen::Affine3d& extraTransform, Color color, float pointSize, bool useIntensityColor) const +void ScanRenderer::drawCachedWithTransform( + size_t index, const Eigen::Affine3d& extraTransform, Color color, float pointSize, bool useIntensityColor) const { if (!shaderValid_ || index >= clouds_.size()) { @@ -433,7 +435,7 @@ void ScanRenderer::drawCachedWithTransform(size_t index, const Eigen::Affine3d& // transform to the cached points first, then the normal view/projection. 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}; + float colorF[4] = { color.r / 255.f, color.g / 255.f, color.b / 255.f, color.a / 255.f }; int colorMode = useIntensityColor ? 1 : 0; rlEnableShader(shader_.id); @@ -460,52 +462,52 @@ void ScanRenderer::drawCachedWithTransform(size_t index, const Eigen::Affine3d& namespace { -constexpr double RAD_TO_DEG = 180.0 / M_PI; + constexpr double RAD_TO_DEG = 180.0 / M_PI; -// Ported from the "fuse_inclination_from_IMU" / "fixed_om && fixed_fi" / -// "show_IMU" / "show_pose" quad-and-cross markers in -// core/src/point_cloud.cpp's PointCloud::render(). -void drawSquareOutline(const Eigen::Affine3d& m, double half, Color color) -{ - Eigen::Vector3d a1 = m * Eigen::Vector3d(-half, -half, 0); - Eigen::Vector3d a2 = m * Eigen::Vector3d(half, -half, 0); - Eigen::Vector3d a3 = m * Eigen::Vector3d(half, half, 0); - Eigen::Vector3d a4 = m * Eigen::Vector3d(-half, half, 0); - - Vector3 p1 = toVec3(a1); - Vector3 p2 = toVec3(a2); - Vector3 p3 = toVec3(a3); - Vector3 p4 = toVec3(a4); - - DrawLine3D(p1, p2, color); - DrawLine3D(p2, p3, color); - DrawLine3D(p3, p4, color); - DrawLine3D(p4, p1, color); -} + // Ported from the "fuse_inclination_from_IMU" / "fixed_om && fixed_fi" / + // "show_IMU" / "show_pose" quad-and-cross markers in + // core/src/point_cloud.cpp's PointCloud::render(). + void drawSquareOutline(const Eigen::Affine3d& m, double half, Color color) + { + Eigen::Vector3d a1 = m * Eigen::Vector3d(-half, -half, 0); + Eigen::Vector3d a2 = m * Eigen::Vector3d(half, -half, 0); + Eigen::Vector3d a3 = m * Eigen::Vector3d(half, half, 0); + Eigen::Vector3d a4 = m * Eigen::Vector3d(-half, half, 0); + + Vector3 p1 = toVec3(a1); + Vector3 p2 = toVec3(a2); + Vector3 p3 = toVec3(a3); + Vector3 p4 = toVec3(a4); + + DrawLine3D(p1, p2, color); + DrawLine3D(p2, p3, color); + DrawLine3D(p3, p4, color); + DrawLine3D(p4, p1, color); + } -void drawOrientationCross(const Eigen::Affine3d& m) -{ - Vector3 origin = toVec3(m.translation()); - DrawLine3D(origin, toVec3(m.translation() + m.linear().col(0)), RED); - DrawLine3D(origin, toVec3(m.translation() + m.linear().col(1)), GREEN); - DrawLine3D(origin, toVec3(m.translation() + m.linear().col(2)), BLUE); -} + void drawOrientationCross(const Eigen::Affine3d& m) + { + Vector3 origin = toVec3(m.translation()); + DrawLine3D(origin, toVec3(m.translation() + m.linear().col(0)), RED); + DrawLine3D(origin, toVec3(m.translation() + m.linear().col(1)), GREEN); + DrawLine3D(origin, toVec3(m.translation() + m.linear().col(2)), BLUE); + } -// The IMU-fused orientation (used by both the fuse-inclination quad marker -// and show_IMU) mirrors the local pose's translation with orientation taken -// from the first local_trajectory node's raw IMU om/fi/ka instead of the -// LIO-optimized pose. -Eigen::Affine3d imuOrientationAtPose(const PointCloud& pc) -{ - TaitBryanPose tb; - tb.px = pc.m_pose(0, 3); - tb.py = pc.m_pose(1, 3); - tb.pz = pc.m_pose(2, 3); - tb.om = pc.local_trajectory[0].imu_om_fi_ka.x(); - tb.fi = pc.local_trajectory[0].imu_om_fi_ka.y(); - tb.ka = pc.local_trajectory[0].imu_om_fi_ka.z(); - return affine_matrix_from_pose_tait_bryan(tb); -} + // The IMU-fused orientation (used by both the fuse-inclination quad marker + // and show_IMU) mirrors the local pose's translation with orientation taken + // from the first local_trajectory node's raw IMU om/fi/ka instead of the + // LIO-optimized pose. + Eigen::Affine3d imuOrientationAtPose(const PointCloud& pc) + { + TaitBryanPose tb; + tb.px = pc.m_pose(0, 3); + tb.py = pc.m_pose(1, 3); + tb.pz = pc.m_pose(2, 3); + tb.om = pc.local_trajectory[0].imu_om_fi_ka.x(); + tb.fi = pc.local_trajectory[0].imu_om_fi_ka.y(); + tb.ka = pc.local_trajectory[0].imu_om_fi_ka.z(); + return affine_matrix_from_pose_tait_bryan(tb); + } } // namespace void ScanRenderer::drawTrajectories(const std::vector& pointClouds, int reduceRenderedTrajectory, bool visibleImuDiff) const @@ -534,11 +536,10 @@ void ScanRenderer::drawTrajectories(const std::vector& pointClouds, if (pc.line_width > 0 && pc.local_trajectory.size() >= 2) { - Color c = Color{ - static_cast(pc.traj_color[0] * 255.f), - static_cast(pc.traj_color[1] * 255.f), - static_cast(pc.traj_color[2] * 255.f), - 255}; + Color c = Color{ static_cast(pc.traj_color[0] * 255.f), + static_cast(pc.traj_color[1] * 255.f), + static_cast(pc.traj_color[2] * 255.f), + 255 }; Vector3 prev = toVec3((pc.m_pose * pc.local_trajectory[0].m_pose).translation()); for (size_t i = stride; i < pc.local_trajectory.size(); i += stride) From ce68d62eef99b9a50a6a40fe0d027efb7338ebe0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Sat, 25 Jul 2026 15:15:36 +0200 Subject: [PATCH 04/17] ci: install GLFW's X11 dev headers for raylib's bundled GLFW build raylib (new dependency, fetched via cmake/raylib.cmake for the multi_view_tls_registration raylib port) builds GLFW from source, which needs libxrandr-dev/libxinerama-dev/libxcursor-dev/libxext-dev to configure its X11 backend -- CI's minimal images don't have these by default, unlike this dev machine. Co-Authored-By: Claude Sonnet 5 Claude-Session: https://claude.ai/code/session_01XgxCVLnEEAftKKRvZHPUB9 --- .github/workflows/cmake-linux-deb.yml | 2 +- .github/workflows/cmake-linux.yml | 2 +- .github/workflows/python-bindings-linux.yml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/cmake-linux-deb.yml b/.github/workflows/cmake-linux-deb.yml index 4727ed79..4a8ff14b 100644 --- a/.github/workflows/cmake-linux-deb.yml +++ b/.github/workflows/cmake-linux-deb.yml @@ -23,7 +23,7 @@ jobs: - name: Load repository cache run: sudo apt-get update --allow-releaseinfo-change - name: Install Libs - run: sudo apt-get install -y --no-install-recommends libeigen3-dev libglew-dev freeglut3-dev libx11-dev libxi-dev libtbb-dev liblaszip-dev libopencv-dev libproj-dev + run: sudo apt-get install -y --no-install-recommends libeigen3-dev libglew-dev freeglut3-dev libx11-dev libxi-dev libxrandr-dev libxinerama-dev libxcursor-dev libxext-dev libtbb-dev liblaszip-dev libopencv-dev libproj-dev - uses: actions/checkout@v4 with: diff --git a/.github/workflows/cmake-linux.yml b/.github/workflows/cmake-linux.yml index b584782e..d2f0900b 100644 --- a/.github/workflows/cmake-linux.yml +++ b/.github/workflows/cmake-linux.yml @@ -23,7 +23,7 @@ jobs: - name: Load repository cache run: sudo apt-get update --allow-releaseinfo-change - name: Install Libs - run: sudo apt-get install -y --no-install-recommends libx11-dev libxi-dev libtbb-dev libegl1-mesa-dev libglu1-mesa-dev libopencv-dev libproj-dev + run: sudo apt-get install -y --no-install-recommends libx11-dev libxi-dev libxrandr-dev libxinerama-dev libxcursor-dev libxext-dev libtbb-dev libegl1-mesa-dev libglu1-mesa-dev libopencv-dev libproj-dev - uses: actions/checkout@v4 with: diff --git a/.github/workflows/python-bindings-linux.yml b/.github/workflows/python-bindings-linux.yml index e8b2065a..c68bdf46 100644 --- a/.github/workflows/python-bindings-linux.yml +++ b/.github/workflows/python-bindings-linux.yml @@ -21,7 +21,7 @@ jobs: run: sudo apt-get update --allow-releaseinfo-change - name: Install system libraries - run: sudo apt-get install -y --no-install-recommends libx11-dev libxi-dev libtbb-dev libegl1-mesa-dev libglu1-mesa-dev libopencv-dev libproj-dev + run: sudo apt-get install -y --no-install-recommends libx11-dev libxi-dev libxrandr-dev libxinerama-dev libxcursor-dev libxext-dev libtbb-dev libegl1-mesa-dev libglu1-mesa-dev libopencv-dev libproj-dev - uses: actions/checkout@v4 with: From 0eccf1577d88c2c8700a5445411769de1b9cc4e2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Sat, 25 Jul 2026 16:28:24 +0200 Subject: [PATCH 05/17] Fix Windows build: raylib.h/windows.h symbol collisions raylib.h's DrawText/CloseWindow/ShowCursor collide with windows.h's identically-named GDI macro and WinUser functions once both headers land in the same translation unit (something this app never did before, since the original never included raylib.h). Fixed with NOGDI/NOUSER before in rl_utils.cpp (the only file that still needs windows.h, for ShellExecuteA), pinned with clang-format off/on since alphabetical reordering silently reintroduces the windows.h-before-shellapi.h ordering bug this also needs to avoid. gui.cpp's own include is dropped entirely -- nothing in it actually used anything from windows.h. Co-Authored-By: Claude Sonnet 5 Claude-Session: https://claude.ai/code/session_01XgxCVLnEEAftKKRvZHPUB9 --- .../multi_view_tls_registration_gui.cpp | 7 +++++-- apps/multi_view_tls_registration/rl_utils.cpp | 21 +++++++++++++++++-- 2 files changed, 24 insertions(+), 4 deletions(-) diff --git a/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp b/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp index dcf11107..029c9cf2 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 @@ -68,9 +68,12 @@ #include #ifdef _WIN32 +// Just numeric resource IDs (IDI_ICON1 etc.) -- no windows.h needed to parse +// it, and this file makes no direct WinAPI calls, so windows.h isn't +// included here (unlike the original): raylib.h's DrawText/CloseWindow/ +// ShowCursor collide with identically-named windows.h macros/functions +// (see rl_utils.cpp's comment, which does need windows.h, for the fix). #include "resource.h" -#include - #endif // Camera/picking/mini-compass/misc-ImGui-widget API this app used to get diff --git a/apps/multi_view_tls_registration/rl_utils.cpp b/apps/multi_view_tls_registration/rl_utils.cpp index 217c577e..9d78ac11 100644 --- a/apps/multi_view_tls_registration/rl_utils.cpp +++ b/apps/multi_view_tls_registration/rl_utils.cpp @@ -22,8 +22,25 @@ #include #ifdef _WIN32 -#include +// NOGDI/NOUSER: windows.h's wingdi.h/winuser.h #define (or, for CloseWindow/ +// ShowCursor, directly declare) identifiers that collide with raylib.h's +// own DrawText/CloseWindow/ShowCursor -- without these, windows.h wins and +// every call to raylib's DrawText() above silently becomes a call to the +// Win32 GDI DrawTextA() instead, which doesn't compile against raylib's +// arguments. NOUSER also strips SW_SHOWNORMAL (a windows.h macro), so +// ImGuiHyperlink's ShellExecuteA call below uses its literal value (1, a +// stable, decades-unchanged Win32 constant) instead. +// +// windows.h must come before shellapi.h -- shellapi.h depends on macros/ +// types windows.h defines, and this file (unlike core/src/utils.cpp, which +// gets windows.h transitively via its precompiled header before this same +// ordering matters) has nothing else pulling windows.h in first. +#define NOGDI +#define NOUSER +// clang-format off #include +#include +// clang-format on #endif /////////////////////////////////////////////////////////////////////////////////// @@ -752,7 +769,7 @@ void ImGuiHyperlink(const char* url, ImVec4 color) if (ImGui::IsItemClicked()) { #ifdef _WIN32 - ShellExecuteA(0, "open", url, 0, 0, SW_SHOWNORMAL); + ShellExecuteA(0, "open", url, 0, 0, 1 /* SW_SHOWNORMAL, unavailable under NOUSER -- see this file's top comment */); #elif __APPLE__ std::string cmd = std::string("open ") + url; system(cmd.c_str()); From d9b53bd5dec6db99159d2128e83aaea5e870eb61 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Sat, 25 Jul 2026 17:30:04 +0200 Subject: [PATCH 06/17] Fix Windows build: raylib.h/windows.h collision reaches gui.cpp too The previous fix only covered rl_utils.cpp. gui.cpp itself pulls in real windows.h transitively via portable-file-dialogs.h (needed for SendMessage/DispatchMessage/MessageBoxW/GetActiveWindow, so NOGDI/NOUSER isn't an option here like it was in rl_utils.cpp), which collides with raylib.h's CloseWindow(void)/ShowCursor(void) declarations -- a hard extern-"C"-redeclaration error regardless of include order, not just a macro issue. Renamed raylib's versions via macro just around its own #include, and #undef DrawText after the last include (windows.h #defines it to DrawTextA) so plain DrawText/CloseWindow calls later in the file keep meaning raylib's functions. Co-Authored-By: Claude Sonnet 5 Claude-Session: https://claude.ai/code/session_01XgxCVLnEEAftKKRvZHPUB9 --- .../multi_view_tls_registration_gui.cpp | 44 +++++++++++++++++-- 1 file changed, 40 insertions(+), 4 deletions(-) diff --git a/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp b/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp index 029c9cf2..88411dd7 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 @@ -16,11 +16,32 @@ // shared with GLUT apps, so they can't be changed either) are replaced with // Core/raylib_render.hpp's ScanRenderer instead. See each local // function/global below for what it replaces. +#ifdef _WIN32 +// raylib.h declares CloseWindow(void)/ShowCursor(void), which collide with +// windows.h's CloseWindow(HWND)/ShowCursor(BOOL) once portable-file-dialogs.h +// (included below) pulls in real windows.h later in this translation unit +// -- since both are extern "C" declarations of the same name with different +// signatures, this is a hard error regardless of include order, not just a +// macro-textual issue. Unlike rl_utils.cpp (which only needs ShellExecuteA, +// so NOGDI/NOUSER before its own windows.h include is enough), this file +// also needs portable-file-dialogs.h's real winuser.h functionality +// (SendMessage/DispatchMessage/MessageBoxW/GetActiveWindow), so NOUSER +// isn't an option here -- raylib's versions are renamed instead, just +// around this include, so windows.h's later declarations don't collide. +// The one call site (CloseWindow() in main()) uses the renamed identifier +// on Windows; ShowCursor() isn't called anywhere in this file. +#define CloseWindow CloseWindow_rl +#define ShowCursor ShowCursor_rl +#endif #include "external/glad.h" #include "raylib.h" #include "raymath.h" #include "rlImGui.h" #include "rlgl.h" +#ifdef _WIN32 +#undef CloseWindow +#undef ShowCursor +#endif #include #include @@ -69,10 +90,11 @@ #include #ifdef _WIN32 // Just numeric resource IDs (IDI_ICON1 etc.) -- no windows.h needed to parse -// it, and this file makes no direct WinAPI calls, so windows.h isn't -// included here (unlike the original): raylib.h's DrawText/CloseWindow/ -// ShowCursor collide with identically-named windows.h macros/functions -// (see rl_utils.cpp's comment, which does need windows.h, for the fix). +// it, and this file makes no direct WinAPI calls itself, so windows.h isn't +// included directly here (it still arrives transitively, via +// portable-file-dialogs.h above -- see the CloseWindow/ShowCursor rename +// near the top of this file, and the #undef DrawText below, for how that's +// handled). #include "resource.h" #endif @@ -81,6 +103,16 @@ // local header instead). #include "rl_utils.h" +#ifdef _WIN32 +// windows.h (pulled in transitively above, via portable-file-dialogs.h) +// #defines DrawText as DrawTextA -- undefining it restores plain DrawText +// calls below to mean raylib's function again (its declaration, parsed +// before windows.h's macro existed, is unaffected either way; this only +// affects how later *source text* that writes the identifier "DrawText" +// gets preprocessed). +#undef DrawText +#endif + /////////////////////////////////////////////////////////////////////////////////// // GPU (rlgl-based) point cloud renderer -- replaces core's legacy-GL @@ -4749,7 +4781,11 @@ int main(int argc, char* argv[]) } rlImGuiShutdown(); +#ifdef _WIN32 + CloseWindow_rl(); // raylib's CloseWindow(), renamed -- see the include block's comment +#else CloseWindow(); +#endif } catch (const std::bad_alloc& e) { spdlog::error("System is out of memory : {}", e.what()); From ec19b2c2c28ad273764c5e59244a2b7859fc7a96 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Sat, 25 Jul 2026 18:18:51 +0200 Subject: [PATCH 07/17] Fix Windows build: rename windows.h's symbols, not raylib's The previous attempt renamed raylib.h's CloseWindow/ShowCursor around its own #include, which avoided the compile-time redeclaration clash but broke the link: the compiled raylib library still only exports the symbol under its real name, so the renamed *declaration* just became an unresolved external symbol. Flipped it around: rename windows.h's versions instead (scoped to just the portable-file-dialogs.h include, which is what actually pulls windows.h in) -- safe since portable-file-dialogs.h itself never calls CloseWindow/ShowCursor, so nothing there breaks, and raylib's real CloseWindow/ShowCursor stay callable normally everywhere else in the file, matching what's actually in the library. Co-Authored-By: Claude Sonnet 5 Claude-Session: https://claude.ai/code/session_01XgxCVLnEEAftKKRvZHPUB9 --- .../multi_view_tls_registration_gui.cpp | 47 +++++++++---------- 1 file changed, 22 insertions(+), 25 deletions(-) diff --git a/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp b/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp index 88411dd7..a016959a 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 @@ -16,32 +16,11 @@ // shared with GLUT apps, so they can't be changed either) are replaced with // Core/raylib_render.hpp's ScanRenderer instead. See each local // function/global below for what it replaces. -#ifdef _WIN32 -// raylib.h declares CloseWindow(void)/ShowCursor(void), which collide with -// windows.h's CloseWindow(HWND)/ShowCursor(BOOL) once portable-file-dialogs.h -// (included below) pulls in real windows.h later in this translation unit -// -- since both are extern "C" declarations of the same name with different -// signatures, this is a hard error regardless of include order, not just a -// macro-textual issue. Unlike rl_utils.cpp (which only needs ShellExecuteA, -// so NOGDI/NOUSER before its own windows.h include is enough), this file -// also needs portable-file-dialogs.h's real winuser.h functionality -// (SendMessage/DispatchMessage/MessageBoxW/GetActiveWindow), so NOUSER -// isn't an option here -- raylib's versions are renamed instead, just -// around this include, so windows.h's later declarations don't collide. -// The one call site (CloseWindow() in main()) uses the renamed identifier -// on Windows; ShowCursor() isn't called anywhere in this file. -#define CloseWindow CloseWindow_rl -#define ShowCursor ShowCursor_rl -#endif #include "external/glad.h" #include "raylib.h" #include "raymath.h" #include "rlImGui.h" #include "rlgl.h" -#ifdef _WIN32 -#undef CloseWindow -#undef ShowCursor -#endif #include #include @@ -70,7 +49,29 @@ #include #include +#ifdef _WIN32 +// portable-file-dialogs.h pulls in real windows.h, whose CloseWindow(HWND)/ +// ShowCursor(BOOL) collide with raylib.h's already-declared CloseWindow(void)/ +// ShowCursor(void) (both extern "C", so this is a hard redeclaration error, +// not just a macro-textual one -- and unlike rl_utils.cpp, which only needs +// ShellExecuteA, this file also needs portable-file-dialogs.h's own +// winuser.h functionality, i.e. SendMessage/DispatchMessage/MessageBoxW/ +// GetActiveWindow, so suppressing all of winuser.h via NOUSER isn't an +// option here). Renaming raylib's versions doesn't work either: the +// compiled raylib library still only exports the symbol under its real +// name, so a renamed *declaration* just becomes an unresolved symbol at +// link time. windows.h's versions are renamed instead -- safe because +// portable-file-dialogs.h itself never calls CloseWindow/ShowCursor +// (verified: neither name appears in its source) -- leaving raylib's +// real CloseWindow/ShowCursor callable normally everywhere in this file. +#define CloseWindow CloseWindow_win32 +#define ShowCursor ShowCursor_win32 +#endif #include +#ifdef _WIN32 +#undef CloseWindow +#undef ShowCursor +#endif #include @@ -4781,11 +4782,7 @@ int main(int argc, char* argv[]) } rlImGuiShutdown(); -#ifdef _WIN32 - CloseWindow_rl(); // raylib's CloseWindow(), renamed -- see the include block's comment -#else CloseWindow(); -#endif } catch (const std::bad_alloc& e) { spdlog::error("System is out of memory : {}", e.what()); From 39ffca5d95ee602eb9c00b051af62c9172420372 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Sat, 25 Jul 2026 23:57:35 +0200 Subject: [PATCH 08/17] Keep the legacy GLUT step2 app alongside the raylib port Stakeholder request: apps/multi_view_tls_registration_legacy is a full, independent copy of step2 as it was before the GLUT-to-raylib port (restored from the pre-port commit) -- new target multi_view_tls_registration_step_2_legacy, built side by side with the raylib-based multi_view_tls_registration_step_2. Not sharing translation units between the two, so they can diverge or be retired independently. Co-Authored-By: Claude Sonnet 5 Claude-Session: https://claude.ai/code/session_01XgxCVLnEEAftKKRvZHPUB9 --- CMakeLists.txt | 1 + .../CMakeLists.txt | 80 + .../icon.ico | Bin 0 -> 22282 bytes .../multi_view_tls_registration.cpp | 878 ++++ .../multi_view_tls_registration.h | 181 + .../multi_view_tls_registration_gui.cpp | 4243 +++++++++++++++++ .../perform_experiment.cpp | 2428 ++++++++++ .../resource.h | 17 + .../resource.rc | 49 + 9 files changed, 7877 insertions(+) create mode 100644 apps/multi_view_tls_registration_legacy/CMakeLists.txt create mode 100644 apps/multi_view_tls_registration_legacy/icon.ico create mode 100644 apps/multi_view_tls_registration_legacy/multi_view_tls_registration.cpp create mode 100644 apps/multi_view_tls_registration_legacy/multi_view_tls_registration.h create mode 100644 apps/multi_view_tls_registration_legacy/multi_view_tls_registration_gui.cpp create mode 100644 apps/multi_view_tls_registration_legacy/perform_experiment.cpp create mode 100644 apps/multi_view_tls_registration_legacy/resource.h create mode 100644 apps/multi_view_tls_registration_legacy/resource.rc diff --git a/CMakeLists.txt b/CMakeLists.txt index 169448ee..4a19ba7f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -117,6 +117,7 @@ add_subdirectory(apps/lidar_odometry_step_1) add_subdirectory(apps/manual_color) add_subdirectory(apps/multi_session_registration) add_subdirectory(apps/multi_view_tls_registration) +add_subdirectory(apps/multi_view_tls_registration_legacy) add_subdirectory(apps/split_multi_livox) add_subdirectory(apps/precision_forestry_tools) add_subdirectory(apps/mandeye_raw_data_viewer) diff --git a/apps/multi_view_tls_registration_legacy/CMakeLists.txt b/apps/multi_view_tls_registration_legacy/CMakeLists.txt new file mode 100644 index 00000000..57549a5b --- /dev/null +++ b/apps/multi_view_tls_registration_legacy/CMakeLists.txt @@ -0,0 +1,80 @@ +cmake_minimum_required(VERSION 4.0.0) + +# Legacy GLUT + legacy immediate-mode OpenGL build of step2, kept side by +# side with apps/multi_view_tls_registration (raylib-based since it was +# ported off GLUT) at a stakeholder's request. This is a full, independent +# copy of the app as it was before that port -- not sharing translation +# units with apps/multi_view_tls_registration -- so the two can diverge or +# be retired independently. +project(multi_view_tls_registration_step_2_legacy) + +# Source files +set(SOURCES + multi_view_tls_registration.cpp perform_experiment.cpp + multi_view_tls_registration_gui.cpp multi_view_tls_registration.h + ../lidar_odometry_step_1/lidar_odometry_utils.cpp + "../../core/src/utils.cpp" +) + +# Windows: add resource file +if(WIN32) + list(APPEND SOURCES "resource.rc") +endif() + +add_executable( + multi_view_tls_registration_step_2_legacy ${SOURCES} + ) + +target_compile_definitions(multi_view_tls_registration_step_2_legacy PRIVATE -DWITH_GUI=1) + +target_include_directories( + multi_view_tls_registration_step_2_legacy + PRIVATE include + ${REPOSITORY_DIRECTORY}/core/include + ${THIRDPARTY_DIRECTORY}/glm + ${EIGEN3_INCLUDE_DIR} + ${THIRDPARTY_DIRECTORY} + ${THIRDPARTY_DIRECTORY}/imgui + ${THIRDPARTY_DIRECTORY}/imgui/backends + ${THIRDPARTY_DIRECTORY}/ImGuizmo + ${THIRDPARTY_DIRECTORY}/glew-cmake/include + ${FREEGLUT_INCLUDE_DIR} + ${THIRDPARTY_DIRECTORY}/json/include + ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master + ${LASZIP_INCLUDE_DIR}/LASzip/include + ${THIRDPARTY_DIRECTORY}/observation_equations/codes + ${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp + ${EXTERNAL_LIBRARIES_DIRECTORY}/include) + + target_link_libraries( + multi_view_tls_registration_step_2_legacy + PRIVATE + WGS84toCartesian + wgs84_do_puwg92 + unordered_dense::unordered_dense + spdlog::spdlog + OpenGL::GLU + ${FREEGLUT_LIBRARY} + ${OPENGL_gl_LIBRARY} + ${PLATFORM_LASZIP_LIB} + ${PLATFORM_MISCELLANEOUS_LIBS} + ${CORE_LIBRARIES} + ${GUI_LIBRARIES} + PROJ::proj) + +if(WIN32) + add_custom_command( + TARGET multi_view_tls_registration_step_2_legacy + POST_BUILD + COMMAND + ${CMAKE_COMMAND} -E copy + $ + $ + COMMAND_EXPAND_LISTS) +endif() + +if (MSVC) + target_compile_options(multi_view_tls_registration_step_2_legacy PRIVATE /bigobj) +endif() + +install (TARGETS multi_view_tls_registration_step_2_legacy DESTINATION bin) diff --git a/apps/multi_view_tls_registration_legacy/icon.ico b/apps/multi_view_tls_registration_legacy/icon.ico new file mode 100644 index 0000000000000000000000000000000000000000..310689ece87833d73eb6857f8869a1703cd082d7 GIT binary patch literal 22282 zcmXV115{-H+po6TY}>ZAnVUA-uFbZsZL+mlTm9N>+qP}{fA_raRA=sKy3JgCpXZ|o z3=ACj`u6|_BLTZp2LrxZ5Cz{r`a~rY2dC6b{KMwv8;`y@o|}ihQ4}F4M@L6l zi9=rKz%!h%l$4YP7#|;B3&hm)^fYEbTwGi=n2U=GA5GW-Uu}7LY5B>AyeLRG#SBAY z+FL@Zgq#*kewK!omXwh(cD>cz`R+)Gjh&sAmR14=Q3mvjBJ$54mYv;Q83l!6#auxO zN=j5BqH205!t94)u#kYUF$BN2Ybo^_{qcwM)#0I`-vtF(iO)nbAaMr=mh0>5yUWeo zQKr@yRB|RVR#tEs(4GG5v=!;^lm|!`zU;S`(;5ct{~|Ey)qka=EEtyio6|&z8XJ@O zzwHt?-!JGKZ+3Q8!??JtWI!yndvn)xen88n_(;mgL~1bcaB-zXqEoY(3?Q_)otHo6 z#VOd(2SnY#ZTKD9>U*Davazu#JS~F-i-yD}Xx1l?1cN7%86}x!iT0Mm-c&a1X`d%8 zXJ;Mtw76eRoGsU<3wW}1baZ$ro*exb2C>v=PmC<|#$w?A&Y4ln`3-!ke=@u~KM(;P zxj-Y2IeUys6akK^xtn9-TTzV@-*CgqvWnS$3Ftdls%nB!EoTLS92I&ZmHf^ybuI-d zsmP@~VRn5P*g~aVZ~NogXwEn9j5dp36cfnsdtnpXx&%bx)CR`5TkYZUIV9 zU*2oq5b=|fjoU9j`q6^`mqGR8l%fqc-~MXyW%E?9T$)4{jZz`m49aXwan!;;xoDVI zMMWi@-=3(Dxmo;DjgZiuE+Dx=o%Mdw-;V6f4?2|AQE5y%ige_9!=g5 z8ZH;V{qOy2C3RG;dFSDTtgJEq%eLN9rNXhl5A&`=LXK@%4s#FK%)^KV%$z{*3s1R3v67jHZl|Yzp=J^-OqSyNR?1x3L6=b zqEjoFp06|z4JN*gzdg2@N`^oezyMK@QdR-Sjwp*xU0ptG5om?heH6 z1|tiSaB!$z8WCorJB)M-XB1!Gj52ehen1Kl>z|~KN#uidB|K~qrp5z zCq6yF2+u-lNQ7S12+D}R;C$Qod|0&yPAZbuU-s-5R`R^2-y!AYbnUDY;^1#jyYXY0 z+>^VSrNdq9zG%2aXaV)zNuPZx93! zT&e9rXES=d-!Cq=dDd_A^6_LV98CB|(^oeIomRKT5ieBD;`fi!_CshgVCnlkn;~Bc zN(6QOcG-!FFjPI?Q}kVCyh`WdUmdksQc@b9#>2-dfrfcQfCfQ~-621i7&?IIjZsku z^T#pfU;HYDwL8I>*!T{A8-K9anVXwCd0Nvs5di+>>iT-x`-Q;#@nT)zx(^)#3u}Qs zgt_w#w6$fZ2c9(&zEG*(8-+)-M-^FEZ6TtYZFgk zaCmgIu(Go9Fqmjxv)lS+aDbg13Qad$fT8~)J^{qR*RpkgL~&3pu$C2qw&Cn7?{(*Tb~Fw#J21kxcwF_EJU&gU+oe zE!}#5-0VGeIZ<{%Qo**Xc7}#Ce0?~tH0a#eUE<6p$3fYNSKn(<$-{w`XqA);RN!%q{R#o0-`wr3E++3+&pCRb` za@6h{H?uZtpvdM72bb5@mepXh;6Z?ocK~-Zt-unSY8n||AMMH9-X-?r>+6dvzpDKN$LYXHNoGt9 zT}&gwA(k4vxtEuhe-{^@%uI`vHLNao+7sI>R2l(w;%uWmGbjk+%*{J-Qe8vs7HBWz zGH8K;ffQ6!Y(JMA24b2)+r8oFsHpk$U;){!Zk}?VX%b3ay-Md>dmu3DV&+(YtED3z~0_3{cCbzO*O5x()9Xv*ISzu7w+sS4Nb*SZf9># zO`zazZkHSFJFCI?NH{q;fy)+#Ec6s`f4aD6f1eE}3PVRvzuaJh-RgF}v+n*Wl`H7m zy!~?8v^{`-yilbRZr_QH2mW}rabCNjfOto_@ssc3&LD+k(A+{oBz^EY)2=1KYPL9g z0Yer<0rcXD`$dCGM;}i=+-9z?H;x_O(Sz*3&5g&4UrgLuv)^AIk7s5AQAGdvy?aq9 z=8W~hqCcJ0Pb$bhWfuPZn^L9Icy&b{&tg7?ljC`SdvkL;DGz1nzQpUa7cp3S0p5{@ z`P`J_S4UCXP>?YOt@s%c0b@Rq3d)XqOoDS z7Yu~oypjfqykqdIJ97Xi;G)@hvr7k7(SNcn0RHR9LXx-JJPYhWRa4;MIUI@b`zQGO z4GFhq6mnOYl`VYJAP95@nwdiavwd|5?)YIgQStJ#w4 zzC=Z3Rn@sY-$aIK=DyrkW#e?AQhEdZrD$refkKWnU&$w&bfG5vT*UKX`Y*~j#Mm-f z2DiwivH09l zxa^wkUas{){z~F3iRcAQEF8an1S{w2(bW$eu&SF<^dQ1zzmI2O?+wP*Zkx8IRuh|H z*4E6hZ_7D4vVJ~3ev&f_#SapDX9WZV#MOXhDOj{$BkG=d3kB~)t=Lc<4emE0r$Ec_ zQOJ-nFa+CA&;+rj&w?T`At42_*SLSzA zGldC+#x0?m3K0(8jYB2Xb)?Af#u&_ul~as4h-Woo7@^&1zk`6Y9W;+=c+l2=yYzSD zBM^8!b0(xht@44uwKo2bKcWAR9p6Q*I$!A22D@n1hXnvX*F{!ofzm1Pv=uBLvPmN& z7Z-2by=AwCfge`Guss!LmJb3OCV&2Y_*9Kg0RqbpEQd3em%LEC=<^P4WfdSB+Er9l z1MaT_7K;f2jw+KS$T<4hr~*Sv7CrUQyfMce>Qcxjj;lXt86@1)kH9gT=!D3mtWkCC z4tI1dl;T7VoAww^m3*Efs;N9*`hSjsC}joWzT#jI9perPdH2uu|LoDiW?2ws3iZxj z$=XT9d+Xf(F@pjl&%!g5%l%ryM#?nO8(`VE>iN)o`1G)9MyLb+^!)58*UlLj?%JtU zL@HuTZZZ-_xSHuYz@x^11;6^&_Lc|h^3+-MD|88pyfLzmuWy2xFZ9%gW`h$Z0B>6c z$JnOwu2gG0%qb*IKopC!O%pWxe#1>S3Yr`uwY|rzunusGteAl!)>kxEhI7iHBrBF% z%lg-HQPDR|#dR~vMBZntGjT-cO>kX8Bzciis@k!!=$OySzmAA^E{z=5n;g+!A9Fof zCIG;`+~R5z44-m@e&((#1P1%3TpQgiIW4UeA8Zx3zrSDL^X-;s!cwqqY}31swON*e z13j8b7EKhiWM6yjq)n9ANU(M7i_3#&86cXOZKZ?6W@3RyypN@_r>ca}NM`vfRRKK+ zOlv3MLa&KzWfrNyWDrT;cRzVk@1c5}4lHqcsV~62Y2|x^ifjK9_boTxl+e;?jR_Dz8w9>BnQ*I*6f^F`7S9H_G`;Vls`^S3Yx95A!#XonO;ti z%IoeZUsjc$jGsFE`F8W6lxv=PDYsRH&!~JelF!^M#VkE{r2Kg{MQrRFNGkAr&PR{~ z+?&oit^Jk;`FxzDaUqV(TX0)derk3z6~IO}rRHk7x?^pgw+{cAe1gzhARaH~;e~`c zUp8gWkRDWZctv1@>L^&tWGS|bO=iyl;QH~<*ja1X{k1g1Z6?}@hoB8$SO2gdkxs%k z6*>AYxt-C+EHr%?@&IVQPRK;^E5yi#bes<95 z@ZsfmyEsaN%hP$haJg7({%}|8swtYUF1d7c6XB4b5*o@~MH4`zetv#l^L=Fac-RUC zz{HF~x1Mi#obQDCyfV+}N zDdt^gD9ufDwh;<2DEW!@>e8UW2Q%dh{SzWt7kZcJ!>T3wn|E<2ENZ~t+uD= zl3yeT`=&bm{o~0jDDDeAEvEPVtYkv$HaaiWb0{U^4hZs&G`qO~;mY=tYTAPII3SF`liJLity1?w1Y!0|mf8 zjGnHHT2!;CJUN@+bekgcmBfzEHjgWFS{>C(H4V_o$$|t5f2^k$ryitSsW=u5ZayMj zANF?)-yz?LVfL`QOSGH(A{ntXbo)-d^X}vEQ;`o7F{$en_gNPeQ_kSgee0thCAjCP z6a^XTYBli7KTHbG)Reb=e=Fk*fkZ4AQ(9^=`)pk@5(X+y^m?*_XnDf%EHXfAq?GiQ z%f#Dp;fbSvqtY@q)}^SqlP8-PbXmjLc3lcJv{ed~Vo??qg?RspFh%*o@yxa>`_XU; zm9uiqAyqN4;n!Gf^>ewde^)b?#GmgqO@P3cBCl_IFR_v=o)p!D>-uMn_zTp_Hb#bb z3D~5@JC5r)#To(rMDY5q0NTjGDuEA3WyZ8o1`Im{ddr^Y;7VOzeIkyKf5l8FR~7uX z+xbYI$d_+%Rg82cWBECAqZ;Tawj4KdlOFn~xjn!B2qV*)DD@8>(A*Ztk14WAW*O;d4Eyl)bZ3Hmd2P!Clh%n@Zg%)W1>JZn59Si3Kl( z&%1O7`!J~Su;!^u5lNVg^`PMy2JuPDnvqZHs#bpG^MjN7ZcR2gGMgM1_t_}64!yPRDkZfV1VmwgO`+Jc zp#%2;;@Jq}wbC8@bxD0qBuir8kHkRD`pAy%51^@;UUaEq1<;@o2&x5o5?6DLRg6Y2Ju5Q8W{)$VtTQCFtY`rq zg$PO50+oU4Ize4mk&^X`!jHD%Vd*^b_I7(^$zjq1L*(|;&O=l9mx#HjF2w-!OpaNZ znSTSOL`s6GhP5N9NW`>sL8k@F4{dEc^$N_A@*UM|p6m&?1pl2RxL{qk=l#C^{>0>c zy03OBoE^jTMyei$^#eR*>2B`UL533Mx~ecHWMZX1)aVH4)XJl%AVs1?|4a){ zR-iWUxrQ?Ed!_`)hBPRIVN|n9-Rbs5z{_364L0+8IUZDoOeOP8);S^!VqWy6V@eb? zEq|3&kcb`iU%5t+TJ>3vb7>|UxY4ykH{VDKncv)S-k$)e_@{%_F)R)eiAzYZ8udUj zwHy^ZRDrb=7dssn6?$H_Kc2+D|4{8#l4UIGFxQaSfdt;O; zhc(EkH_X%2TQtwHRQ~ac!>%5w+`srUR zh3(%1N@AgfLCQL`m=AE}+4HJP^nZA4c|yMghJ}oEj|2~b$|Fo-nPt&3emv8hn?%_} zr_bA9Z3}Lf_%f#II>AOpM*c1-XJql=e0Kwwr^e;0PwC3YAur`x;@XtaPT%*uYsIqkIIA@o>KAS5Ac98q&VLY$zHV?pW!q{vs870)#YxWFxw@uB&w-M*^C{hVSG0 z%W@!@%o+~1OnL5d!`~Ww5>Ov^Gv%4x){A%PK$srP;(reqgT}qJ>ObAR#1p=EH7TIp z!9O_L;&S06Ztf-5m2@|&C@ZJ&I2ymFp^F+m+?$OhYy3UtNty-yVl^3X0Q&Il?WE8m z@heYj#3^4Wx_gkzc1aXZ92w7s1qSS2RqZFP&DkgSWwTVr?NIETdeL01?BTyVwr|>R z^k3;i;aqjx%vUNI#NDVw*WGI5P@+t zCY-Y5qm#X|k>Gqr%pSr>=SM4Oq7W$EaD&>yz*`-y_=eGyClc)OLadm}q9m`g_aB!0 z_V%`1HXmm2b=n4xvaN}d+ca&>W>F$cd!LMj{rwVDTdI<@Rd9*Fp&5WVS0OOW^6++UBmlo|I;3P z>95y7s0)laLC9Hoz&O2Gc)xgsK|r{RSVigjpwn?)9jx)nixn%rS+r1y%sNYJe zz`&}T5AY-r9@n&Ac`70R?J-`Qgj%^}{7%FnePao zv+HMcK{?Y%?Il+aOVkXdcwV%KJ$AiLPcJ0Aj=leOaC<{?dXT+6!Q93NRcfoQ)1oJs~VrEUdcxQvX9EObMDwJcO5ZhSup^e)fKcT`(b)6H~{ zk6v4yf)Dpnya#0tFu6xJfMfIEzv?Wr-!YnY9F&+6ciL3ttwO~=1zAEFFEewtPb%zSSi4sey3v{ zlBbS}Ex8{=e;CLWvV$ViGPd%AJ&hLNeCU%w$K|Rt`TC*CU-5i5WPGuC2NsRQTrj5| zB*H~D3P+M*t1~^7MHkl&2_bJ;Dk?Uws+zwur>ZJ10aF!t>05aJpNrg;nT)yU2qck! zJ=gZZ6;|^ge37(WG()vQAH znHZA0I6NE#YS7>d;wdc?og1=T@*3ZPO*9H>l2rZBN+Zl z?DNC_tfCE#s$T*Cd~;uqV8FYh#x`4Bx1}4#d-<6GF;1*?=81yyEqNnD{3_`>he>60c8oXJ%Q^>4q!R0?j zy-n|bSDz>SKUDmGn#f>YT3|XFIQ>k#FY2qc#h`F!dN7CVsD*djwH0YaX-h~MOgJ&g zW*lW>x0v`@=XOS;nT3@?@SYVvBUn76A4o=jZ&Nm4tJ$fK4WU*F18LqLF1@J<`T~-M z`%bnYltZ&g)8iw0t@3uo`DC0Fi|7WkRkY50ZFazo^BY>MuN{92LxF5;`>p8Fpy|Ka zB1XFxHXMYQPEP^Z6ozhN$9=aY11lpdeOh+30zH|xbqh64kxpTuoXf;ob6g^#KB#VR`MO39Zev9W*tz^6v{AT`CxMcs zUHgC63GfCsNJE7tL4)|Oo2Ggn$kNg{rlGsznUE0kgb?^Y;m9ayLIY919AkSC!3M&H zq=_o5`)Y8U6D~gT%fLu)1$H2_&=}_}UU<*-7d9OgL~GU>OT6#DU0Q;tibKw>PwFyv zfplPX(TMsg-c>xD48jH%3<;6sn|`VeiX>2cOwqCG`L>deIVY=^yAs-oH}b z6o)Wf?tN9Ag~SepzS_vMYfDt>I^^vq@_rc?kG5!R-b&XpoXQhJ+zwY$JhK zLo{)_L?tI9v;B9~&VJG4%)NDF-Jut;QnNM)@NNHmEWH%KZSZAGis|u_?;_o8_np2c zO1$zywq>NOinnfh9%k#hEt?T4JaJ>&9fYob7I!HJ)J|)68f99Eib1@DJnXN8RN&%q zCZVVdU1C4Ae9BaBsBk84D`7kN=e{1fq_9E$gKfIr;|&ZiN1Bqy_BY?9?{2s4%d%NL z_RgHgA^$PTHg9Yy7bFV_B`6_GjT@chelc7cka%5ZIv_H{#2}cvh_ht-#8i6)#fBQB z4E|=w)hO9#9?N+lbxG- zeQZCQnR*Ck$$0VV?cwm5?i4?@9lSd^yJqiVqh4FQISc8ZNddSgheHV_g3y2f_^m9v ziJumlSj#$zY=hGaqp@D43dKj~$G|#2(xMdXz)QxHr8IKOYq}D}$+{1|VhsXF+^s5j zxi!A7*s*eWsY3Hj4>mO1^5N#nKG6=jJ6%uuTt0^QOBAX_dmpP*VGTWCNxu@=; zKz9wD=gID$$lL!Q=k*}bsY6d^p(!=<*$X!%Vo2xpjg0fEI@38v1~&9(G(K95z0&sA zMf3?Kf}y&eq&eELi`OnKY`=ip3I2HM9FHFIGMlR6>zRCl4^NtKIJKs~Irv)9YTI?; zPts^clKs23!4&A$93;C)iZ%FY7hx$Bixv-RI?tmMH%O|Tn@`0;W*BWSWJhzyS?P`0zD75fYN)e`Tek{nG#_2yVZSZiO>jKeu~Ky2fz30 zTk_U+p8)T^>GR`teReiZq@yU{^X!8ecOLiN<|FL85{ZMg0vZH*bIGDXGl_MCM$O@j zLLslvjZcugNY%xz9Se@fRrgm~diq^}KGxJ=Q3NDInVr?tu`tqx;RE>Eimkw^I`6>b zb!u>xvuyNAto?DX8xQ}|6ywIq4RtD>!rG3gZ2r(m4*ajd(hj_iM=Ghh|k}_&|$PqjHK(eK!!8Kk5cEuiMHp3%)7H` z$A>pY1$gFj&-8JLOug3&P@bc7l8VYP;Y#M{Sx|@;YhyYXcH*#VuC~qT zLKV?|6CEMlX%zjErgo?vOpm-|Pz)TjmSlqI3?rA|Bx6|(4TdoX3#A~)u#lc*q}=MP ztd5PjJ>U?@w6?mm$7C$frtvs;QfN|--JHZdk06n;94z4F+w^7UXNTX3nF^L zW=`;NCV!y5dscpuQ$SL=q~)-fyU zDcgZ$vvupBr>XFNdcwze359a{voeD1N%^m3<6C)FGFM;GpeRc%R_l4*H)u4QjW7~^ zf65Pa1IJTQQ|li~cfZ~ahyNBAAA@jke+!CqeyC?{-A!$X6o!>G) z*P5nxpPrgvaxRl2IvEzJZGSoNW(-Mda>NckSlk$y=MQe`ISo@X=r;XYT3SMyeOl(^ zeS2T*Ow>=FuKXr8pYc~%gAA@tE>Cx z#%k7?!UC1HpuGGfrep9Y*KDi1y6M zeVeWBM(WCyr^n2Ybqu$D=A`kLlUGZZ$S^QlRgD|QH+#c`<=dU(+7q4b?}H5`mqr%O zo*U12ZyrsXo6S52Ych5>-n7ij7QmIS)alHT$LDss+7f|-hc7EDyDpVvltGIs*g8Fx z6&HsDeuIF=5rIlB$znQ$LPkzLna*|VwJ;`=NR^P1G9Z-Whd1BuyNeNojl7TYlg&Xk zmBv)YSWATVQy^`h^sSD;dDHh1(2zoq=8t$ed3_u_z!kQ|6ZVRkIv}X1*2nr;$z96? z5rJin!|OyrE)~~jUazF8DsE@Tw8>~N!9FRdV^0r*jC>f1{UOnK@$G1)nLQyPfr$2U zlCrHD3`1fYFarRmH?wimv-P7%V&22tA4`M^eVXcv;@Rlc&>1`n;M@`cXAlWR9NgSn5F6s00Du;Z3LFpsK#143(Pvx zs|fc}_%q5qt*bTYDe|^ivnl6|e+F5N4$5+)0oO!jy z70RI;0cR?g*ZJUAR@T~H`|sZr2NT(=r1UQ-^C`7^M~9`_g~z4d4$$ck&7AjGZwQCQ ztRtCy{e!6J=qjJymmBS{rvb30E1?izO@Q>4&h0NKBO^o9Bjo?#!_>I0HmLQs|Hk2>ZoebR;|5VJh81ca?_W{FM?{IfzUymnD9C#}OctGl^r%r0Jeb62Y9 z89{5V=SX40q*yuWmUKgtKmWI%G*Bbpkr+uDsj!O@<_)%-UT{(QgSJ_F?+0e4F@>de zThA~nhu?`L*f_4m=0ix<2)dJ!hSN+F|9|&4^$+|@A|Gruo&>yofr{JbPTFJ17O2L{ zD7Js;Pb+aPTU`6IOtJh1nC12V?s?xSHT*ikx4$%TZCkh3y0}11C6ub!q<^mZxFwxA zs9o8r!bqS8{mA|Cv39Ni5!-@a4kSnU5JW9~K!4Nh>r=^F>0dYw4ZiWZq8)}lerv@0 zmoz2Ju3_)oeiwsB5%owCu3$c-%!-L>PdnYIna7v?_j=^zZO7sGyR)OEo4aj`iFDK7 zH|1dE^bKdPiiO2lAfYM@UoYtYjrqbe(%QoHeXf(VdW z4!4faf|L2uJ2`3vuCacyR?S}zgq*n3JC?(aizE2Mo`p~k+yyc#Be4iq*7LQCUo$Jk z{M&m7igaBc;kw=72$RN9Y;hrdzyS&jQW7!A7mtqQdr_^aZKF z#dini8Tqv=j*fBF#rbI~!0Z(@mY=}$V|-3N{#MM+;be&}rmbV-eoS=m&J|Bs#IHnw z+l<@o7TGc28oB6b(a4_}3o$osCHtTCcb0&e73G|@8_3p?MKKIi_z^7FqZRN`xZ2Z;@_%ak< ziyXve?sXTAl||)9tK8((;^-Eze;UfX91<`HMUa_NXxT|Vg?fJ)?1c55rmy(Yb+lZa z9>Hzh@#uP{l{CHwizSD%(m=OB7vF?G2{|Z@!H|tx)M%P$+42fuzl|+w8PwPK5N7~7 zm{=mLzn&Mf+AIqlwB4~6e4+}~F{&SMQ}L((CvWRH`*X)|UxdPx0+#R9eY+qHH70^c z!TLwJ#&>hAEUcLd_9e|fk@rxi(lukkm~>GLDu?6DoC$ugB?lB^1S4}qD!7YgKNDj* z`KrX^W1$jQ z(SBZnpIjnU>l4_cohIvwLg?s({n?3B)0AA2_Gu5l!N8&_CB~M8!snZ9L;}t>aDOOG zhlI)8kj@c&x9C)$fTlvjMv+QVr06atfshx$Syyvc%Qtq_E^|{=v16v51SzV^V`*_} zVQ9jYx~T3e62KoOSsWDa`i;2Y_Wz@4L?WxpAvx4U#K-zeJLKpmW;}wM= zP@v{E@a3Gf&o1DefVE6?{w=*JO~iLe+5+urvYjB2v^vT<1P2EfWh$JfWK3T?q&@Vo zb50^d1By}NGFFwN0R>SsquKLuP$L;h&8wys5&fco#l@8YojBif&jpR!id&0{ksP7U z1h2Mi^!z#6md^@s(f-=n+x<0KXgAuvQw+jXLQ;*ABmX$o8Es_`A&<26yZh}%{^OEz z8>YO&M=i1-_qtcJQ7sw35Pg%kH;V72XB?8Kc zefR|z{-Bbs+&J$96hI`B28mmAPUtQ?I}HrP<|h(O%@&tdHQa7cM{BkCG*`q^!m6o7 z#>MOfwwh1XxHGpW@C8AX7WU|Ozq}zRxQN+%VfD0SVQDnS&E9Gp@A4kW!R#~ zOju~re0^A6pYm`Ja4?E`k#%)!89#OISPiN)w{M7C;r-=QG3qZfCOq3+wl5vv@{lU5 z_YI##xUlWaf&R;8%~n>Y(HtTMTC?$4oE&iW6UiQ}F0H|}|Ah$=D?$ND0jNDBVq{^z z@UnumuJzwK%F9*NHB^+fCH;(0JwrqHFfo+1)g_f=XtAP0@qq74#FE~~Vk;*6FrL4A z2^6q0DXe%1kWbHOTbJRT{$Q-As3D=DGrU}33e!q`;u5HM1Ti%=&EqZFxlE(#HQNpD zAK`#&5vZajb2Jm`UHp+Q_^ss>9wIiTE0ddcsd~z$EY&xn!)bLpmCsBxc zi-|m&V9+feb&1{tm_i7hs`b2fIpO?Vxf?-GoQiW0q~vgf64y>)8Z66`iLy_d`LKps za?cLh12JFU!@Bpf_ZCVzFR!Yq`pyrv)PPa8I`07|Hr>Cm1A3I4ECJu`c4_2OfE0N? z>>?0-qL7m$PcSn2O-0R?r)d9je<9%{Di8<`eqNlrXumQxdV8Q^Tl))?*7ZJHa@iZQ*NmDs!c3T`oX$q9=C8!LCuK=}vmIjTI%B1|j_(D|( zURcm{veKH$nK3oRfmou_pUrhry*8=+S zbZ|=K%keU2=vH1H*XF`kB;x$i|B{HxXvHPOkG}lS0IL#{Fr(CQKQ}rL%Ux=*;SQyw zw9E*rq_Lx>D)Td_Rk(u88)EA)XBPnn7Y_?Z#Y9z*GX(}0y2V4o(NVVIYnEd=Ync5y zI&cu6O|?W}QpG^)7hTr%Mzz|tE7oO|IOIvV zmBrD}AOg#qF(Q@+<&1SL!m@?ZWWx-kACpXDBnvl&TOcg?F)%P|Jy+czzCbh?Zh^h| z=up2feWTB8!1pu04!0)di*bxRb0 zkBv_7?>$OB?ERU6bsaq}2^=$2sKjpzGv^~qzVMkiP;#b}rv#LOngqxUqfxX}$9#@9 zc&khJ3bNHXmzI}-%k<AR*dVDcVRy*HW_x``=gcA5Vb%KJlO(HlLok)K50zNjN+}U&Ck~e0bpkOS`c9=sOq{WX1^v3rT?A zH*bgG2qOgCsayrtX?@IJDP&!nxwl^Tdu{sP(p`ER2o(57T4FP`Iz8EswU&XI3XJ}M zoed@dpa4ZXZ&Gp6CgiNQc3L6DL2X?g4&Lb$ zU*&RdQjqhna`I4y&YyCVaKvi)zn}+#GuA+6DNv{LHq%7Wv;Qh@35o{xZ8BX7)%6_i zzdOo^iZ6Sog1qL;JYKD!_VvCuow;AU8GShSX+8M*_hKcs-`shb8zUXBA~b4*t_JpV zP`1-_`p}>Wp$rsKZUy-j%5m|$5p}bF=bEcJGEJ61u3M6PG&IMb}3{ zOw35?JBkjQ?Nnqo&0A_O^1PlRUAr2reR}$@ayCgt?b-A=mEkANTlsfQO`ONKXN9m_ zA*Y(c&sQu$pwfe6mY{=`K_!dmu3uf1c#TFmgad|!zFAV zDlh?aWYVL+09YN!j&@kz5l_42jsL*+iZlad2o$2C78x72vtgxzJ5>_TR0XW~@9K6; zpE?R81)!<#UVSJ5!HY zasrk;pBZ%8@mkj>)KcT9I9Zi0*WD!bjYth@G$ZHDcSn>8T}p`cz=0t;k3^(gZmn4k zU>Tu`l2@ygxYcrRUZd_EU>myg9HHT_TI(n99R@%=xsBrqKDoe(!VKuU(W2M1{?$95 z=U1D@oT#v}%n*h_gdJ|UKXwxlJV;o2P~wcO(Wu;;t?4LU&>tlR#(i|`TBh}aMF5h- z7}0OMy*h70OJ5_bZYua;Osk$5%TJ|Xsq(#R+u?_jnuL-gtY@gig>;AU$Ot_RV@;QJ zLw>mehng77cruih(l1O6x<6Za!abrvut;#l&ObS(6Hvva#Q!!k{F{DSUfjIu-kEeL zR|pBXT=?ixE0jVfEDOIqfy*nhpD?80;DHG&MRs_0dWNH*xJdJ0FIHEE_`9Ng0|&>O z&Hu4^B)ib%*ka7aHm8h5TN8aQDm2$=jNC z5#S)B$XcN!b_Rsvb{8`3`dm_iT*}KUcpZ3R4gOvlw(_?%6x5-GtGH60JB06zT11Yyr$+MPypK4EM!NA`9=1F1yjeBs2-Xt)YiAZXKTaJ_Tz<3zcD0~PI6Nh^*F;; zsn^aL_5-T^rifWdE&qAO=K5$!CdWTOK9!3i*^V8 zPpIC=L@)nq9|(wa++XDr_^;@5gpUtk#~$brDrmBcrjlcP>d?7V%VJV2bP;5`kck z?Cn8fO2T#qEKK0E;a;=Uhr%Y&H|;Y4tmx!U!51DKw~GNwj^>%Cd5f>g;4oLWg(`9! zdWnfg^Hub;u-7h8k)#wnFb4I^(7nfsqehJt{qIJFqG?{(Ar%P(Zs_fxz;|2^ zeFLs;)D$6-SArr6tRwLaPe)2Uj2OG3?Xoc5hq#kx8AibMsLUe3Qvc z2v(ywvtCC~^QC{p`ZfuzU|e*VephJlR}ji#ky;}Or#yM*pNR9;$3G8)xy#M&#QSP$ zchU+@MXTQ}n_YCui?4@gn6>_Tagug-uW|Vah(X^i7AXKYFuvcq2)O2drcm5z97oAQ zNcfqN!`DURTvibjocOW+Q|w=JuABj;;V0RRi+pKe_PT zS_$WEX*ls>ff)%@92{vtU{+F65|@%fN5_{1s&%)B2+jFak(PUpov@4+E?6Bs!}Pd3E9Jh6mv?k2Y6r&YIOToiJAJW>+`Mgdc(6OcahgN%V}o-FrcNS^>g)a zY_8u+BoN|u0OHJ1(b0}P1p55;!00nDK#f2wnDsAi)(wS?vJr+pd&C|94HS%D!mKnT zkib8d`C$l#Ufl{Uaq&diI2n>GRJofSN~~n#-c<0CrBJDkFN*Gvk|_rx;^xLB^nQm9 zNa=+5+;$Y~?C6=}ZVT1<1qDI2tAA^4SGj=HoZid*=^$XgO#;HItdo;d=BW@cRuT{_ zHRJKpk4?+K)J;Nf34@_!{>JHVy3?>|#CFUc7}oQ!zH*uVxh)F7$U25XSYVNE)wpkT z?-w!&+AJE^YA)58!ONyHna!805%9T0tgNhX&8pWi%oa)^+cocF70acIrZVXPVxszT zhc`0wc&4JCNd9Wt{>`7xlNs42=NpQ*dsP$5jDf?^lzPMKk|jbnQyEEgJm|s%p>=_r z(*b=~^&6(7)YN*fduuEv-JvigYWAFJl1+iFwe%m-{6b7sRtNFfav3?@FYe02?AWo9|X1O?Xwx6P}GoQqmX z${+t|GW^R1G&>*G0plVJShB6wz5gSyq$t_6uAT-)9WPq?g zzh&(%yY(E+zxl`sq0cw|f%`pef4ykC zRamGo*tA~En*>wnso{6Kdg1>hulW-(bmj1IS>rxi0`(;vZ`Xl`wxX1zCNE&^4WiEJ zmWGkOu&oZQqH5vWdCK)QD;gSx^bbQ6CDd(~xY&HzpA5j()9bV6`O6JF{PSQmvm-#@ z0|UX=_&%trJ9gk|E+?6QjU1yO^ftk`{l7-4`A)05u#h0wzoLXYI}_ zaq0j1xbko)_pq%X*ldWm2iDa9Y zBiZ+L!i2Fi-}8R|e%JN=HLmf_%==s3=eeKfci(r_kkKv*24MBcN$N;T(P@3YWJXp- z`j28~qovQ5gXGv;{jrL1KHxK5-=V{T%QyA>v!f1i>9)?x=As?{{W$yW^<8Rn45?n} z(8Hm++!~&+M$55MKUMvz!@HX+u5;b_b6;QW0Q(}CT#C=@a{ai>92E0UQSn-@&6*Te z^4kN2$8s{s5Tg97`Te=9zBnm-#tfIll;ErTV9!T0;D2tw5`O&@W$-NOFhz0qjoW$d z3<#2zaImY3z9)Y~SC=az-PGJ}iKX3#cqW>|;N&sPVd)0iQ@lu+p0hc3!gPqCt;TGE zg$W%TEyK;2nrEo`jykN*-{MpAdsjv`c*+0Etm7ro=y7reR_;_%afYXgn93{u8UvT* zqlu5y@}Xz*cVLQ_-z~$pkkCb&u0lsg!oS3)$cOh)G-Jm_7OGDln*wA&HKzHy(TPKS!#y;=%aS2D6;1Vw?V7p({9q~nVGA9zKKlv zsreuIk*SKaG!o8JOpBKFq22_q4vR1vy?5=dCY0k#lbUx79KZK>44Z@pSZ<&Vn#YJK z3pKYVVtl5oOW(fvd6Uhkw5!A%s}iz~tdv@Kh~L~qC{4TnpXrcu1#K+YpQTe!hIsLA zmb-OWjM~@o#adfdUOU@JOd-(xP*CGNM;ljhw{>+IfeIby>qbL z?%^~ud*7H10Afr&8@e*mvg&- z`^w49tqbWMXsug2JNGYT8=Ge=F1jZqB(#11E~>1Ym1B|Dq?&JtL^ZFv86H-&pca&f zqNZu7pIirDZ*S(+v~>}0q>6nmdRv+JP+KZKev8ilYQMexy-&WqI%S`n?qKb+E_5^u zaSB8Gn|83zuk?^F>L|Ooy!=a$F7NH{%jb3p4;7V^+y{svBQpci;SV4-K#U2-B6gF1 zQnH5X!%?~|Eniz(LzVzQ;uMZrwt>DI5;= zwx?%jF@2%z%*^*-Z?Os}b%siuP*d}KWr+@M;b!$=!GRBh&xn72d`lv4A|M$a0;2)x zYmTDFSD<_wZd+SV7D*-qZH@%i4c6~)j$bgbKzEmg@BZ1`Ec7N~N-YGZA^P6vO-cPx zbnLzU_e$#5s(e&ntJ7oEvqJ1zT3Thbt2f^omwI8+S;IUfu?9R3J(K;x^PEIH5i=?VnVEr+QHjj)k0c9hqbU$(xcvV0FGEl;UeTeE4ikA0>_u3D zE-_1Y`~KYYbb(t=f583q{rrk6ZdR(H?1FxwKbh;Ts&bCJj))n#r{&R4A#VuPZgiu# zvNG?>NpfYTmxpg+l*R%;@X+N#N=`|c8*qK?{AaQyNknUu{KftoOn-N!8>!p6o7UTF zihC>H&)sNsT0!hp`d5Z5Bf@l!sjSt8%o@+9#|xpk$u`xLNgE_L-pJ(cw|G4;{KlWI z4%xHH!;hk*XH+;iI2yP9Oj*?i$+K&$bI8lfyHZ1(tPn*+kxh-4>r5C=;*nxf=)WAu zf{f3RJN}b03p%Lk|K8T!S|IsAo6^^w@#}Zhu>X(@WY^G%&S&OYA6!Rck3sKLJJar5 zkF%8vg>D?GN7immuHECCmU4Zh@TG?Z~?p(C_DJy0VwSt zqJZfbqk5vCN?0_lcAJNPU_kaLuc)zkau09v_%g~%qToofr8ZN&U0uw!$c3*ixKj4t z@_ekf@qZY-cBVuIb&oexw|{i*KvXjQmpSc$Kodp=@p`dnO2lBEV{s$`;R z&x;p^Iyzsi-c#TZlufTDoohK)Gh;rE>36_Uw&Jw$#sZ!Zo}{F>D@+sZZ2z>~uK&YW zMsU9!`@v`I)YjG(2Po30Yq0%(3S6ms>c#Nz9JJ}Xnl}$JF4pg7S`(tktuI8yB;U2v z(-x(XUdqetv}WCF7suOgU0*Od;L*u+4kK!3e<~2W5UnVKWV0?mXmstN0%D`W)g32Jz>Mr=w7dp;_(|Hr$O zS4vMxO42hjUZ$np3fVDX6IRy)d46_w_8oI`kho|Y7?`JdY8QUjx%)t}NmLa2h%aBg z(hCbyw<83clTyt#%vU?%$9S(Cjz(mI`l2Ak+yuzIHspQuSEbw8hP~Be z06ZilBw|-rS4pe;n!TO!&?ETL+>F>hMbZvf`7cE`MIB8`PxrGhHF$jt3}Z}fZGZRq zBE_4~$x`VjFSL_ezH|$r?jDRhM$BV?zEyWA`9o>xIf(AV!?uV^l#`XXxOmJ{`lFBg z%q@LDUGp+0r(}g?kFB@2w-$|+1F9vIX?BMy1EG@IMx<^+{8v8bZJ%^B z60Ntk7$AJHh~Lg@1FpJtZO}ZvvCNL}uC$c8s0#iI^ZWd{7~j@VpgYF_*pC;z7Q!pw7QdDP>Y5GV7EZ|r{Jx9ZNN#9o0Iv1({uNzII&`B~O(L#- z=caAgpB8NsR!>8UKm*W}zUBvxN(C$p7QsuoN55>(dou~#29j@d(P#{O)SjVQ4l2Xp zz_IY#81-y_^3T40mIockYN#$s~5r!fA-i;aV> z`JLJs0Qmls&yK}ZZ%Z7+qpug53_tFWIisOrxAMaUYHQxY8z~#^A`C~5CMtUT%*)tC zC=?Jq@0O3#_Y~dA3Xs`5Zr>cs&YK%K@FCFaZ2kU*4HVn+ebo(<3DO|xYq7$Mgfml6 zJqpP+qalfe6^t(6t5`e^sbrNS3;&gHPXI|^LKfdwq;gl6QhRZ;pd*N;)6w-AeFObR|0%6U~Z;N@1;5YNeo^~+T`Cr2y^4nZ5P7CZz-ogr)s3ZV$ z3RmrR`*zaZhKO+N@M}$a6$75g9VtSBxPl13w0AY<{rraGCB{Ha2#o1!!9rKBIqqK{aGPruO~U|tS^-E zoJ=QMw!`)9z0OK24hFDWpsuwdwV&Ryhs00 zW+)SSN+tjFZ*zVEG@3Emfil(-x3L)9n#A8c5;!)pcy17SK?$HZ^o0gUo#-$KB@*Fr zW(%|j+N&l|a;cq-!(YIxXZ1fbqx_6^ZHa3}%mE2Nf&j5Zd`XEO=V>w9w^yDlZ!? zrLC{~EjQ32a4QsB-!8>Yj0T~LNo!t-F5fo~JcnF}m@ep5tn{t13ab3&`Z_!_E-p^V zpSHs~&cMaXy9S#2w5+WDX^lf=)Osf`+#U&JuVp#cm7kQRQMIS$=-Vg~-Cu-4# zv2(y)oO;fSE&$z{C+>vb%E2rUvQH|Hg;f}H&E!#cwfZb|bfSYIn|7BLs~Js;T~1iErltX=flFnY6rz# zDy)!=%!z>i>gP}&v+-b$xT5qHMl2-Th~B)}FQJnGv9qLqB#c}49&8oxav1Ua>|TNV zz(QF(aeKr4=+M)Wl1qWhwwUk@s}B}fOQNFBqz>X;vbVQ4=$8vthI4sWn3{H18imTtgH8z^(!l^B5NAmwVJ$Dh z8~yHXb$&<2b=^Idel3+PovE1`^C_iIwK*?Pe^HUcLiB~uyOc%`3O;0_3}uvdQfbR> zwe~bK@uK_u`(Nw30(N@)+v?uaces)+bU=w!jaP|>O)clYc&GZ|z+~T=n1sY-?@@Jx zn;D>5?|-C#Gy!%JT7XYxAe5{w26J+9BE#WR_Oc~-N{=uWkp9*)GMXC=U7>!aeEaro zZ4ryB_L%7BeqtDV29Uz;G9d*XG7R^GcZRkwVmIk_6Pz(BT!}6Vy+y9NTR8k|4qJ~G zLPgv}%%J8AQ-IX}I*6+7H#Y}^p!b#BJ?&V}TX5J{DGi6to1>wK5eoV^ z`8YH41bz;R)F2O%D_>mp!pC9ZCky&gYCYs9aELPzO~z zT`ot`cE|7_*ciGqP*(92&97H%Q1PJW3~pz$pP6=zUBj=N8Jx)c1{D?-7J%da&0~8P z3Je(8aKMl!tcQZnInB|Hu?0czco`il6F9k}B$$}O?RaKF%Iw81)Qaif2ZVyy)D?Q& zzxKE1@|f%D!HNk3W7JcyvwI99;uH}<*6M|kxjA=7rpz^x0xtiWxjF6>k4-ALW`Wz* z*Ie&<{VP|lB!=U>82)ohb8>PjcO?qfiJmI=TQCJs_>E`Z5LpQQ|HXh`3+qhm?7E;4 zSMa~zYhjUTnHX4Y2~$gYbI2IkOKH$4sos9m{?f85Q)gj3FQLtLP2mkFouDFtXjbfaQc$oH>d|$pEA)ry>G>7i?<7VP z*SGb$RoF)EQ=u}T0B-S_{~?Nm!{(@=OX6wqh`(vr7KLXYT_^j*FjoS=G&DKE=u_X> zc}8_kOiZ5>o+jIf?HQSR)EjBw +#include +#include + +bool has_extension(const std::string file_path, const std::string extension) +{ + std::string::size_type dot_pos = file_path.find_last_of('.'); + if (dot_pos == std::string::npos) + { + return false; // No extension found + } + std::string file_extension = file_path.substr(dot_pos); + return file_extension == extension; +} + +void initial_pose_to_identity(Session& session) +{ + if (session.point_clouds_container.point_clouds.size() > 0) + { + auto m_inv = session.point_clouds_container.point_clouds[0].m_pose.inverse(); + for (int i = 0; i < session.point_clouds_container.point_clouds.size(); i++) + { + session.point_clouds_container.point_clouds[i].m_pose = m_inv * session.point_clouds_container.point_clouds[i].m_pose; + } + } +} + +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; + if (clouds.size() > 65535) + { + std::cerr << "warning: more than 65535 scans, point_source_ID capped at 65535\n"; + } + for (size_t scan_idx = 0; scan_idx < clouds.size(); ++scan_idx) + { + const auto& p = clouds[scan_idx]; + if (p.visible) + { + const unsigned short psid = static_cast(std::min(scan_idx, 65535)); + fs::path file_path_in = p.file_name; + fs::path file_path_put = outwd; + file_path_put /= (file_path_in.stem().string() + "_processed" + extension); + std::cout << "file_in: " << file_path_in << std::endl; + std::cout << "file_out: " << file_path_put << std::endl; + std::cout << "start save_processed_pc" << std::endl; + bool compressed = (extension == ".laz"); + save_processed_pc(file_path_in, file_path_put, p.m_pose, session.point_clouds_container.offset, compressed, psid); + std::cout << "save_processed_pc finished" << std::endl; + } + } +} + +void save_trajectories_to_laz( + const Session& session, + std::string output_file_name, + float curve_consecutive_distance_meters, + float not_curve_consecutive_distance_meters, + bool is_trajectory_export_downsampling) +{ + std::vector pointcloud; + std::vector intensity; + std::vector timestamps; + std::vector point_source_ids; + + const auto& clouds = session.point_clouds_container.point_clouds; + if (clouds.size() > 65535) + { + std::cerr << "warning: more than 65535 scans, point_source_ID capped at 65535\n"; + } + + float consecutive_distance = 0; + for (size_t scan_idx = 0; scan_idx < clouds.size(); ++scan_idx) + { + const auto& p = clouds[scan_idx]; + if (p.visible) + { + const unsigned short psid = static_cast(std::min(scan_idx, 65535)); + for (int i = 0; i < p.local_trajectory.size(); i++) + { + const auto& pp = p.local_trajectory[i].m_pose.translation(); + Eigen::Vector3d vp; + vp = p.m_pose * pp; // + session.point_clouds_container.offset; + + if (i > 0) + { + double dist = (p.local_trajectory[i].m_pose.translation() - p.local_trajectory[i - 1].m_pose.translation()).norm(); + consecutive_distance += dist; + } + + bool is_curve = false; + + if (i > 100 && i < p.local_trajectory.size() - 100) + { + Eigen::Vector3d position_prev = p.local_trajectory[i - 100].m_pose.translation(); + Eigen::Vector3d position_curr = p.local_trajectory[i].m_pose.translation(); + Eigen::Vector3d position_next = p.local_trajectory[i + 100].m_pose.translation(); + + Eigen::Vector3d v1 = position_curr - position_prev; + Eigen::Vector3d v2 = position_next - position_curr; + + if (v1.norm() > 0 && v2.norm() > 0) + { + double angle_deg = fabs(acos(v1.dot(v2) / (v1.norm() * v2.norm())) * 180.0 / M_PI); + + if (angle_deg > 10.0) + { + is_curve = true; + } + } + } + double tol = not_curve_consecutive_distance_meters; + + if (is_curve) + { + tol = curve_consecutive_distance_meters; + } + + if (!is_trajectory_export_downsampling) + { + pointcloud.push_back(vp); + intensity.push_back(0); + timestamps.push_back(p.local_trajectory[i].timestamps.first); + point_source_ids.push_back(psid); + } + else + { + if (consecutive_distance >= tol) + { + consecutive_distance = 0; + pointcloud.push_back(vp); + intensity.push_back(0); + timestamps.push_back(p.local_trajectory[i].timestamps.first); + point_source_ids.push_back(psid); + } + } + } + } + } + // if (!exportLaz(output_file_name, pointcloud, intensity, gnss.offset_x, gnss.offset_y, gnss.offset_alt)) + if (!exportLaz( + output_file_name, + pointcloud, + intensity, + timestamps, + session.point_clouds_container.offset.x(), + session.point_clouds_container.offset.y(), + session.point_clouds_container.offset.z(), + &point_source_ids)) + { + std::cout << "problem with saving file: " << output_file_name << std::endl; + } +} + +void createDXFPolyline(const std::string& filename, const std::vector& points) +{ + std::ofstream dxfFile(filename); + dxfFile << std::setprecision(20); + if (!dxfFile.is_open()) + { + std::cerr << "Failed to open file: " << filename << std::endl; + return; + } + + // DXF header + dxfFile << "0\nSECTION\n2\nHEADER\n0\nENDSEC\n"; + dxfFile << "0\nSECTION\n2\nTABLES\n0\nENDSEC\n"; + + // Start the ENTITIES section + dxfFile << "0\nSECTION\n2\nENTITIES\n"; + + // Start the POLYLINE entity + dxfFile << "0\nPOLYLINE\n"; + dxfFile << "8\n0\n"; // Layer 0 + dxfFile << "66\n1\n"; // Indicates the presence of vertices + dxfFile << "70\n8\n"; // 1 = Open polyline + + // Write the VERTEX entities + for (const auto& point : points) + { + dxfFile << "0\nVERTEX\n"; + dxfFile << "8\n0\n"; // Layer 0 + dxfFile << "10\n" << point.x() << "\n"; // X coordinate + dxfFile << "20\n" << point.y() << "\n"; // Y coordinate + dxfFile << "30\n" << point.z() << "\n"; // Z coordinate + } + + // End the POLYLINE + dxfFile << "0\nSEQEND\n"; + + // End the ENTITIES section + dxfFile << "0\nENDSEC\n"; + + // End the DXF file + dxfFile << "0\nEOF\n"; + + dxfFile.close(); + std::cout << "DXF file created: " << filename << std::endl; +} + +void save_trajectories( + Session& session, + std::string output_file_name, + float curve_consecutive_distance_meters, + float not_curve_consecutive_distance_meters, + bool is_trajectory_export_downsampling, + bool write_lidar_timestamp, + bool write_unix_timestamp, + bool use_quaternions, + bool save_to_dxf) +{ + std::ofstream outfile; + if (!save_to_dxf) + { + outfile.open(output_file_name); + } + if (save_to_dxf || outfile.good()) + { + float consecutive_distance = 0; + std::vector polylinePoints; + for (auto& p : session.point_clouds_container.point_clouds) + { + if (p.visible) + { + for (int i = 0; i < p.local_trajectory.size(); i++) + { + const auto& m = p.local_trajectory[i].m_pose; + Eigen::Affine3d pose = p.m_pose * m; + pose.translation() += session.point_clouds_container.offset; + + if (i > 0) + { + double dist = (p.local_trajectory[i].m_pose.translation() - p.local_trajectory[i - 1].m_pose.translation()).norm(); + consecutive_distance += dist; + } + + bool is_curve = false; + + if (i > 100 && i < p.local_trajectory.size() - 100) + { + Eigen::Vector3d position_prev = p.local_trajectory[i - 100].m_pose.translation(); + Eigen::Vector3d position_curr = p.local_trajectory[i].m_pose.translation(); + Eigen::Vector3d position_next = p.local_trajectory[i + 100].m_pose.translation(); + + Eigen::Vector3d v1 = position_curr - position_prev; + Eigen::Vector3d v2 = position_next - position_curr; + + if (v1.norm() > 0 && v2.norm() > 0) + { + double angle_deg = fabs(acos(v1.dot(v2) / (v1.norm() * v2.norm())) * 180.0 / M_PI); + + if (angle_deg > 10.0) + { + is_curve = true; + } + } + } + double tol = not_curve_consecutive_distance_meters; + + if (is_curve) + { + tol = curve_consecutive_distance_meters; + } + + if (!is_trajectory_export_downsampling || (is_trajectory_export_downsampling && consecutive_distance >= tol)) + { + if (is_trajectory_export_downsampling) + { + consecutive_distance = 0; + } + if (save_to_dxf) + { + polylinePoints.push_back(pose.translation()); + } + else + { + outfile << std::setprecision(20); + if (write_lidar_timestamp) + { + outfile << p.local_trajectory[i].timestamps.first << ","; + } + if (write_unix_timestamp) + { + outfile << p.local_trajectory[i].timestamps.second << ","; + } + outfile << pose(0, 3) << "," << pose(1, 3) << "," << pose(2, 3) << ","; + if (use_quaternions) + { + Eigen::Quaterniond q(pose.rotation()); + outfile << q.x() << "," << q.y() << "," << q.z() << "," << q.w() << std::endl; + } + else + { + outfile << pose(0, 0) << "," << pose(0, 1) << "," << pose(0, 2) << "," << pose(1, 0) << "," << pose(1, 1) + << "," << pose(1, 2) << "," << pose(2, 0) << "," << pose(2, 1) << "," << pose(2, 2) << std::endl; + } + } + } + } + } + } + if (!save_to_dxf) + { + outfile.close(); + } + else + { + createDXFPolyline(output_file_name, polylinePoints); + } + } +} + +void save_scale_board_to_laz(const Session& session, std::string output_file_name, float dec, float side_len) +{ + std::vector pointcloud; + std::vector intensity; + std::vector timestamps; + + float min_x = 1000000000.0; + float max_x = -1000000000.0; + float min_y = 1000000000.0; + float max_y = -1000000000.0; + float min_z = 1000000000.0; + float max_z = -1000000000.0; + float xy_incr = 0.001; + float z = 0.0; + if (side_len < 0.0) + { + for (auto& p : session.point_clouds_container.point_clouds) + { + if (p.visible) + { + for (int i = 0; i < p.local_trajectory.size(); i++) + { + const auto& pp = p.local_trajectory[i].m_pose.translation(); + Eigen::Vector3d vp; + vp = p.m_pose * pp; + if (vp.x() < min_x) + { + min_x = vp.x(); + } + if (vp.x() > max_x) + { + max_x = vp.x(); + } + if (vp.y() < min_y) + { + min_y = vp.y(); + } + if (vp.y() > max_y) + { + max_y = vp.y(); + } + if (vp.z() < min_z) + { + min_z = vp.z(); + } + if (vp.z() > max_z) + { + max_z = vp.z(); + } + } + } + } + min_x -= 100.0; + min_y -= 100.0; + max_x += 100.0; + max_y += 100.0; + z = (max_z + min_z) / 2.0; + } + else + { + min_x = -side_len / 2.0; + min_y = -side_len / 2.0; + max_x = side_len / 2.0; + max_y = side_len / 2.0; + xy_incr = 0.2; + z = 0.0; + } + + for (float x = min_x; x <= max_x; x += dec) + { + for (float y = min_y; y <= max_y; y += xy_incr) + { + Eigen::Vector3d vp(x, y, z); + pointcloud.push_back(vp); + intensity.push_back(0); + timestamps.push_back(0.0); + } + } + + for (float y = min_y; y <= max_y; y += dec) + { + for (float x = min_x; x <= max_x; x += xy_incr) + { + Eigen::Vector3d vp(x, y, z); + pointcloud.push_back(vp); + intensity.push_back(0); + timestamps.push_back(0.0); + } + } + + if (!exportLaz( + output_file_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_file_name << std::endl; + } +} + +std::vector get_matching_files(const std::string& directory, const std::string& pattern) +{ + std::vector matching_files; + std::regex regex_pattern(pattern); + try + { + for (const auto& entry : fs::directory_iterator(directory)) + { + if (entry.is_regular_file()) + { // Ensure it's a regular file + const std::string filename = entry.path().filename().string(); + if (std::regex_match(filename, regex_pattern)) + { + matching_files.push_back(entry.path().string()); + } + } + } + } catch (const std::exception& e) + { + std::cerr << "Error accessing directory: " << e.what() << std::endl; + } + + return matching_files; +} + +void run_multi_view_tls_registration(std::string input_file_name, TLSRegistration& tls_registration, std::string output_dir) +{ + fs::path outwd = fs::path(output_dir); + Session session; + if (!fs::exists(input_file_name)) + { + std::cout << "Provided input path does not exist." << std::endl; + return; + } + if (has_extension(input_file_name, ".json") || has_extension(input_file_name, ".mjs")) + { + std::cout << "Session file: '" << input_file_name << "'" << std::endl; + session.working_directory = fs::path(input_file_name).parent_path().string(); + session.load( + fs::path(input_file_name).string(), + tls_registration.is_decimate, + tls_registration.bucket_x, + tls_registration.bucket_y, + tls_registration.bucket_z, + tls_registration.calculate_offset); + } + else if (has_extension(input_file_name, ".reg") || has_extension(input_file_name, ".mjp")) + { + std::cout << "RESSO file: '" << input_file_name << "'" << std::endl; + session.working_directory = fs::path(input_file_name).parent_path().string(); + session.point_clouds_container.load( + session.working_directory.c_str(), + input_file_name.c_str(), + tls_registration.is_decimate, + tls_registration.bucket_x, + tls_registration.bucket_y, + tls_registration.bucket_z, + session.load_cache_mode); + } + else if (has_extension(input_file_name, ".txt")) + { + std::cout << "ETH file: '" << input_file_name << "'" << std::endl; + session.working_directory = fs::path(input_file_name).parent_path().string(); + session.point_clouds_container.load_eth( + session.working_directory.c_str(), + input_file_name.c_str(), + tls_registration.is_decimate, + tls_registration.bucket_x, + tls_registration.bucket_y, + tls_registration.bucket_z); + } + else if (!has_extension(input_file_name, "")) + { + session.point_clouds_container.point_clouds.clear(); + session.working_directory = fs::path(input_file_name).string(); + std::vector las_files; + std::vector txt_files; + for (const auto& entry : std::filesystem::directory_iterator(input_file_name)) + { + auto file_name = entry.path().string(); + if (has_extension(file_name, ".laz") || (has_extension(file_name, ".las"))) + { + las_files.push_back(file_name); + } + else if (has_extension(file_name, ".txt")) + { + txt_files.push_back(file_name); + } + } + if (las_files.size() > 0) + { + std::cout << "Las/Laz files:" << std::endl; + for (size_t i = 0; i < las_files.size(); i++) + { + std::cout << las_files[i] << std::endl; + } + session.point_clouds_container.load_whu_tls( + las_files, + tls_registration.is_decimate, + tls_registration.bucket_x, + tls_registration.bucket_y, + tls_registration.bucket_z, + tls_registration.calculate_offset, + session.load_cache_mode); + } + else if (txt_files.size() > 0) + { + std::cout << "txt files:" << std::endl; + for (size_t i = 0; i < txt_files.size(); i++) + { + std::cout << txt_files[i] << std::endl; + } + session.point_clouds_container.load_3DTK_tls( + txt_files, tls_registration.is_decimate, tls_registration.bucket_x, tls_registration.bucket_y, tls_registration.bucket_z); + } + else + { + std::cout << "No WHU-TLS / 3DTK files available in the given directory, check path." << std::endl; + return; + } + } + else + { + std::cout << "Session file: '" << input_file_name << "'" << std::endl; + session.load( + fs::path(input_file_name).string(), + tls_registration.is_decimate, + tls_registration.bucket_x, + tls_registration.bucket_y, + tls_registration.bucket_z, + tls_registration.calculate_offset); + } + std::cout << "loaded: " << session.point_clouds_container.point_clouds.size() << " point_clouds" << std::endl; + + int number_of_point = 0; + for (const auto& pc : session.point_clouds_container.point_clouds) + { + number_of_point += pc.points_local.size(); + } + session.point_clouds_container.print_point_cloud_dimension(); + + if (tls_registration.resso_upd_init.size() > 0) + { + std::cout << "RESSO file: '" << tls_registration.resso_upd_init << "'" << std::endl; + session.working_directory = fs::path(tls_registration.resso_upd_init).parent_path().string(); + if (!session.point_clouds_container.update_initial_poses_from_RESSO( + session.working_directory.c_str(), tls_registration.resso_upd_init.c_str())) + { + std::cout << "check input files" << std::endl; + return; + } + else + { + session.point_clouds_container.initial_poses_file_name = tls_registration.resso_upd_init; + std::cout << "updated: " << session.point_clouds_container.point_clouds.size() << " point_clouds" << std::endl; + } + } + if (tls_registration.resso_upd.size() > 0) + { + std::cout << "RESSO file: '" << tls_registration.resso_upd << "'" << std::endl; + session.working_directory = fs::path(tls_registration.resso_upd).parent_path().string(); + if (!session.point_clouds_container.update_poses_from_RESSO(session.working_directory.c_str(), tls_registration.resso_upd.c_str())) + { + std::cout << "check input files" << std::endl; + return; + } + else + { + std::cout << "updated: " << session.point_clouds_container.point_clouds.size() << " point_clouds" << std::endl; + session.point_clouds_container.poses_file_name = tls_registration.resso_upd; + } + } + if (tls_registration.resso_upd_inv.size() > 0) + { + std::cout << "RESSO file: '" << tls_registration.resso_upd_inv << "'" << std::endl; + session.working_directory = fs::path(tls_registration.resso_upd_inv).parent_path().string(); + if (!session.point_clouds_container.update_poses_from_RESSO_inverse( + session.working_directory.c_str(), tls_registration.resso_upd_inv.c_str())) + { + std::cout << "check input files" << std::endl; + return; + } + else + { + std::cout << "updated: " << session.point_clouds_container.point_clouds.size() << " point_clouds" << std::endl; + session.point_clouds_container.poses_file_name = tls_registration.resso_upd_inv; + } + } + + if (tls_registration.initial_pose_to_identity) + { + initial_pose_to_identity(session); + } + + if (tls_registration.use_ndt) + { + if (tls_registration.compute_only_mahalanobis_distance) + { + tls_registration.ndt.optimize( + session.point_clouds_container.point_clouds, true, tls_registration.compute_mean_and_cov_for_bucket); + } + else if (tls_registration.use_lie_algebra_left_jacobian_ndt) + { + tls_registration.ndt.optimize_lie_algebra_left_jacobian( + session.point_clouds_container.point_clouds, tls_registration.compute_mean_and_cov_for_bucket); + } + else if (tls_registration.use_lie_algebra_right_jacobian_ndt) + { + tls_registration.ndt.optimize_lie_algebra_right_jacobian( + session.point_clouds_container.point_clouds, tls_registration.compute_mean_and_cov_for_bucket); + } + else + { + std::cout << "No additional optimization option selected for NDT: using default..." << std::endl; + tls_registration.ndt.optimize( + session.point_clouds_container.point_clouds, false, tls_registration.compute_mean_and_cov_for_bucket); + } + } + + if (tls_registration.use_icp) + { + if (tls_registration.point_to_point_source_to_target) + { + tls_registration.icp.optimization_point_to_point_source_to_target(session.point_clouds_container); + } + else if (tls_registration.use_lie_algebra_left_jacobian_icp) + { + tls_registration.icp.optimize_source_to_target_lie_algebra_left_jacobian(session.point_clouds_container); + } + else if (tls_registration.use_lie_algebra_right_jacobian_icp) + { + tls_registration.icp.optimize_source_to_target_lie_algebra_right_jacobian(session.point_clouds_container); + } + else if (tls_registration.point_to_point_source_to_target_compute_rms) + { + double rms = 0.0; + tls_registration.icp.optimization_point_to_point_source_to_target_compute_rms(session.point_clouds_container, rms); + std::cout << "rms(optimization_point_to_point_source_to_target): " << rms << std::endl; + } + else + { + std::cout << "No optimization option selected for ICP: skipping..." << std::endl; + } + } + + if (tls_registration.use_plane_features) + { + if (tls_registration.point_to_projection_onto_plane_source_to_target) + { + tls_registration.registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target( + session.point_clouds_container); + } + else if (tls_registration.use_lie_algebra_left_jacobian_plane_features) + { + tls_registration.registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_left_jacobian( + session.point_clouds_container); + } + else if (tls_registration.use_lie_algebra_right_jacobian_plane_features) + { + tls_registration.registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_right_jacobian( + session.point_clouds_container); + } + else if (tls_registration.point_to_plane_source_to_target_dot_product) + { + tls_registration.registration_plane_feature.optimize_point_to_plane_source_to_target(session.point_clouds_container); + } + else if (tls_registration.point_to_plane_source_to_target) + { + tls_registration.registration_plane_feature.optimize_distance_point_to_plane_source_to_target(session.point_clouds_container); + } + else if (tls_registration.plane_to_plane_source_to_target) + { + tls_registration.registration_plane_feature.optimize_plane_to_plane_source_to_target(session.point_clouds_container); + } + else + { + std::cout << "No optimization option selected for the plane features: skipping..." << std::endl; + } + } + + // TODO: add with GTSAM and with MANIF + if (tls_registration.use_pgslam) + { + tls_registration.pose_graph_slam.ndt_bucket_size[0] = tls_registration.ndt.bucket_size[0]; + tls_registration.pose_graph_slam.ndt_bucket_size[1] = tls_registration.ndt.bucket_size[1]; + tls_registration.pose_graph_slam.ndt_bucket_size[2] = tls_registration.ndt.bucket_size[2]; + tls_registration.pose_graph_slam.optimize(session.point_clouds_container); + } + + if (output_dir.length() > 0) + { + if (session.point_clouds_container.initial_poses_file_name.empty() && tls_registration.save_initial_poses) + { + std::string initial_poses_file_name = (outwd / "session_step2_ini_poses.mri").string(); + std::cout << "saving initial poses to: " << initial_poses_file_name << std::endl; + session.point_clouds_container.save_poses(initial_poses_file_name, false); + } + + if (session.point_clouds_container.poses_file_name.empty() && tls_registration.save_poses) + { + std::string poses_file_name = (outwd / "session_step2_poses.mrp").string(); + std::cout << "saving poses to: " << poses_file_name << std::endl; + session.point_clouds_container.save_poses(poses_file_name, false); + } + + session.save( + (outwd / "session_step2.mjs").string(), + session.point_clouds_container.poses_file_name, + session.point_clouds_container.initial_poses_file_name, + false); + std::cout << "saving result to: " << session.point_clouds_container.poses_file_name << std::endl; + session.point_clouds_container.save_poses(fs::path(session.point_clouds_container.poses_file_name).string(), false); + } + + if (tls_registration.save_laz) + { + save_all_to_las(session, (outwd / "all_step_2.laz").string(), false, true); + } + if (tls_registration.save_las) + { + save_all_to_las(session, (outwd / "all_step_2.las").string(), false, true); + } + if (tls_registration.save_as_separate_las) + { + save_separately_to_las(session, outwd, ".las"); + } + if (tls_registration.save_as_separate_laz) + { + save_separately_to_las(session, outwd, ".laz"); + } + + if (tls_registration.save_trajectories_laz) + { + save_trajectories_to_laz( + session, + (outwd / "trajectories.laz").string(), + tls_registration.curve_consecutive_distance_meters, + tls_registration.not_curve_consecutive_distance_meters, + tls_registration.is_trajectory_export_downsampling); + } + + if (tls_registration.save_gnss_laz) + { + tls_registration.gnss.save_to_laz( + (outwd / "gnss.laz").string(), + session.point_clouds_container.offset.x(), + session.point_clouds_container.offset.y(), + session.point_clouds_container.offset.z()); + } + + if (tls_registration.save_scale_board_laz) + { + save_scale_board_to_laz( + session, (outwd / "scale_board.laz").string(), tls_registration.scale_board_dec, tls_registration.scale_board_side_len); + } + + if (tls_registration.save_trajectories_csv) + { + save_trajectories( + session, + (outwd / "trajectories.csv").string(), + tls_registration.curve_consecutive_distance_meters, + tls_registration.not_curve_consecutive_distance_meters, + tls_registration.is_trajectory_export_downsampling, + tls_registration.write_lidar_timestamp, + tls_registration.write_unix_timestamp, + tls_registration.use_quaternions, + false); + } + if (tls_registration.save_trajectories_dxf) + { + save_trajectories( + session, + (outwd / "trajectories.dxf").string(), + tls_registration.curve_consecutive_distance_meters, + tls_registration.not_curve_consecutive_distance_meters, + tls_registration.is_trajectory_export_downsampling, + tls_registration.write_lidar_timestamp, + tls_registration.write_unix_timestamp, + tls_registration.use_quaternions, + true); + } +} \ No newline at end of file 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 new file mode 100644 index 00000000..0553abb5 --- /dev/null +++ b/apps/multi_view_tls_registration_legacy/multi_view_tls_registration.h @@ -0,0 +1,181 @@ + +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace fs = std::filesystem; + +struct TLSRegistration +{ + // NDT + bool use_ndt = false; + NDT ndt; + bool compute_only_mahalanobis_distance = false; + bool compute_mean_and_cov_for_bucket = false; + bool use_lie_algebra_left_jacobian_ndt = false; + bool use_lie_algebra_right_jacobian_ndt = false; + void set_zoller_frohlich_tls_imager_5006i_errors() + { + ndt.sigma_r = 0.0068; + ndt.sigma_polar_angle = 0.007 / 180.0 * M_PI; + ndt.sigma_azimuthal_angle = 0.007 / 180.0 * M_PI; + } + void set_zoller_frohlich_tls_imager_5010c_errors() + { + ndt.sigma_r = 0.01; + ndt.sigma_polar_angle = 0.007 / 180.0 * M_PI; + ndt.sigma_azimuthal_angle = 0.007 / 180.0 * M_PI; + } + void set_zoller_frohlich_tls_imager_5016_errors() + { + ndt.sigma_r = 0.00025; + ndt.sigma_polar_angle = 0.004 / 180.0 * M_PI; + ndt.sigma_azimuthal_angle = 0.004 / 180.0 * M_PI; + } + void set_faro_focus3d_errors() + { + ndt.sigma_r = 0.001; + ndt.sigma_polar_angle = 19.0 * (1.0 / 3600.0) / 180.0 * M_PI; + ndt.sigma_azimuthal_angle = 19.0 * (1.0 / 3600.0) / 180.0 * M_PI; + } + void set_leica_scanstation_c5_c10_errors() + { + ndt.sigma_r = 0.006; + ndt.sigma_polar_angle = 0.00006; + ndt.sigma_azimuthal_angle = 0.00006; + } + void set_riegl_vz400_errors() + { + ndt.sigma_r = 0.005; + ndt.sigma_polar_angle = 0.0005 / 180.0 * M_PI + 0.0003; // Laser Beam Dicvergence + ndt.sigma_azimuthal_angle = 0.0005 / 180.0 * M_PI + 0.0003; // Laser Beam Dicvergence + } + void set_leica_hds6100_errors() + { + ndt.sigma_r = 0.009; + ndt.sigma_polar_angle = 0.000125; + ndt.sigma_azimuthal_angle = 0.000125; + } + void set_leica_p40_errors() + { + ndt.sigma_r = 0.0012; + ndt.sigma_polar_angle = 8.0 / 3600; + ndt.sigma_azimuthal_angle = 8.0 / 3600; + } + + void set_livox_mid360_errors() + { + ndt.sigma_r = 0.02; + ndt.sigma_polar_angle = 0.15 / 180.0 * M_PI; + ndt.sigma_azimuthal_angle = 0.15 / 180.0 * M_PI; + } + + // ICP + bool use_icp = false; + ICP icp; + bool point_to_point_source_to_target = true; + bool use_lie_algebra_left_jacobian_icp = false; + bool use_lie_algebra_right_jacobian_icp = false; + bool point_to_point_source_to_target_compute_rms = false; + + // Plane features + bool use_plane_features = false; + RegistrationPlaneFeature registration_plane_feature; + bool point_to_projection_onto_plane_source_to_target = true; + bool use_lie_algebra_left_jacobian_plane_features = false; + bool use_lie_algebra_right_jacobian_plane_features = false; + bool point_to_plane_source_to_target_dot_product = false; + bool point_to_plane_source_to_target = false; + bool plane_to_plane_source_to_target = false; + + // PGSLAM + bool use_pgslam = false; + PoseGraphSLAM pose_graph_slam; + + // GNSS + GNSS gnss; + + // Loading + bool calculate_offset; // Whether to calculate offset to point cloud on loading + bool is_decimate = true; // Whether to decimate point clouds on loading + double bucket_x = 0.1; // Bucket size for decimation in x dimension + double bucket_y = 0.1; // Bucket size for decimation in y dimension + double bucket_z = 0.1; // Bucket size for decimation in z dimension + std::string resso_upd_init = ""; // Path to RESSO initial poses + std::string resso_upd = ""; // Path to RESSO poses + std::string resso_upd_inv = ""; // Path to RESSO inverse poses + + // Registration + bool initial_pose_to_identity = true; // Whether to set first pose as identity and recalculate the rest relatively to it + + // Export + bool save_las = false; // Save resulting point cloud as las + bool save_laz = false; // Save resulting point cloud as laz + bool save_as_separate_las = false; // Whether to save all scans as separate global scans in las format + bool save_as_separate_laz = false; // Whether to save all scans as separate global scans in laz format + bool save_trajectories_laz = false; // Save laz with all trajectories + bool save_gnss_laz = false; // Save laz with GNSS data + bool save_scale_board_laz = false; // Save laz with scale board + float scale_board_dec = 0.1; // Decimation for scale board laz export + float scale_board_side_len = -1.0; // Range covered in x and y dimensions + bool save_initial_poses = false; // Path where initial poses are saved + bool save_poses = false; // Path where poses are saved + bool is_trajectory_export_downsampling = false; // Whether to downsample trajectory on export + float curve_consecutive_distance_meters = 1.0f; // Meters after which trajectory point is saved (if downsampling and curve is detected) + float not_curve_consecutive_distance_meters = + 0.05f; // Meters after which trajectory point is saved (if downsampling and no curve detected) + bool save_trajectories_csv = false; // Save trajectories as csv + bool save_trajectories_dxf = false; // Save trajectories as dxf + bool write_lidar_timestamp = true; // Whether lidar timestamp is wrriten to csv trajectory output + bool write_unix_timestamp = false; // Whether unix timestamp is written to csv trajectory + bool use_quaternions = true; // Whether quaternions are used when writing csv trajectory +}; + +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( + const Session& session, + std::string output_file_name, + float curve_consecutive_distance_meters, + float not_curve_consecutive_distance_meters, + bool is_trajectory_export_downsampling); + +void save_scale_board_to_laz(const Session& session, std::string output_file_name, float dec, float side_len = -1.0); + +void createDXFPolyline(const std::string& filename, const std::vector& points); + +// void load_available_geo_points(Session& session, std::string input_file_name); + +void save_trajectories( + Session& session, + std::string output_file_name, + float curve_consecutive_distance_meters, + float not_curve_consecutive_distance_meters, + bool is_trajectory_export_downsampling, + bool write_lidar_timestamp = true, + bool write_unix_timestamp = false, + bool use_quaternions = true, + bool save_to_dxf = false); + +void run_multi_view_tls_registration(std::string input_file_name, TLSRegistration& tls_registration, std::string output_dir = ""); 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 new file mode 100644 index 00000000..70f21708 --- /dev/null +++ b/apps/multi_view_tls_registration_legacy/multi_view_tls_registration_gui.cpp @@ -0,0 +1,4243 @@ +#include + +#include + +#include +#include +#include +#include + +#include + +#include + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include +#include +#include + +#include "../lidar_odometry_step_1/lidar_odometry_utils.h" +#include "multi_view_tls_registration.h" + +#include + +#include "WGS84toCartesian/WGS84toCartesian.hpp" +#include "wgs84_do_puwg92/wgs84_do_puwg92.h" + +#include +#ifdef _WIN32 +#include "resource.h" +#include + +#endif + +/////////////////////////////////////////////////////////////////////////////////// + +#ifdef _WIN32 +bool consWin = true; +#endif +bool consImGui = false; + +std::string winTitle = std::string("Step 2 (Multi view TSL registration) ") + HDMAPPING_VERSION_STRING; + +std::vector infoLines = { + "This program is second step in MANDEYE process", + "", + "It refines trajectory (e.g with loop closure)", + "It refines trajectory with many approaches (e.g. Iterative Closest Point, Normal Distributions Transform)", + "It exports session as rigid point cloud to single LAZ file", + "LAZ files are the product of MANDEYE process (open them with Cloud Compare)", +}; + +// App specific shortcuts (Type and Shortcut are just for easy reference) +static const std::vector appShortcuts = { { "Normal keys", "A", "" }, + { "", "Ctrl+A", "point cloud Alignment" }, + { "", "B", "" }, + { "", "Ctrl+B", "" }, + { "", "C", "" }, + { "", "Ctrl+C", "Control points" }, + { "", "D", "" }, + { "", "Ctrl+D", "" }, + { "", "E", "" }, + { "", "Ctrl+E", "lio segments Editor" }, + { "", "F", "" }, + { "", "Ctrl+F", "" }, + { "", "G", "" }, + { "", "Ctrl+G", "Ground control points" }, + { "", "H", "" }, + { "", "Ctrl+H", "" }, + { "", "I", "" }, + { "", "Ctrl+I", "" }, + { "", "J", "" }, + { "", "Ctrl+K", "" }, + { "", "K", "" }, + { "", "Ctrl+K", "" }, + { "", "L", "" }, + { "", "Ctrl+L", "manual Loop closure" }, + { "", "M", "" }, + { "", "Ctrl+M", "" }, + { "", "N", "" }, + { "", "Ctrl+N", "" }, + { "", "O", "" }, + { "", "Ctrl+O", "Open session" }, + { "", "P", "" }, + { "", "Ctrl+P", "Pose graph slam" }, + { "", "Q", "" }, + { "", "Ctrl+Q", "" }, + { "", "R", "" }, + { "", "Ctrl+R", "Random cloud colors" }, + { "", "Shift+R", "" }, + { "", "S", "" }, + { "", "Ctrl+S", "Save session" }, + { "", "Ctrl+Shift+S", "Save subsession" }, + { "", "T", "" }, + { "", "Ctrl+T", "Solid cloud color" }, + { "", "U", "" }, + { "", "Ctrl+U", "" }, + { "", "V", "" }, + { "", "Ctrl+V", "" }, + { "", "W", "" }, + { "", "Ctrl+W", "" }, + { "", "X", "" }, + { "", "Ctrl+X", "" }, + { "", "Y", "" }, + { "", "Ctrl+Y", "" }, + { "", "Z", "" }, + { "", "Ctrl+Z", "" }, + { "", "Shift+Z", "" }, + { "", "1-9", "" }, + { "Special keys", "Up arrow", "" }, + { "", "Shift + up arrow", "" }, + { "", "Ctrl + up arrow", "" }, + { "", "Down arrow", "" }, + { "", "Shift + down arrow", "" }, + { "", "Ctrl + down arrow", "" }, + { "", "Left arrow", "" }, + { "", "Shift + left arrow", "" }, + { "", "Ctrl + left arrow", "" }, + { "", "Right arrow", "" }, + { "", "Shift + right arrow", "" }, + { "", "Ctrl + right arrow", "" }, + { "", "Pg down", "" }, + { "", "Pg up", "" }, + { "", "- key", "" }, + { "", "+ key", "" }, + { "Mouse related", "Left click + drag", "" }, + { "", "Right click + drag", "n" }, + { "", "Scroll", "" }, + { "", "Shift + scroll", "" }, + { "", "Shift + drag", "" }, + { "", "Ctrl + left click", "" }, + { "", "Ctrl + right click", "" }, + { "", "Ctrl + middle click", "" } }; + +namespace fs = std::filesystem; + +static bool show_demo_window = true; +static bool show_another_window = false; + +bool gnssWithOffset = false; + +// radio button selectors +static int NDTnomSelection = 0; +static int NDTpeSelection = 0; +static int NDT3dSelection = 0; +static int ICPnomSelection = 0; +static int ICPpeSelection = 0; +static int ICP3dSelection = 0; +static int RPFnomSelection = 0; +static int RPFpeSelection = 0; +static int RPF3dSelection = 0; +static int PGSnomSelection = 0; +static int PGSpeSelection = 0; +static int PGS3dSelection = 0; +static int PGSpwmtSelection = 0; + +std::string session_file_name = ""; +int session_total_number_of_points = 0; +// bool dynamicSubsampling = true; +// static double lastAdjustTime = 0.0; // last time we changed subsampling +// const double cooldownSeconds = 1; // wait between auto adjustments +// static float fps_avg = 60.0f; + +bool is_pca_gui = false; +bool is_ndt_gui = true; +bool is_icp_gui = false; +bool is_rpf_gui = false; +bool is_pose_graph_slam = false; +bool is_manual_analisys = false; +bool is_loop_closure_gui = false; +bool is_lio_segments_gui = false; +bool is_settings_gui = true; +bool is_translate_gui = false; + +struct TranslateTool +{ + enum class Step + { + Idle, + PickOrigin, + PickXAxis, + PickYHint, + Ready + }; + Step step = Step::Idle; + Eigen::Vector3d origin = Eigen::Vector3d::Zero(); + Eigen::Vector3d x_point = Eigen::Vector3d::Zero(); + Eigen::Vector3d y_hint = Eigen::Vector3d::Zero(); + Eigen::Affine3d transform = Eigen::Affine3d::Identity(); + bool has_transform = false; + float plane_z = 0.0f; +}; +TranslateTool translate_tool; + +bool fillInSession = true; + +TLSRegistration tls_registration; +ObservationPicking observation_picking; +std::vector picked_points; + +bool new_loop_closure_index = false; +int num_edge_extended_before = 0; +int num_edge_extended_after = 0; +int index_loop_closure_source = 0; +int index_loop_closure_target = 0; +int index_begin = 0; +int index_end = 0; + +ColorScheme csPointCloud = CS_SOLID; +ColorScheme csTrajectory = CS_SOLID; + +float m_gizmo[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; + +bool manipulate_only_marked_gizmo = false; + +Session session; +PointClouds::PointCloudDimensions session_dims; +bool session_loaded = false; +std::vector geoids; +std::string selected_geoid_model; + +// these functions performs experiment from paper +//@article +//{BEDKOWSKI2023113199, +// title = {Benchmark of multi-view Terrestrial Laser Scanning Point Cloud data registration algorithms}, +// journal = {Measurement}, +// pages = {113199}, +// year = {2023}, +// issn = {0263-2241}, +// doi = {https://doi.org/10.1016/j.measurement.2023.113199}, +// url = {https://www.sciencedirect.com/science/article/pii/S0263224123007637}, +// author = {Janusz Będkowski}, +// keywords = {TLS, Point cloud, Open-source, Multi-view data registration, LiDAR data metrics, Robust loss function, Tait-bryan +// angles, Quaternions, Rodrigues’ formula, Lie algebra, Rotation matrix parameterization}, abstract = {This study addresses multi-view +// Terrestrial Laser Scanning Point Cloud data registration methods. Multiple rigid point cloud data registration is mandatory for +// aligning all scans into a common reference frame and it is still considered a challenge looking from a large-scale surveys point of +// view. The goal of this work is to support the development of cutting-edge registration methods in geoscience and mobile robotics +// domains. This work evaluates 3 data sets of total 20 scenes available in the literature. This paper provides a novel open-source +// framework for multi-view Terrestrial Laser Scanning Point Cloud data registration benchmarks. The goal was to verify experimentally +// which registration variant can improve the open-source data looking from the quantitative and qualitative points of view. In +// particular, the following scanners provided measurement data: Z+F TLS Imager 5006i, Z+F TLS Imager 5010C, Leica ScanStation C5, +// Leica ScanStation C10, Leica P40 and Riegl VZ-400. The benchmark shows an impact of the metric e.g. point to point, point to +// projection onto a plane, plane to plane etc..., rotation matrix parameterization (Tait-Bryan, quaternion, Rodrigues) and other +// implementation variations (e.g. multi-view Normal Distributions Transform, Pose Graph SLAM approach) onto the multi-view data +// registration accuracy and performance. An open-source project is created and it can be used for improving existing data sets +// reported in the literature, it is the added value of the presented research. The combination of metrics, rotation matrix +// parameterization and optimization algorithms creates hundreds of possible approaches. It is shown that chosen metric is a dominant +// factor in data registration. The rotation parameterization and other degrees of freedom of proposed variants are rather negligible +// compared with chosen metric. Most of the proposed approaches improve registered reference data provided by other researchers. Only +// for 2 from 20 scenes it was not possible to provide significant improvement. The largest improvements are evident for large-scale +// scenes. The project is available and maintained at https://github.com/MapsHD/HDMapping.} +// } + +void export_result_to_folder(std::string output_folder_name, ObservationPicking& observation_picking, Session& session); +void perform_experiment_on_windows( + Session& session, + ObservationPicking& observation_picking, + ICP& icp, + NDT& ndt, + RegistrationPlaneFeature& registration_plane_feature, + PoseGraphSLAM& pose_graph_slam); +void perform_experiment_on_linux( + Session& session, + ObservationPicking& observation_picking, + ICP& icp, + NDT& ndt, + RegistrationPlaneFeature& registration_plane_feature, + PoseGraphSLAM& pose_graph_slam); +double compute_rms(bool initial, Session& session, ObservationPicking& observation_picking); +void reset_poses(Session& session); +void translate_gui(); +void draw_translate_preview(); + +/////////////////////////////////////////////////////////////////////////////////// + +void ndt_gui() +{ + ImGui::InputFloat3("Bucket size (x,y,z) [m]", tls_registration.ndt.bucket_size); + if (tls_registration.ndt.bucket_size[0] < 0.01) + tls_registration.ndt.bucket_size[0] = 0.01f; + if (tls_registration.ndt.bucket_size[1] < 0.01) + tls_registration.ndt.bucket_size[1] = 0.01f; + if (tls_registration.ndt.bucket_size[2] < 0.01) + tls_registration.ndt.bucket_size[2] = 0.01f; + + ImGui::PushItemWidth(ImGuiNumberWidth); + ImGui::InputInt("Number of threads", &tls_registration.ndt.number_of_threads); + if (tls_registration.ndt.number_of_threads < 1) + tls_registration.ndt.number_of_threads = 1; + ImGui::SameLine(); + ImGui::InputInt("Number of iterations", &tls_registration.ndt.number_of_iterations); + if (tls_registration.ndt.number_of_iterations < 1) + tls_registration.ndt.number_of_iterations = 1; + ImGui::PopItemWidth(); + + ImGui::Checkbox("Fix first node (add I to first pose in Hessian)", &tls_registration.ndt.is_fix_first_node); + + ImGui::Text("Nonlinear optimization method:"); + ImGui::SameLine(); + ImGui::RadioButton("Gauss-Newton", &NDTnomSelection, 0); + ImGui::SameLine(); + ImGui::RadioButton("Levenberg-Marguardt", &NDTnomSelection, 1); + + tls_registration.ndt.is_gauss_newton = (NDTnomSelection == 0); + tls_registration.ndt.is_levenberg_marguardt = (NDTnomSelection == 1); + + ImGui::Text("Poses expressed as:"); + ImGui::SameLine(); + ImGui::RadioButton("camera<-world (cw)", &NDTpeSelection, 0); + ImGui::SameLine(); + ImGui::RadioButton("camera->world (wc)", &NDTpeSelection, 1); + + tls_registration.ndt.is_cw = (NDTpeSelection == 0); + tls_registration.ndt.is_wc = (NDTpeSelection == 1); + + ImGui::Text("Parameterizations of 3D rotation:"); + ImGui::RadioButton("Tait-Bryan angles (om fi ka: RxRyRz)", &NDT3dSelection, 0); + ImGui::SameLine(); + ImGui::RadioButton("Quaternion (q0 q1 q2 q3)", &NDT3dSelection, 1); + ImGui::SameLine(); + ImGui::RadioButton("Rodrigues (sx sy sz)", &NDT3dSelection, 2); + + tls_registration.ndt.is_tait_bryan_angles = (NDT3dSelection == 0); + tls_registration.ndt.is_quaternion = (NDT3dSelection == 1); + tls_registration.ndt.is_rodrigues = (NDT3dSelection == 2); + + if (ImGui::Button("Optimization")) + { + double rms_initial = 0.0; + double rms_final = 0.0; + double mui = 0.0; + // ndt.optimize(point_clouds_container.point_clouds, rms_initial, rms_final, mui); + // spdlog::info("mui: " << mui << " rms_initial: " << rms_initial << " rms_final: " << rms_final << std::endl; + tls_registration.ndt.optimize(session.point_clouds_container.point_clouds, false, tls_registration.compute_mean_and_cov_for_bucket); + } + + if (ImGui::Button("Compute mean Mahalanobis distance")) + { + double rms_initial = 0.0; + double rms_final = 0.0; + double mui = 0.0; + + tls_registration.ndt.optimize(session.point_clouds_container.point_clouds, true, tls_registration.compute_mean_and_cov_for_bucket); + } + if (ImGui::IsItemHovered()) + { + ImGui::BeginTooltip(); + ImGui::Text( + "Average of all Mahalanobis distances between transformed source points\nand their corresponding Gaussian cells, where:"); + ImGui::Text( + "Mahalanobis distance measures how far a point is from the mean of a multivariate Gaussian distribution,\ntaking into " + "account the covariance (shape and orientation) of that distribution"); + ImGui::EndTooltip(); + } + + ImGui::Separator(); + + ImGui::Text("NDT optimization Lie algebra:"); + ImGui::SameLine(); + if (ImGui::Button("left Jacobian")) + { + tls_registration.ndt.optimize_lie_algebra_left_jacobian( + session.point_clouds_container.point_clouds, tls_registration.compute_mean_and_cov_for_bucket); + } + ImGui::SameLine(); + if (ImGui::Button("right Jacobian")) + { + tls_registration.ndt.optimize_lie_algebra_right_jacobian( + session.point_clouds_container.point_clouds, tls_registration.compute_mean_and_cov_for_bucket); + } + + ImGui::Separator(); + + ImGui::Checkbox("Generalized", &tls_registration.ndt.is_generalized); + ImGui::BeginDisabled(!tls_registration.ndt.is_generalized); + { + ImGui::PushItemWidth(ImGuiNumberWidth); + ImGui::InputDouble("sigma_r", &tls_registration.ndt.sigma_r, 0.01, 0.01); + ImGui::InputDouble("sigma_polar_angle_rad", &tls_registration.ndt.sigma_polar_angle, 0.0001, 0.0001); + ImGui::InputDouble("sigma_azimuthal_angle_rad", &tls_registration.ndt.sigma_azimuthal_angle, 0.0001, 0.0001); + ImGui::InputInt("num_extended_points", &tls_registration.ndt.num_extended_points, 1, 1); + ImGui::PopItemWidth(); + + ImGui::Checkbox("compute_mean_and_cov_for_bucket", &tls_registration.compute_mean_and_cov_for_bucket); + } + ImGui::EndDisabled(); + + ImGui::Text("Set error presets:"); + ImGui::Text("Zoller+Fröhlich TLS Imager"); + ImGui::SameLine(); + if (ImGui::Button("5006i")) + tls_registration.set_zoller_frohlich_tls_imager_5006i_errors(); + ImGui::SameLine(); + if (ImGui::Button("5010C")) + tls_registration.set_zoller_frohlich_tls_imager_5010c_errors(); + ImGui::SameLine(); + if (ImGui::Button("5016")) + tls_registration.set_zoller_frohlich_tls_imager_5016_errors(); + + ImGui::Text("Leica"); + ImGui::SameLine(); + if (ImGui::Button("ScanStation C5 C10")) + tls_registration.set_leica_scanstation_c5_c10_errors(); + ImGui::SameLine(); + if (ImGui::Button("Leica HDS6100")) + tls_registration.set_leica_hds6100_errors(); + ImGui::SameLine(); + if (ImGui::Button("Leica P40")) + tls_registration.set_leica_p40_errors(); + + if (ImGui::Button("Faro Focus3D")) + tls_registration.set_faro_focus3d_errors(); + ImGui::SameLine(); + if (ImGui::Button("Riegl VZ400")) + tls_registration.set_riegl_vz400_errors(); + ImGui::SameLine(); + if (ImGui::Button("Livox mid360")) + tls_registration.set_livox_mid360_errors(); +} + +void icp_gui() +{ + ImGui::PushItemWidth(ImGuiNumberWidth); + ImGui::InputFloat("Search radius", &tls_registration.icp.search_radius, 0.01f, 0.1f); + if (tls_registration.icp.search_radius < 0.01f) + tls_registration.icp.search_radius = 0.01f; + if (tls_registration.icp.search_radius > 2.0f) + tls_registration.icp.search_radius = 2.0f; + + ImGui::InputInt("Number of threads", &tls_registration.icp.number_of_threads); + if (tls_registration.icp.number_of_threads < 1) + tls_registration.icp.number_of_threads = 1; + ImGui::SameLine(); + ImGui::InputInt("Number of iterations", &tls_registration.icp.number_of_iterations); + if (tls_registration.icp.number_of_iterations < 1) + tls_registration.icp.number_of_iterations = 1; + ImGui::PopItemWidth(); + + ImGui::Checkbox("Adaptive robust kernel", &tls_registration.icp.is_adaptive_robust_kernel); + ImGui::SameLine(); + ImGui::Checkbox("Fix first node (add I to first pose in Hessian)", &tls_registration.icp.is_fix_first_node); + + ImGui::Text("Nonlinear optimization method:"); + ImGui::SameLine(); + ImGui::RadioButton("Gauss-Newton", &ICPnomSelection, 0); + ImGui::SameLine(); + ImGui::RadioButton("Levenberg-Marguardt", &ICPnomSelection, 1); + + tls_registration.icp.is_gauss_newton = (ICPnomSelection == 0); + tls_registration.icp.is_levenberg_marguardt = (ICPnomSelection == 1); + + ImGui::Text("Poses expressed as:"); + ImGui::SameLine(); + ImGui::RadioButton("camera<-world (cw)", &ICPpeSelection, 0); + ImGui::SameLine(); + ImGui::RadioButton("camera->world (wc)", &ICPpeSelection, 1); + + tls_registration.icp.is_cw = (ICPpeSelection == 0); + tls_registration.icp.is_wc = (ICPpeSelection == 1); + + ImGui::Text("Parameterizations of 3D rotation:"); + ImGui::RadioButton("Tait-Bryan angles (om fi ka: RxRyRz)", &ICP3dSelection, 0); + ImGui::SameLine(); + ImGui::RadioButton("Quaternion (q0 q1 q2 q3)", &ICP3dSelection, 1); + ImGui::SameLine(); + ImGui::RadioButton("Rodrigues (sx sy sz)", &ICP3dSelection, 2); + + tls_registration.icp.is_tait_bryan_angles = (ICP3dSelection == 0); + tls_registration.icp.is_quaternion = (ICP3dSelection == 1); + tls_registration.icp.is_rodrigues = (ICP3dSelection == 2); + + if (ImGui::Button("Optimization point to point source to target")) + tls_registration.icp.optimization_point_to_point_source_to_target(session.point_clouds_container); + + ImGui::Separator(); + + ImGui::Text("Optimization source to target Lie-algebra:"); + ImGui::SameLine(); + if (ImGui::Button("left Jacobian")) + tls_registration.icp.optimize_source_to_target_lie_algebra_left_jacobian(session.point_clouds_container); + + ImGui::SameLine(); + if (ImGui::Button("right Jacobian")) + tls_registration.icp.optimize_source_to_target_lie_algebra_right_jacobian(session.point_clouds_container); + + ImGui::Separator(); + + if (ImGui::Button("Compute RMS (optimization_point_to_point_source_to_target)")) + { + double rms = 0.0; + tls_registration.icp.optimization_point_to_point_source_to_target_compute_rms(session.point_clouds_container, rms); + spdlog::info("RMS (optimization_point_to_point_source_to_target): {}", rms); + } +} + +void rpf_gui() +{ + ImGui::PushItemWidth(ImGuiNumberWidth); + ImGui::InputFloat("Search radius", &tls_registration.registration_plane_feature.search_radius, 0.01, 2.0); + if (tls_registration.registration_plane_feature.search_radius < 0.01) + tls_registration.registration_plane_feature.search_radius = 0.01; + if (tls_registration.registration_plane_feature.search_radius > 2.0) + tls_registration.registration_plane_feature.search_radius = 2.0; + + ImGui::InputInt("Number of threads", &tls_registration.registration_plane_feature.number_of_threads); + if (tls_registration.registration_plane_feature.number_of_threads < 1) + tls_registration.registration_plane_feature.number_of_threads = 1; + ImGui::SameLine(); + ImGui::InputInt("Number of iterations", &tls_registration.registration_plane_feature.number_of_iterations); + if (tls_registration.registration_plane_feature.number_of_iterations < 1) + tls_registration.registration_plane_feature.number_of_iterations = 1; + ImGui::PopItemWidth(); + + ImGui::Checkbox("Adaptive robust kernel", &tls_registration.registration_plane_feature.is_adaptive_robust_kernel); + ImGui::SameLine(); + ImGui::Checkbox("Fix first node (add I to first pose in Hessian)", &tls_registration.registration_plane_feature.is_fix_first_node); + + ImGui::Text("Nonlinear optimization method:"); + ImGui::SameLine(); + ImGui::RadioButton("Gauss-Newton", &RPFnomSelection, 0); + ImGui::SameLine(); + ImGui::RadioButton("Levenberg-Marguardt", &RPFnomSelection, 1); + + tls_registration.registration_plane_feature.is_gauss_newton = (RPFnomSelection == 0); + tls_registration.registration_plane_feature.is_levenberg_marguardt = (RPFnomSelection == 1); + + ImGui::Text("Poses expressed as:"); + ImGui::SameLine(); + ImGui::RadioButton("camera<-world (cw)", &RPFpeSelection, 0); + ImGui::SameLine(); + ImGui::RadioButton("camera->world (wc)", &RPFpeSelection, 1); + + tls_registration.registration_plane_feature.is_cw = (RPFpeSelection == 0); + tls_registration.registration_plane_feature.is_wc = (RPFpeSelection == 1); + + ImGui::Text("Parameterizations of 3D rotation:"); + ImGui::RadioButton("Tait-Bryan angles (om fi ka: RxRyRz)", &RPF3dSelection, 0); + ImGui::SameLine(); + ImGui::RadioButton("Quaternion (q0 q1 q2 q3)", &RPF3dSelection, 1); + ImGui::SameLine(); + ImGui::RadioButton("Rodrigues (sx sy sz)", &RPF3dSelection, 2); + + tls_registration.registration_plane_feature.is_tait_bryan_angles = (RPF3dSelection == 0); + tls_registration.registration_plane_feature.is_quaternion = (RPF3dSelection == 1); + tls_registration.registration_plane_feature.is_rodrigues = (RPF3dSelection == 2); + + ImGui::Separator(); + ImGui::Text("Optimize point to projection onto plane source to target:"); + if (ImGui::Button("Basic Jacobian")) + tls_registration.registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target( + session.point_clouds_container); + ImGui::SameLine(); + if (ImGui::Button("Lie-algebra left Jacobian")) + tls_registration.registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_left_jacobian( + session.point_clouds_container); + ImGui::SameLine(); + if (ImGui::Button("Lie-algebra right Jacobian")) + tls_registration.registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_right_jacobian( + session.point_clouds_container); + + ImGui::Separator(); + + ImGui::Text("Optimize source to target:"); + if (ImGui::Button("point to plane (using dot product)")) + tls_registration.registration_plane_feature.optimize_point_to_plane_source_to_target(session.point_clouds_container); + ImGui::SameLine(); + if (ImGui::Button("distance point to plane")) + tls_registration.registration_plane_feature.optimize_distance_point_to_plane_source_to_target(session.point_clouds_container); + ImGui::SameLine(); + if (ImGui::Button("plane to plane")) + tls_registration.registration_plane_feature.optimize_plane_to_plane_source_to_target(session.point_clouds_container); +} + +void pca_gui() +{ + ImGui::Begin("Point cloud alignment", &is_pca_gui, ImGuiWindowFlags_MenuBar); + { + if (ImGui::BeginMenuBar()) + { + bool justPushed = false; + + if (is_ndt_gui) + ImGui::PushStyleColor(ImGuiCol_Button, orangeBorder); + if (ImGui::Button("Normal Distributions Transform")) + { + if (!is_ndt_gui) + { + is_ndt_gui = true; + is_icp_gui = false; + is_rpf_gui = false; + justPushed = true; + } + } + if (is_ndt_gui && !justPushed) + ImGui::PopStyleColor(); + if (ImGui::IsItemHovered()) + { + ImGui::BeginTooltip(); + ImGui::Text( + "Probabilistic alternative to ICP that models one cloud (the target)\nas a set of Gaussian distributions " + "rather than raw points"); + ImGui::Text( + "Robust for rough initial poses but can converge to a local optimum\nif the initial misalignment is very large"); + ImGui::Text( + "Known for being faster and smoother in optimization because\nit replaces discrete point-point correspondences " + "with continuous probability density functions."); + ImGui::EndTooltip(); + } + + ImGui::SameLine(); + + if (is_icp_gui) + ImGui::PushStyleColor(ImGuiCol_Button, orangeBorder); + if (ImGui::Button("Iterative Closest Point")) + { + if (!is_icp_gui) + { + is_ndt_gui = false; + is_icp_gui = true; + is_rpf_gui = false; + justPushed = true; + } + } + if (is_icp_gui && !justPushed) + ImGui::PopStyleColor(); + if (ImGui::IsItemHovered()) + { + ImGui::BeginTooltip(); + ImGui::Text( + "Geometric registration algorithm that aligns two point clouds\nby minimizing the Euclidean distances between " + "corresponding points"); + ImGui::Text( + "Very precise at local refinement, especially point-to-plane ICP,\nbut it struggles if the starting alignment " + "is too far off"); + ImGui::EndTooltip(); + } + + ImGui::SameLine(); + + if (is_rpf_gui) + ImGui::PushStyleColor(ImGuiCol_Button, orangeBorder); + if (ImGui::Button("Registration Plane Feature")) + { + if (!is_rpf_gui) + { + is_ndt_gui = false; + is_icp_gui = false; + is_rpf_gui = true; + justPushed = true; + } + } + if (is_rpf_gui && !justPushed) + ImGui::PopStyleColor(); + if (ImGui::IsItemHovered()) + { + ImGui::BeginTooltip(); + ImGui::Text( + "Feature based registration technique that uses detected\nplanar surfaces in the environment (walls, floors, " + "ceilings, etc.) as constraints for alignment"); + ImGui::Text( + "Can be much more robust to noise and partial overlap,\nrequire far fewer correspondences (just a few planes " + "can define a full 6 DOF pose),\nhandle low texture regions better than ICP"); + ImGui::EndTooltip(); + } + + ImGui::EndMenuBar(); + } + + if (is_ndt_gui) + ndt_gui(); + if (is_icp_gui) + icp_gui(); + if (is_rpf_gui) + rpf_gui(); + } + + ImGui::End(); +} + +void pose_graph_slam_gui() +{ + ImGui::Begin("Pose Graph SLAM", &is_pose_graph_slam); + { + ImGui::PushItemWidth(ImGuiNumberWidth); + ImGui::InputFloat("Search radius", &tls_registration.pose_graph_slam.search_radius, 0.01f, 2.0f); + if (tls_registration.pose_graph_slam.search_radius < 0.01f) + tls_registration.pose_graph_slam.search_radius = 0.01f; + + ImGui::InputInt("Number of threads", &tls_registration.pose_graph_slam.number_of_threads); + if (tls_registration.pose_graph_slam.number_of_threads < 1) + tls_registration.pose_graph_slam.number_of_threads = 1; + + ImGui::InputInt( + "Number of iterations (pair wise matching)", &tls_registration.pose_graph_slam.number_of_iterations_pair_wise_matching); + if (tls_registration.pose_graph_slam.number_of_iterations_pair_wise_matching < 1) + tls_registration.pose_graph_slam.number_of_iterations_pair_wise_matching = 1; + + ImGui::InputFloat("Overlap threshold", &tls_registration.pose_graph_slam.overlap_threshold, 0.1f, 0.8f); + if (tls_registration.pose_graph_slam.overlap_threshold < 0.1f) + tls_registration.pose_graph_slam.overlap_threshold = 0.1f; + ImGui::PopItemWidth(); + + // ImGui::Checkbox("pgslam adaptive_robust_kernel", &pose_graph_slam.icp.is_adaptive_robust_kernel); + + //-- + ImGui::Checkbox("Adaptive robust kernel", &tls_registration.pose_graph_slam.is_adaptive_robust_kernel); + ImGui::SameLine(); + ImGui::Checkbox("Fix first node (add I to first pose in Hessian)", &tls_registration.pose_graph_slam.is_fix_first_node); + + ImGui::Text("Nonlinear optimization method:"); + ImGui::SameLine(); + ImGui::RadioButton("Gauss-Newton", &PGSnomSelection, 0); + ImGui::SameLine(); + ImGui::RadioButton("Levenberg-Marguardt", &PGSnomSelection, 1); + + tls_registration.pose_graph_slam.is_gauss_newton = (PGSnomSelection == 0); + tls_registration.pose_graph_slam.is_levenberg_marguardt = (PGSnomSelection == 1); + + ImGui::Text("Poses expressed as:"); + ImGui::SameLine(); + ImGui::RadioButton("camera<-world (cw)", &PGSpeSelection, 0); + ImGui::SameLine(); + ImGui::RadioButton("camera->world (wc)", &PGSpeSelection, 1); + + tls_registration.pose_graph_slam.is_cw = (PGSpeSelection == 0); + tls_registration.pose_graph_slam.is_wc = (PGSpeSelection == 1); + + ImGui::Text("Parameterizations of 3D rotation:"); + ImGui::RadioButton("Tait-Bryan angles (om fi ka: RxRyRz)", &PGS3dSelection, 0); + ImGui::SameLine(); + ImGui::RadioButton("Quaternion (q0 q1 q2 q3)", &PGS3dSelection, 1); + ImGui::SameLine(); + ImGui::RadioButton("Rodrigues (sx sy sz)", &PGS3dSelection, 2); + + tls_registration.pose_graph_slam.is_tait_bryan_angles = (PGS3dSelection == 0); + tls_registration.pose_graph_slam.is_quaternion = (PGS3dSelection == 1); + tls_registration.pose_graph_slam.is_rodrigues = (PGS3dSelection == 2); + + ImGui::Separator(); + + ImGui::Text("Method for pair wise matching (general):"); + ImGui::RadioButton("NDT", &PGSpwmtSelection, 0); + ImGui::RadioButton("Optimization_point_to_point_source_to_target", &PGSpwmtSelection, 1); + ImGui::RadioButton("Optimize_point_to_projection_onto_plane_source_to_target", &PGSpwmtSelection, 2); + ImGui::RadioButton("Optimize_point_to_plane_source_to_target", &PGSpwmtSelection, 3); + ImGui::RadioButton("Optimize_distance_point_to_plane_source_to_target", &PGSpwmtSelection, 4); + ImGui::RadioButton("Optimize_plane_to_plane_source_to_target", &PGSpwmtSelection, 5); + + ImGui::Separator(); + + ImGui::Text("Method for pair wise matching (with Lie-algebra):"); + ImGui::RadioButton("Optimize NDT (Lie-algebra left Jacobian)", &PGSpwmtSelection, 6); + ImGui::RadioButton("Optimize NDT (Lie-algebra right Jacobian)", &PGSpwmtSelection, 7); + ImGui::RadioButton("Optimize point to point source to target (Lie-algebra left Jacobian)", &PGSpwmtSelection, 8); + ImGui::RadioButton("Optimize point to point source to target (Lie-algebra right Jacobian)", &PGSpwmtSelection, 9); + ImGui::RadioButton("Optimize point to projection onto plane source to target (Lie-algebra left Jacobian)", &PGSpwmtSelection, 10); + ImGui::RadioButton("Optimize point to projection onto plane source to target (Lie-algebra right Jacobian)", &PGSpwmtSelection, 11); + +#ifdef WITH_PCL + ImGui::Separator(); + ImGui::Text("Method for pair wise matching (with PCL):"); + ImGui::RadioButton("Optimize with PCL (NDT based pair wise matching)", &PGSpwmtSelection, 12); + ImGui::RadioButton("Optimize with PCL (ICP based pair wise matching)", &PGSpwmtSelection, 13); +#endif + + // tls_registration.pose_graph_slam.set_all_to_false(); + tls_registration.pose_graph_slam.is_ndt = (PGSpwmtSelection == 0); + tls_registration.pose_graph_slam.is_optimization_point_to_point_source_to_target = (PGSpwmtSelection == 1); + tls_registration.pose_graph_slam.is_optimize_point_to_projection_onto_plane_source_to_target = (PGSpwmtSelection == 2); + tls_registration.pose_graph_slam.is_optimize_point_to_plane_source_to_target = (PGSpwmtSelection == 3); + tls_registration.pose_graph_slam.is_optimize_distance_point_to_plane_source_to_target = (PGSpwmtSelection == 4); + tls_registration.pose_graph_slam.is_optimize_plane_to_plane_source_to_target = (PGSpwmtSelection == 5); + + tls_registration.pose_graph_slam.is_ndt_lie_algebra_left_jacobian = (PGSpwmtSelection == 6); + tls_registration.pose_graph_slam.is_ndt_lie_algebra_right_jacobian = (PGSpwmtSelection == 7); + tls_registration.pose_graph_slam.is_optimize_point_to_point_source_to_target_lie_algebra_left_jacobian = (PGSpwmtSelection == 8); + tls_registration.pose_graph_slam.is_optimize_point_to_point_source_to_target_lie_algebra_right_jacobian = (PGSpwmtSelection == 9); + tls_registration.pose_graph_slam.is_optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_left_jacobian = + (PGSpwmtSelection == 10); + tls_registration.pose_graph_slam.is_optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_right_jacobian = + (PGSpwmtSelection == 11); + + tls_registration.pose_graph_slam.is_optimize_pcl_ndt = (PGSpwmtSelection == 12); + tls_registration.pose_graph_slam.is_optimize_pcl_icp = (PGSpwmtSelection == 13); + + if (PGSpwmtSelection >= 0 && PGSpwmtSelection <= 11) + tls_registration.pose_graph_slam.pair_wise_matching_type = PoseGraphSLAM::PairWiseMatchingType::general; + if (PGSpwmtSelection == 6) + tls_registration.pose_graph_slam.pair_wise_matching_type = PoseGraphSLAM::PairWiseMatchingType::pcl_ndt; + if (PGSpwmtSelection == 7) + tls_registration.pose_graph_slam.pair_wise_matching_type = PoseGraphSLAM::PairWiseMatchingType::pcl_icp; + + ImGui::Separator(); + if (ImGui::Button("Optimize")) + { + tls_registration.pose_graph_slam.ndt_bucket_size[0] = tls_registration.ndt.bucket_size[0]; + tls_registration.pose_graph_slam.ndt_bucket_size[1] = tls_registration.ndt.bucket_size[1]; + tls_registration.pose_graph_slam.ndt_bucket_size[2] = tls_registration.ndt.bucket_size[2]; + // double rms_initial = 0.0; + // double rms_final = 0.0; + // double mui = 0.0; + tls_registration.pose_graph_slam.optimize(session.point_clouds_container); + // pose_graph_slam.optimize(point_clouds_container, rms_initial, rms_final, mui); + // spdlog::info("mean uncertainty impact: " << mui << " rms_initial: " << rms_initial << " rms_final: " << rms_final << + // std::endl; + } + +#if WITH_GTSAM + if (ImGui::Button("Optimize with GTSAM")) + { + ImGui::Separator(); + ImGui::Text("With GTSAM:"); + tls_registration.pose_graph_slam.ndt_bucket_size[0] = tls_registration.ndt.bucket_size[0]; + tls_registration.pose_graph_slam.ndt_bucket_size[1] = tls_registration.ndt.bucket_size[1]; + tls_registration.pose_graph_slam.ndt_bucket_size[2] = tls_registration.ndt.bucket_size[2]; + double rms_initial = 0.0; + double rms_final = 0.0; + double mui = 0.0; + tls_registration.pose_graph_slam.optimize_with_GTSAM(session.point_clouds_container); + // spdlog::info("mean uncertainty impact: " << mui << " rms_initial: " << rms_initial << " rms_final: " << rms_final << + // std::endl; + } +#endif + +#if WITH_MANIF + ImGui::Separator(); + ImGui::Text("With MANIF:"); + if (ImGui::Button("Optimize with manif (a small header-only library for Lie theory)")) + { + tls_registration.pose_graph_slam.optimize_with_manif(session.point_clouds_container); + spdlog::info("Optimize with manif (a small header-only library for Lie theory) DONE" << std::endl; + } +#endif + } + + ImGui::End(); +} + +void observation_picking_gui() +{ + static std::string observations_file_name = ""; + + ImGui::Begin("Observations", &is_manual_analisys); + { + ImGui::Checkbox("Observation picking mode", &observation_picking.is_observation_picking_mode); + ImGui::BeginDisabled(!observation_picking.is_observation_picking_mode); + { + ImGui::Text("Grid [m]:"); + ImGui::Checkbox("10x10", &observation_picking.grid10x10m); + ImGui::SameLine(); + ImGui::Checkbox("1x1", &observation_picking.grid1x1m); + ImGui::SameLine(); + ImGui::Checkbox("0.1x0.1", &observation_picking.grid01x01m); + ImGui::SameLine(); + ImGui::Checkbox("0.01x0.01", &observation_picking.grid001x001m); + + ImGui::Text("Picking plane:"); + ImGui::PushItemWidth(ImGuiNumberWidth); + // ImGui::SliderFloat("picking_plane_height", &observation_picking.picking_plane_height, -20.0f, 20.0f); + ImGui::InputFloat("Height [m]", &observation_picking.picking_plane_height); + // ImGui::SliderFloat("picking_plane_threshold", &observation_picking.picking_plane_threshold, 0.01f, 200.0f); + ImGui::InputFloat("Threshold [m]", &observation_picking.picking_plane_threshold); + // ImGui::SliderFloat("picking_plane_max_xy", &observation_picking.max_xy, 10.0f, 1000.0f); + ImGui::InputFloat("Grid size [m]", &observation_picking.max_xy); + // ImGui::SliderInt("point_size", &observation_picking.point_size, 1, 10); + ImGui::InputInt("Point size", &observation_picking.point_size); + ImGui::PopItemWidth(); + if (observation_picking.point_size < 1) + observation_picking.point_size = 1; + if (observation_picking.point_size > 20) + observation_picking.point_size = 20; + + if (ImGui::Button("Accept current observation")) + { + std::vector m_poses; + for (size_t i = 0; i < session.point_clouds_container.point_clouds.size(); i++) + m_poses.push_back(session.point_clouds_container.point_clouds[i].m_pose); + observation_picking.accept_current_observation(m_poses); + } + ImGui::SameLine(); + if (ImGui::Button("Clear current observation")) + observation_picking.current_observation.clear(); + + if (ImGui::Button("Reset view")) + { + new_rotation_center = rotation_center; + new_rotate_x = 0.0; + new_rotate_y = 0.0; + new_translate_x = translate_x; + new_translate_y = translate_y; + new_translate_z = translate_z; + camera_transition_active = true; + } + } + ImGui::EndDisabled(); + + ImGui::Text((std::string("Number of observations: ") + std::to_string(observation_picking.observations.size())).c_str()); + + if (ImGui::Button("Load observations")) + { + std::string input_file_name = ""; + input_file_name = mandeye::fd::OpenFileDialogOneFile("Load observations", {}); + + if (input_file_name.size() > 0) + { + observations_file_name = input_file_name; + observation_picking.import_observations(input_file_name); + + for (const auto& obs : observation_picking.observations) + { + for (const auto& [key, value] : obs) + { + if (session.point_clouds_container.show_with_initial_pose) + { + auto p = session.point_clouds_container.point_clouds[key].m_initial_pose * value; + observation_picking.add_intersection(p); + } + else + { + auto p = session.point_clouds_container.point_clouds[key].m_pose * value; + observation_picking.add_intersection(p); + } + break; + } + } + } + } + ImGui::SameLine(); + if (ImGui::Button("Save observations")) + { + const auto output_file_name = mandeye::fd::SaveFileDialog("Save observations", {}, ".json"); + spdlog::info("JSON file to save: '{}'", output_file_name); + + if (output_file_name.size() > 0) + observation_picking.export_observation(output_file_name); + } + + ImGui::Text((std::string("Loaded observations from file: '") + observations_file_name + std::string("'")).c_str()); + + if (ImGui::Button("Compute RMS (xy)")) + { + double rms = compute_rms(true, session, observation_picking); + spdlog::info("RMS (initial poses): {}", rms); + rms = compute_rms(false, session, observation_picking); + spdlog::info("RMS (current poses): {}", rms); + } + + ImGui::Separator(); + if (ImGui::Button("Add intersection")) + observation_picking.add_intersection(Eigen::Vector3d(0.0, 0.0, 0.0)); + + int index_intersection_to_remove = -1; + for (size_t i = 0; i < observation_picking.intersections.size(); i++) + { + ImGui::Separator(); + ImGui::SetWindowFontScale(1.25f); + ImGui::Text("Intersection %zu", i); + ImGui::SetWindowFontScale(1.0f); + ImGui::SameLine(); + ImGui::ColorEdit3( + std::string("Color##" + std::to_string(i)).c_str(), + observation_picking.intersections[i].color, + ImGuiColorEditFlags_NoInputs); + ImGui::SameLine(); + if (ImGui::Button(std::string("Remove##" + std::to_string(i)).c_str())) + index_intersection_to_remove = i; + + ImGui::InputFloat3( + std::string("Translation [m]##" + std::to_string(i)).c_str(), observation_picking.intersections[i].translation); + ImGui::InputFloat3(std::string("Rotation [deg]##" + std::to_string(i)).c_str(), observation_picking.intersections[i].rotation); + ImGui::InputFloat3( + std::string("Width length height [m]##" + std::to_string(i)).c_str(), + observation_picking.intersections[i].width_length_height); + } + + if (index_intersection_to_remove != -1) + { + std::vector intersections; + for (size_t i = 0; i < observation_picking.intersections.size(); i++) + { + if (i != index_intersection_to_remove) + intersections.push_back(observation_picking.intersections[i]); + } + observation_picking.intersections = intersections; + } + + ImGui::Separator(); + + ImGui::BeginDisabled(observation_picking.intersections.size() <= 0); + { + if (ImGui::Button("Export point clouds inside intersections, RMS and poses")) + { + std::string output_folder_name = ""; + output_folder_name = mandeye::fd::SelectFolder("Choose folder"); + spdlog::info("folder: '{}'", output_folder_name); + + if (output_folder_name.size() > 0) + export_result_to_folder(output_folder_name, observation_picking, session); + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("in RESSO format to folder"); + ImGui::SetNextItemWidth(ImGuiNumberWidth); + ImGui::InputFloat("Label distance [m]", &observation_picking.label_dist); + } + ImGui::EndDisabled(); + } + + ImGui::End(); +} + +void loop_closure_gui() +{ + ImGui::Begin("Manual Pose Graph Loop Closure", &is_loop_closure_gui); + { + const auto point_cloud_upper = session.point_clouds_container.point_clouds.size() - 1; + + ImGui::Checkbox("Render source as red target as blue", &session.pose_graph_loop_closure.render_source_as_red_target_as_blue); + + ImGui::Text("Num edge extended:"); + + ImGui::Text("before: "); + ImGui::SameLine(); + ImGui::PushItemWidth(ImGuiNumberWidth); + ImGui::SliderInt("##fs", &num_edge_extended_before, 0, point_cloud_upper); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("min 0; max %zu", point_cloud_upper); + ImGui::SameLine(); + ImGui::InputInt("##fi", &num_edge_extended_before, 1, 5); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("min 0; max %zu", point_cloud_upper); + 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; + + ImGui::Text(" after: "); + ImGui::SameLine(); + + ImGui::SliderInt("##ts", &num_edge_extended_after, index_loop_closure_target, static_cast(point_cloud_upper)); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("min 0; max %zu", point_cloud_upper); + ImGui::SameLine(); + ImGui::InputInt("##ti", &num_edge_extended_after, 1, 5); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("min 0; max %zu", point_cloud_upper); + 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; + ImGui::PopItemWidth(); + + int prev_index_active_edge = session.pose_graph_loop_closure.index_active_edge; + session.pose_graph_loop_closure.Gui( + session.point_clouds_container, + index_loop_closure_source, + index_loop_closure_target, + m_gizmo, + tls_registration.gnss, + session.ground_control_points, + session.control_points, + num_edge_extended_before, + num_edge_extended_after); + + new_loop_closure_index = (prev_index_active_edge != session.pose_graph_loop_closure.index_active_edge); + } + + ImGui::End(); +} + +void lio_segments_gui() +{ + ImGui::Begin("LIO segments editor", &is_lio_segments_gui); + { + ImGui::Text("index from: "); + ImGui::SameLine(); + ImGui::PushItemWidth(ImGuiNumberWidth); + ImGui::SliderInt("##fs", &index_begin, 0, index_end); + ImGui::SameLine(); + ImGui::InputInt("##fi", &index_begin, 1, 5); + if (index_begin < 0) + index_begin = 0; + if (index_begin >= index_end) + index_begin = index_end; + + ImGui::SameLine(); + ImGui::Text(" to: "); + ImGui::SameLine(); + + ImGui::SliderInt("##ts", &index_end, index_begin, static_cast(session.point_clouds_container.point_clouds.size() - 1)); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("max %zu", session.point_clouds_container.point_clouds.size() - 1); + ImGui::SameLine(); + ImGui::InputInt("##ti", &index_end, 1, 5); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("max %zu", session.point_clouds_container.point_clouds.size() - 1); + if (index_end < index_begin) + index_end = index_begin; + if (index_end >= session.point_clouds_container.point_clouds.size() - 1) + index_end = session.point_clouds_container.point_clouds.size() - 1; + ImGui::PopItemWidth(); + + ImGui::Text("Selection: "); + ImGui::SameLine(); + if (ImGui::Button("show ")) + session.point_clouds_container.show_all_from_range(index_begin, index_end); + ImGui::SameLine(); + if (ImGui::Button("shift -")) + { + int step = index_end - index_begin; + index_begin -= step; + index_end -= step; + + if (index_begin < 0) + index_begin = 0; + if (index_end < 0) + index_end = 0; + + rotation_center.x() = session.point_clouds_container.point_clouds[index_begin].m_pose(0, 3); + rotation_center.y() = session.point_clouds_container.point_clouds[index_begin].m_pose(1, 3); + rotation_center.z() = session.point_clouds_container.point_clouds[index_begin].m_pose(2, 3); + session.point_clouds_container.show_all_from_range(index_begin, index_end); + } + ImGui::SameLine(); + if (ImGui::Button("shift +")) + { + int step = index_end - index_begin; + index_begin += step; + index_end += step; + + if (index_begin > session.point_clouds_container.point_clouds.size() - 1) + index_begin = session.point_clouds_container.point_clouds.size() - 1; + if (index_end > session.point_clouds_container.point_clouds.size() - 1) + index_end = session.point_clouds_container.point_clouds.size() - 1; + + rotation_center.x() = session.point_clouds_container.point_clouds[index_begin].m_pose(0, 3); + rotation_center.y() = session.point_clouds_container.point_clouds[index_begin].m_pose(1, 3); + rotation_center.z() = session.point_clouds_container.point_clouds[index_begin].m_pose(2, 3); + session.point_clouds_container.show_all_from_range(index_begin, index_end); + } + ImGui::SameLine(); + if (ImGui::Button("Show all")) + session.point_clouds_container.show_all(); + ImGui::SameLine(); + if (ImGui::Button("Hide all")) + session.point_clouds_container.hide_all(); + ImGui::SameLine(); + if (ImGui::Button("Reset poses")) + reset_poses(session); + + ImGui::Checkbox("Show with initial pose", &session.point_clouds_container.show_with_initial_pose); + ImGui::SameLine(); + ImGui::Checkbox("Manipulate only marked gizmo", &manipulate_only_marked_gizmo); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("false: move also succesive nodes"); + + ImGui::SameLine(); + + if (ImGui::Button("Set fuse IMU inclination for picked trajectory node")) + { + if (index_loop_closure_target >= 0 && index_loop_closure_target < session.point_clouds_container.point_clouds.size()) + { + session.point_clouds_container.point_clouds[index_loop_closure_target].fuse_inclination_from_IMU = true; + } + + // picked_index + /*int tmp = -1; + getClosestTrajectoryPoint(session, x, y, false, tmp); + + if (io.KeyCtrl) + { + if (tmp != -1) + index_loop_closure_target = tmp; + } + else if (io.KeyShift) + { + if (tmp != -1) + index_loop_closure_source = tmp; + } + + januszjanusz*/ + } + + ImGui::Text("Fuse IMU inclination: "); + ImGui::SameLine(); + + static double angle_diff = 5.0; + + if (ImGui::Button("Set those that satisfy acceptable angle")) + { + for (size_t i = 0; i < session.point_clouds_container.point_clouds.size(); i++) + { + double om = session.point_clouds_container.point_clouds[i].local_trajectory[0].imu_om_fi_ka.x() * RAD_TO_DEG; + double fi = session.point_clouds_container.point_clouds[i].local_trajectory[0].imu_om_fi_ka.y() * RAD_TO_DEG; + + spdlog::info("om: {}, fi {}", om, fi); + if (fabs(om) > angle_diff || fabs(fi) > angle_diff) + { + } + else + session.point_clouds_container.point_clouds[i].fuse_inclination_from_IMU = true; + } + } + ImGui::SameLine(); + if (ImGui::Button("unset all")) + { + for (size_t i = 0; i < session.point_clouds_container.point_clouds.size(); i++) + session.point_clouds_container.point_clouds[i].fuse_inclination_from_IMU = false; + } + + ImGui::SameLine(); + ImGui::SetNextItemWidth(ImGuiNumberWidth); + ImGui::InputDouble("acceptable angle [deg]", &angle_diff); + + ImGui::Separator(); + // ImGui::Text("motion model"); + + // session.pose_graph_loop_closure.edges. + + // ImGui::InputDouble("motion_model_w_px_1_sigma_m", &session.pose_graph_loop_closure.motion_model_w_px_1_sigma_m); + // ImGui::InputDouble("motion_model_w_py_1_sigma_m", &session.pose_graph_loop_closure.motion_model_w_py_1_sigma_m); + // ImGui::InputDouble("motion_model_w_pz_1_sigma_m", &session.pose_graph_loop_closure.motion_model_w_pz_1_sigma_m); + // ImGui::InputDouble("motion_model_w_om_1_sigma_deg", &session.pose_graph_loop_closure.motion_model_w_om_1_sigma_deg); + // ImGui::InputDouble("motion_model_w_fi_1_sigma_deg", &session.pose_graph_loop_closure.motion_model_w_fi_1_sigma_deg); + // ImGui::InputDouble("motion_model_w_ka_1_sigma_deg", &session.pose_graph_loop_closure.motion_model_w_ka_1_sigma_deg); + + // ImGui::Separator(); + + ImGui::BeginChild("LIO segments", ImVec2(0, 0), true); + { + for (size_t i = 0; i < session.point_clouds_container.point_clouds.size(); i++) + { + if (i > 0) + ImGui::Separator(); + ImGui::SetWindowFontScale(1.25f); + ImGui::Checkbox( + std::filesystem::path(session.point_clouds_container.point_clouds[i].file_name).filename().string().c_str(), + &session.point_clouds_container.point_clouds[i].visible); + ImGui::SetWindowFontScale(1.0f); + ImGui::SameLine(); + ImGui::Checkbox(("gizmo##" + std::to_string(i)).c_str(), &session.point_clouds_container.point_clouds[i].gizmo); + +#if 0 + ImGui::SameLine(); + ImGui::Checkbox(("fixed##" + std::to_string(i)).c_str(), &session.point_clouds_container.point_clouds[i].fixed); + ImGui::SameLine(); + ImGui::PushButtonRepeat(true); + float spacing = ImGui::GetStyle().ItemInnerSpacing.x; + if (ImGui::ArrowButton(("left##" + std::to_string(i)).c_str(), ImGuiDir_Left)) + { + (session.point_clouds_container.point_clouds[i].point_size)--; + } + ImGui::SameLine(0.0f, spacing); + if (ImGui::ArrowButton(("right##" + std::to_string(i)).c_str(), ImGuiDir_Right)) + { + (session.point_clouds_container.point_clouds[i].point_size)++; + } + ImGui::PopButtonRepeat(); + ImGui::SameLine(); + ImGui::Text("point size %d", session.point_clouds_container.point_clouds[i].point_size); + if (session.point_clouds_container.point_clouds[i].point_size < 1) + { + session.point_clouds_container.point_clouds[i].point_size = 1; + } + + ImGui::SameLine(); + if (ImGui::Button(std::string("#" + std::to_string(i) + " save scan(global reference frame)").c_str())) + { + const auto output_file_name = mandeye::fd::SaveFileDialog("Choose folder", {}); + spdlog::info("Scan file to save: '" << output_file_name << "'" << std::endl; + if (output_file_name.size() > 0) + { + session.point_clouds_container.point_clouds[i].save_as_global(output_file_name); + } + } + ImGui::SameLine(); + if (ImGui::Button(std::string("#" + std::to_string(i) + " shift points to center").c_str())) + { + session.point_clouds_container.point_clouds[i].shift_to_center(); + } +#endif + if (session.point_clouds_container.point_clouds[i].gizmo) + { + for (size_t j = 0; j < session.point_clouds_container.point_clouds.size(); j++) + { + if (i != j) + { + session.point_clouds_container.point_clouds[j].gizmo = false; + } + } + m_gizmo[0] = (float)session.point_clouds_container.point_clouds[i].m_pose(0, 0); + m_gizmo[1] = (float)session.point_clouds_container.point_clouds[i].m_pose(1, 0); + m_gizmo[2] = (float)session.point_clouds_container.point_clouds[i].m_pose(2, 0); + m_gizmo[3] = (float)session.point_clouds_container.point_clouds[i].m_pose(3, 0); + m_gizmo[4] = (float)session.point_clouds_container.point_clouds[i].m_pose(0, 1); + m_gizmo[5] = (float)session.point_clouds_container.point_clouds[i].m_pose(1, 1); + m_gizmo[6] = (float)session.point_clouds_container.point_clouds[i].m_pose(2, 1); + m_gizmo[7] = (float)session.point_clouds_container.point_clouds[i].m_pose(3, 1); + m_gizmo[8] = (float)session.point_clouds_container.point_clouds[i].m_pose(0, 2); + m_gizmo[9] = (float)session.point_clouds_container.point_clouds[i].m_pose(1, 2); + m_gizmo[10] = (float)session.point_clouds_container.point_clouds[i].m_pose(2, 2); + m_gizmo[11] = (float)session.point_clouds_container.point_clouds[i].m_pose(3, 2); + m_gizmo[12] = (float)session.point_clouds_container.point_clouds[i].m_pose(0, 3); + m_gizmo[13] = (float)session.point_clouds_container.point_clouds[i].m_pose(1, 3); + m_gizmo[14] = (float)session.point_clouds_container.point_clouds[i].m_pose(2, 3); + m_gizmo[15] = (float)session.point_clouds_container.point_clouds[i].m_pose(3, 3); + } + + if (session.point_clouds_container.point_clouds[i].visible) + { + ImGui::SameLine(); + ImGui::ColorEdit3( + ("color##" + std::to_string(i)).c_str(), + session.point_clouds_container.point_clouds[i].render_color, + ImGuiColorEditFlags_NoInputs); + +#if 0 + ImGui::SameLine(); + if (ImGui::Button(std::string(("ICP##" + std::to_string(i)).c_str()) + { + size_t index_target = i; + PointClouds pcs; + for (size_t k = 0; k < index_target; k++) + { + if (session.point_clouds_container.point_clouds[k].visible) + { + pcs.point_clouds.push_back(session.point_clouds_container.point_clouds[k]); + } + } + + if (pcs.point_clouds.size() > 0) + { + for (size_t k = 0; k < pcs.point_clouds.size(); k++) + { + pcs.point_clouds[k].fixed = true; + } + } + pcs.point_clouds.push_back(session.point_clouds_container.point_clouds[index_target]); + pcs.point_clouds[pcs.point_clouds.size() - 1].fixed = false; + + ICP icp; + icp.search_radious = 0.3; // ToDo move to params + for (auto& pc : pcs.point_clouds) + { + pc.rgd_params.resolution_X = icp.search_radious; + pc.rgd_params.resolution_Y = icp.search_radious; + pc.rgd_params.resolution_Z = icp.search_radious; + + pc.build_rgd(); + pc.cout_rgd(); + pc.compute_normal_vectors(0.5); + } + + icp.number_of_threads = std::thread::hardware_concurrency(); + + icp.number_of_iterations = 10; + icp.is_adaptive_robust_kernel = false; + + icp.is_ballanced_horizontal_vs_vertical = false; + icp.is_fix_first_node = false; + icp.is_gauss_newton = true; + icp.is_levenberg_marguardt = false; + icp.is_cw = false; + icp.is_wc = true; + icp.is_tait_bryan_angles = true; + icp.is_quaternion = false; + icp.is_rodrigues = false; + spdlog::info("optimization_point_to_point_source_to_target" << std::endl; + + icp.optimization_point_to_point_source_to_target(pcs); + + spdlog::info("pose before: " << session.point_clouds_container.point_clouds[index_target].m_pose.matrix() << std::endl; + + 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); + } + + session.point_clouds_container.point_clouds[index_target].m_pose = pcs.point_clouds[pcs.point_clouds.size() - 1].m_pose; + + spdlog::info("pose after ICP: " << session.point_clouds_container.point_clouds[index_target].m_pose.matrix() << std::endl; + + // like gizmo + if (!manipulate_only_marked_gizmo) + { + spdlog::info("Update all poses after current pose" << std::endl; + + Eigen::Affine3d curr_m_pose = session.point_clouds_container.point_clouds[index_target].m_pose; + for (size_t j = index_target + 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; + } + } + } +#endif + + ImGui::SameLine(); + ImGui::Checkbox( + ("fuse IMU inclination##" + std::to_string(i)).c_str(), + &session.point_clouds_container.point_clouds[i].fuse_inclination_from_IMU); + + ImGui::SameLine(); + ImGui::Checkbox(("show IMU##" + std::to_string(i)).c_str(), &session.point_clouds_container.point_clouds[i].show_IMU); + + ImGui::SameLine(); + ImGui::Checkbox(("show pose##" + std::to_string(i)).c_str(), &session.point_clouds_container.point_clouds[i].show_pose); + + /*ImGui::SameLine(); + if (ImGui::Button(("set IMU inclination##" + std::to_string(i)).c_str())) + { + //session.point_clouds_container.point_clouds[i].m_initial_pose + //session.point_clouds_container.point_clouds[i].m_pose + //session.point_clouds_container.point_clouds[i].m_pose_temp + + TaitBryanPose target_pose = + pose_tait_bryan_from_affine_matrix(session.point_clouds_container.point_clouds[i].m_pose); + + target_pose.om = session.point_clouds_container.point_clouds[i].local_trajectory[0].imu_om_fi_ka.x(); + target_pose.fi = session.point_clouds_container.point_clouds[i].local_trajectory[0].imu_om_fi_ka.y(); + + Eigen::Affine3d m_pose = affine_matrix_from_pose_tait_bryan(target_pose); + + session.point_clouds_container.point_clouds[i].m_initial_pose = m_pose; + session.point_clouds_container.point_clouds[i].m_pose = m_pose; + session.point_clouds_container.point_clouds[i].m_pose_temp = m_pose; + + //session.point_clouds_container.point_clouds[i].m_pose = m_pose; + //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] = + session.point_clouds_container.point_clouds[i].pose.px; + //session.point_clouds_container.point_clouds[i].gui_translation[1] = + session.point_clouds_container.point_clouds[i].pose.py; + //session.point_clouds_container.point_clouds[i].gui_translation[2] = + session.point_clouds_container.point_clouds[i].pose.pz; + //session.point_clouds_container.point_clouds[i].gui_rotation[0] = + rad2deg(session.point_clouds_container.point_clouds[i].pose.om); + //session.point_clouds_container.point_clouds[i].gui_rotation[1] = + rad2deg(session.point_clouds_container.point_clouds[i].pose.fi); + //session.point_clouds_container.point_clouds[i].gui_rotation[2] = + rad2deg(session.point_clouds_container.point_clouds[i].pose.ka); + }*/ + + ImGui::Text("fixed: "); + + ImGui::SameLine(); + ImGui::Checkbox(("X##" + std::to_string(i)).c_str(), &session.point_clouds_container.point_clouds[i].fixed_x); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(xText); + + ImGui::SameLine(); + ImGui::Checkbox(("Y##" + std::to_string(i)).c_str(), &session.point_clouds_container.point_clouds[i].fixed_y); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(yText); + + ImGui::SameLine(); + ImGui::Checkbox(("Z##" + std::to_string(i)).c_str(), &session.point_clouds_container.point_clouds[i].fixed_z); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(zText); + + ImGui::SameLine(); + ImGui::Checkbox(("om##" + std::to_string(i)).c_str(), &session.point_clouds_container.point_clouds[i].fixed_om); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(omText); + + ImGui::SameLine(); + ImGui::Checkbox(("fi##" + std::to_string(i)).c_str(), &session.point_clouds_container.point_clouds[i].fixed_fi); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(fiText); + + ImGui::SameLine(); + ImGui::Checkbox(("ka##" + std::to_string(i)).c_str(), &session.point_clouds_container.point_clouds[i].fixed_ka); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(kaText); + } +#if 0 + ImGui::SameLine(); + if (ImGui::Button(std::string("#" + std::to_string(i) + " print frame to console").c_str())) + { + spdlog::info(session.point_clouds_container.point_clouds[i].m_pose.matrix() << std::endl; + } +#endif + } + } + ImGui::EndChild(); + } + + ImGui::End(); +} + +void loadSession(const std::string& session_file_name) +{ + spdlog::info("Session file: '{}'", session_file_name); + + if (session.load( + fs::path(session_file_name).string(), + tls_registration.is_decimate, + tls_registration.bucket_x, + tls_registration.bucket_y, + tls_registration.bucket_z, + tls_registration.calculate_offset)) + { + session_loaded = true; + index_begin = 0; + index_end = session.point_clouds_container.point_clouds.size() - 1; + + std::string newTitle = winTitle + " - " + truncPath(session_file_name); + glutSetWindowTitle(newTitle.c_str()); + + for (const auto& pc : session.point_clouds_container.point_clouds) + session_total_number_of_points += pc.points_local.size(); + + session_dims = session.point_clouds_container.compute_point_cloud_dimension(); + } +} + +void openSession() +{ + session_file_name = mandeye::fd::OpenFileDialogOneFile("Open session", mandeye::fd::Session_filter); + + if (session_file_name.size() > 0) + { + loadSession(session_file_name); + } +} + +std::string saveSession() +{ + const std::string output_file_name = + mandeye::fd::SaveFileDialog("Save session as", mandeye::fd::Session_filter, ".mjs", session_file_name); + + if (output_file_name.size() > 0) + { + spdlog::info("Session file to save: '{}'", output_file_name); + + // creating filenames proposal based on current selection + std::filesystem::path path(output_file_name); + // Extract parts + const auto dir = path.parent_path(); + const auto stem = path.stem().string(); + + // Build new names + std::string initial_poses_file_name = (dir / (stem + "_ini_poses.mri")).string(); + std::string poses_file_name = (dir / (stem + "_poses.mrp")).string(); + + if (session.point_clouds_container.initial_poses_file_name.empty()) + { + spdlog::info("Please assign initial_poses_file_name to session"); + spdlog::warn("Session is not saved!"); + + [[maybe_unused]] pfd::message message( + "Please assign initial_poses_file_name to session", + "Session is not saved. Please assign initial_poses_file_name to session. " + "Follow guidlines available here : " + "https://github.com/MapsHD/HDMapping/tree/main/doc/, " + "You can do this using button 'update initial poses from RESSO file'", + pfd::choice::ok, + pfd::icon::error); + message.result(); + + initial_poses_file_name = + mandeye::fd::SaveFileDialog("Initial poses file name", mandeye::fd::IniPoses_filter, initial_poses_file_name); + spdlog::info("Resso file to save: '{}'", initial_poses_file_name); + + if (initial_poses_file_name.size() > 0) + { + spdlog::info("Saving initial poses to: '{}'", initial_poses_file_name); + session.point_clouds_container.save_poses(initial_poses_file_name, false); + } + } + + if (session.point_clouds_container.poses_file_name.empty()) + { + spdlog::info("Please assign poses_file_name to session"); + spdlog::warn("Session is not saved!"); + + [[maybe_unused]] pfd::message message( + "Please assign poses_file_name to session", + "Session is not saved. Please assign poses_file_name to session. " + "Follow guidlines available here : " + "https://github.com/MapsHD/HDMapping/tree/main/doc/," + "You can do this using button 'update poses from RESSO file'", + pfd::choice::ok, + pfd::icon::error); + message.result(); + + poses_file_name = mandeye::fd::SaveFileDialog("Poses file name", mandeye::fd::Poses_filter, poses_file_name); + spdlog::info("Resso file to save: '{}'", poses_file_name); + if (poses_file_name.size() > 0) + { + spdlog::info("Saving poses to: '{}'", poses_file_name); + session.point_clouds_container.save_poses(poses_file_name, false); + } + } + + session.save(output_file_name, poses_file_name, initial_poses_file_name, false); + spdlog::info("Saving result to: '{}'", poses_file_name); + session.point_clouds_container.save_poses(poses_file_name, false); + + try + { + fs::copy_file(poses_file_name, initial_poses_file_name, fs::copy_options::overwrite_existing); + } catch (const fs::filesystem_error& e) + { + spdlog::error("Error copying poses file: {}", e.what()); + } + + return output_file_name; + } + else + { + spdlog::info("Saving canceled"); + + return ""; + } +} + +void openLaz(bool fillInSession) +{ + session.point_clouds_container.point_clouds.clear(); + std::vector input_file_names; + input_file_names = mandeye::fd::OpenFileDialog("Load las/laz files", mandeye::fd::LAS_LAZ_filter, true); + if (input_file_names.size() > 0) + { + session.working_directory = fs::path(input_file_names[0]).parent_path().string(); + + spdlog::info("Creating session from las/laz files:"); + for (size_t i = 0; i < input_file_names.size(); i++) + spdlog::info("{}", input_file_names[i]); + + if (!session.point_clouds_container.load_whu_tls( + input_file_names, + tls_registration.is_decimate, + tls_registration.bucket_x, + tls_registration.bucket_y, + tls_registration.bucket_z, + tls_registration.calculate_offset, + session.load_cache_mode)) + spdlog::error("Error loading session! Check input files laz/las"); + else + spdlog::info("Loaded: {} point_clouds", session.point_clouds_container.point_clouds.size()); + + session_loaded = true; + index_begin = 0; + index_end = session.point_clouds_container.point_clouds.size() - 1; + + std::string newTitle = winTitle + " - " + fs::path(input_file_names[0]).parent_path().string(); + glutSetWindowTitle(newTitle.c_str()); + + for (const auto& pc : session.point_clouds_container.point_clouds) + session_total_number_of_points += pc.points_local.size(); + + session_dims = session.point_clouds_container.compute_point_cloud_dimension(); + + if (fillInSession) + { + int counter = 1; + Eigen::Vector3d mean(session.point_clouds_container.point_clouds[0].points_local[0]); + for (auto& pc : session.point_clouds_container.point_clouds) + { + if (pc.points_local.size() > 100) + { + // spdlog::info("mean " << mean << std::endl; + for (size_t i = 100; i < pc.points_local.size(); i += 100) + { + mean += pc.points_local[i]; + counter++; + } + } + } + mean /= counter; + + for (auto& pc : session.point_clouds_container.point_clouds) + { + Eigen::Affine3d m = Eigen::Affine3d::Identity(); + if (pc.points_local.size() > 100) + { + // int counter = 1; + + // spdlog::info("mean " << mean << std::endl; + // for (size_t i = 100; i < pc.points_local.size(); i += 100) + //{ + // mean += pc.points_local[i]; + // counter++; + //} + + // mean /= counter; + m.translation() = mean; + + PointCloud::LocalTrajectoryNode node; + node.imu_diff_angle_om_fi_ka_deg = { 0, 0, 0 }; + node.imu_om_fi_ka = { 0, 0, 0 }; + node.m_pose = Eigen::Affine3d::Identity(); + node.timestamps = { 0, 0 }; + + pc.local_trajectory.push_back(node); + + for (auto& p : pc.points_local) + { + p -= mean; + } + } + + pc.m_initial_pose = m; + pc.m_pose = m; + pc.m_pose_temp = m; + + pc.pose = pose_tait_bryan_from_affine_matrix(m); + } + + std::string session_fn = get_next_result_path(session.working_directory).string(); + std::filesystem::create_directory(session_fn); + + session_file_name = (fs::path(session_fn) / "newSession.mjs").string(); + session.point_clouds_container.initial_poses_file_name = + "dummy"; // non empty file names signal poses present, new file names will be created on save + session.point_clouds_container.poses_file_name = + "dummy"; // non empty file names signal poses present, new file names will be created on save + + std::string output_file_name = saveSession(); + + if (output_file_name.size() > 0) + { + // creating filenames proposal based on current selection + std::filesystem::path path(output_file_name); + // Extract parts + const auto dir = path.parent_path(); + const auto stem = path.stem().string(); + + // save to folder + for (size_t i = 0; i < session.point_clouds_container.point_clouds.size(); i++) + { + auto pc = session.point_clouds_container.point_clouds[i]; + auto fpath = dir / fs::path(pc.file_name).filename(); + + session.point_clouds_container.point_clouds[i].file_name = fpath.string(); + exportLaz(fpath.string(), pc.points_local, pc.intensities, pc.timestamps); + + // saving terajectory file + auto pathtrj = dir / ("trajectory_lio_" + std::to_string(i) + ".csv"); + spdlog::info("Saving trajectory: '{}'", pathtrj); + + std::ofstream outfile; + outfile.open(pathtrj); + if (!outfile.good()) + { + spdlog::error("Error saving file: '{}'", pathtrj); + return; + } + + outfile << "timestamp_nanoseconds pose00 pose01 pose02 pose03 pose10 pose11 pose12 pose13 pose20 pose21 pose22 pose23 " + "timestampUnix_nanoseconds om_rad fi_rad ka_rad" + << std::endl; + for (int j = 0; j < 1; j++) + { + auto pose = Eigen::Affine3d::Identity(); + + outfile << std::setprecision(20) << 0.0 << " " << std::setprecision(10) << pose(0, 0) << " " << pose(0, 1) << " " + << pose(0, 2) << " " << pose(0, 3) << " " << pose(1, 0) << " " << pose(1, 1) << " " << pose(1, 2) << " " + << pose(1, 3) << " " << pose(2, 0) << " " << pose(2, 1) << " " << pose(2, 2) << " " << pose(2, 3) << " " + << std::setprecision(20) << 0.0 << " " << 0.0 << " " << 0.0 << " " << 0.0 << " " << std::endl; + } + outfile.close(); + } + } + + [[maybe_unused]] pfd::message message( + "Information", + "If you can not see point cloud --> 1. Change 'Points render subsampling', 2. Check console 'min max coordinates " + "should be " + "small numbers to see points in our local coordinate system'. 3. Set checkbox 'calculate_offset for WHU-TLS'. 4. Later " + "on " + "You can change offset directly in session json file.", + pfd::choice::ok, + pfd::icon::info); + message.result(); + + // std::string mes = "Session saved to folder '" + path_ground_truth_session_folder.string() + "'"; + //[[maybe_unused]] pfd::message message( + // "Ground truth session info", mes, + // pfd::choice::ok, pfd::icon::info); + // message.result(); + } + } +} + +void saveSubsession() +{ + int inx_begin = 0; + int inx_end = 0; + + for (size_t i = 0; i < session.point_clouds_container.point_clouds.size(); i++) + { + if (session.point_clouds_container.point_clouds[i].visible) + { + inx_begin = i; + break; + } + } + + for (size_t i = 0; i < session.point_clouds_container.point_clouds.size(); i++) + if (session.point_clouds_container.point_clouds[i].visible) + inx_end = i; + + // creating filename proposal based on current selection + fs::path path(session_file_name); + // Extract parts + fs::path dir = path.parent_path(); + std::string stem = path.stem().string(); + const auto ext = ".mjs"; // forcing new extension even if original session was .json + const std::string indexpart = " " + std::to_string(inx_begin) + "-" + std::to_string(inx_end); + + // Build new name + std::string indexed_file_name = (dir / (stem + indexpart + ext)).string(); + + const auto output_file_name = mandeye::fd::SaveFileDialog("Save subsession", mandeye::fd::Session_filter, ".mjs", indexed_file_name); + + if (output_file_name.size() > 0) + { + spdlog::info("Subsession file to save: '{}'", output_file_name); + + path = fs::path(output_file_name); + dir = path.parent_path(); + stem = path.stem().string(); + + const auto initial_poses_file_name = (dir / (stem + "_ini_poses" + ".mri")).string(); + const auto poses_file_name = (dir / (stem + "_poses" + ".mrp")).string(); + + session.save(fs::path(output_file_name).string(), poses_file_name, initial_poses_file_name, true); + spdlog::info("Saving poses to: '{}'", poses_file_name); + session.point_clouds_container.save_poses(fs::path(poses_file_name).string(), true); + + try + { + fs::copy_file(poses_file_name, initial_poses_file_name, fs::copy_options::overwrite_existing); + } catch (const fs::filesystem_error& e) + { + spdlog::error("Error copying poses file: {}", e.what()); + } + } + else + spdlog::info("Saving canceled"); +} + +void settings_gui() +{ + ImGui::Begin("Settings", &is_settings_gui); + { + std::string wd = "Working directory: '" + session.working_directory + "'"; + ImGui::Text(wd.c_str()); + + ImGui::NewLine(); + + ImGui::InputFloat("camera_x", &new_rotation_center.x()); + ImGui::InputFloat("camera_y", &new_rotation_center.y()); + ImGui::InputFloat("camera_z", &new_rotation_center.z()); + + if (ImGui::Button("set camera")) + { + // new_rotate_x = rotate_x; + // new_rotate_y = rotate_y; + // new_translate_x = -new_rotation_center.x(); + // new_translate_y = -new_rotation_center.y(); + // new_translate_z = -new_rotation_center.z(); + camera_transition_active = true; + } + + if (ImGui::Button("Set initial pose to Identity and update other poses")) + initial_pose_to_identity(session); + + ImGui::NewLine(); + + ImGui::Checkbox("Downsample during load", &tls_registration.is_decimate); + ImGui::Checkbox("Loading Point Cloud Cache Mode", &session.load_cache_mode); + + ImGui::Text("Bucket [m]:"); + ImGui::PushItemWidth(ImGuiNumberWidth); + ImGui::InputDouble("X##b", &tls_registration.bucket_x, 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(xText); + ImGui::SameLine(); + ImGui::InputDouble("Y##b", &tls_registration.bucket_y, 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(yText); + ImGui::SameLine(); + ImGui::InputDouble("Z##b", &tls_registration.bucket_z, 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(zText); + ImGui::PopItemWidth(); + + ImGui::Separator(); + // common_data + // manual_pose_graph_loop_closure_mode + + if (ImGui::Button("Load RESSO file (transformation_GroundTruth.reg)")) + { + std::string input_file_name = ""; + input_file_name = mandeye::fd::OpenFileDialogOneFile("Load RESSO", mandeye::fd::Resso_filter); + spdlog::info("RESSO file: '{}'", input_file_name); + + if (input_file_name.size() > 0) + { + session.working_directory = fs::path(input_file_name).parent_path().string(); + + if (!session.point_clouds_container.load( + session.working_directory.c_str(), + input_file_name.c_str(), + tls_registration.is_decimate, + tls_registration.bucket_x, + tls_registration.bucket_y, + tls_registration.bucket_z, + session.load_cache_mode)) + { + spdlog::error("Error loading file!"); + return; + } + else + spdlog::info("Loaded: {} point_clouds", session.point_clouds_container.point_clouds.size()); + } + } + ImGui::SameLine(); + if (ImGui::Button("Save RESSO file")) + { + const auto output_file_name = mandeye::fd::SaveFileDialog("Save RESSO file", mandeye::fd::Resso_filter); + spdlog::info("RESSO file to save: '{}'", output_file_name); + if (output_file_name.size() > 0) + session.point_clouds_container.save_poses(fs::path(output_file_name).string(), false); + } + ImGui::Text("RESSO dataset: "); + ImGui::SameLine(); + ImGuiHyperlink("https://3d.bk.tudelft.nl/liangliang/publications/2019/plade/resso.html"); + + ImGui::NewLine(); + + if (ImGui::Button("Load ETH file (pairs.txt)")) + { + std::string input_file_name = ""; + input_file_name = mandeye::fd::OpenFileDialogOneFile("Load ETH file", {}); + if (input_file_name.size() > 0) + { + session.working_directory = fs::path(input_file_name).parent_path().string(); + + if (!session.point_clouds_container.load_eth( + session.working_directory.c_str(), + input_file_name.c_str(), + tls_registration.is_decimate, + tls_registration.bucket_x, + tls_registration.bucket_y, + tls_registration.bucket_z)) + { + spdlog::error("Error loading file!"); + return; + } + else + spdlog::info("Loaded: {} point_clouds", session.point_clouds_container.point_clouds.size()); + } + } + ImGui::Text("ETH dataset: "); + ImGui::SameLine(); + ImGuiHyperlink("https://prs.igp.ethz.ch/research/completed_projects/automatic_registration_of_point_clouds.html"); + + ImGui::NewLine(); + + if (ImGui::Button("Load 3DTK files (select all *.txt files)")) + { + session.point_clouds_container.point_clouds.clear(); + std::vector input_file_names; + input_file_names = mandeye::fd::OpenFileDialog("Load txt files", {}, true); + + if (input_file_names.size() > 0) + { + session.working_directory = fs::path(input_file_names[0]).parent_path().string(); + + spdlog::info("TXT files:"); + for (size_t i = 0; i < input_file_names.size(); i++) + spdlog::info("{}", input_file_names[i]); + + if (!session.point_clouds_container.load_3DTK_tls( + input_file_names, + tls_registration.is_decimate, + tls_registration.bucket_x, + tls_registration.bucket_y, + tls_registration.bucket_z)) + { + spdlog::error("Error loading file!"); + return; + } + else + spdlog::info("Loaded: {} point_clouds", session.point_clouds_container.point_clouds.size()); + } + } + ImGui::Text("3DTK dataset (18: the campus of the Jacobs University Bremen)"); + ImGui::SameLine(); + ImGuiHyperlink("http://kos.informatik.uni-osnabrueck.de/3Dscans/"); + + ImGui::NewLine(); + + if (ImGui::Button("Update initial poses from RESSO file")) + { + std::string input_file_name; + input_file_name = mandeye::fd::OpenFileDialogOneFile("Load RESSO file", {}); + if (input_file_name.size() > 0) + { + session.working_directory = fs::path(input_file_name).parent_path().string(); + + if (!session.point_clouds_container.update_initial_poses_from_RESSO( + session.working_directory.c_str(), input_file_name.c_str())) + { + spdlog::error("Error loading file!"); + return; + } + else + { + session.point_clouds_container.initial_poses_file_name = input_file_name; + spdlog::info("Updated: {} point_clouds", session.point_clouds_container.point_clouds.size()); + } + } + } + ImGui::SameLine(); + ImGui::Text(session.point_clouds_container.initial_poses_file_name.c_str()); + + if (ImGui::Button("Update poses from RESSO file")) + { + std::string input_file_name; + input_file_name = mandeye::fd::OpenFileDialogOneFile("Load RESSO file", {}); + + if (input_file_name.size() > 0) + { + session.working_directory = fs::path(input_file_name).parent_path().string(); + + if (!session.point_clouds_container.update_poses_from_RESSO(session.working_directory.c_str(), input_file_name.c_str())) + { + spdlog::error("Error loading file!"); + return; + } + else + { + spdlog::info("Updated: {} point_clouds", session.point_clouds_container.point_clouds.size()); + session.point_clouds_container.poses_file_name = input_file_name; + } + } + } + ImGui::SameLine(); + + if (ImGui::Button("Update poses from RESSO file (inverse)")) + { + std::string input_file_name; + input_file_name = mandeye::fd::OpenFileDialogOneFile("Load RESSO file", {}); + + if (input_file_name.size() > 0) + { + session.working_directory = fs::path(input_file_name).parent_path().string(); + + if (!session.point_clouds_container.update_poses_from_RESSO_inverse( + session.working_directory.c_str(), input_file_name.c_str())) + { + spdlog::error("Error loading file!"); + return; + } + else + { + spdlog::info("Updated: {} point_clouds", session.point_clouds_container.point_clouds.size()); + session.point_clouds_container.poses_file_name = input_file_name; + } + } + } + ImGui::SameLine(); + ImGui::Text(session.point_clouds_container.poses_file_name.c_str()); + + ImGui::Separator(); + + if (!is_loop_closure_gui) + { + static double x_origin = 0.0; + static double y_origin = 0.0; + static double z_origin = 0.0; + + ImGui::Text("Origin [m]: "); + ImGui::SameLine(); + ImGui::PushItemWidth(ImGuiNumberWidth); + ImGui::InputDouble("X##o", &x_origin, 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(xText); + ImGui::SameLine(); + ImGui::InputDouble("Y##o", &y_origin, 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(yText); + ImGui::SameLine(); + ImGui::InputDouble("Z##o", &z_origin, 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(zText); + ImGui::PopItemWidth(); + ImGui::SameLine(); + if (ImGui::Button("Set XYZ origin")) + { + if (session.point_clouds_container.point_clouds.size() != 0) + { + std::vector all_m_poses2; + for (size_t j = 0; j < session.point_clouds_container.point_clouds.size(); j++) + { + all_m_poses2.push_back(session.point_clouds_container.point_clouds[j].m_pose); + } + + session.point_clouds_container.point_clouds[0].m_pose(0, 3) = x_origin; + session.point_clouds_container.point_clouds[0].m_pose(1, 3) = y_origin; + session.point_clouds_container.point_clouds[0].m_pose(2, 3) = z_origin; + + session.point_clouds_container.point_clouds[0].pose = + pose_tait_bryan_from_affine_matrix(session.point_clouds_container.point_clouds[0].m_pose); + + session.point_clouds_container.point_clouds[0].gui_translation[0] = + (float)session.point_clouds_container.point_clouds[0].pose.px; + session.point_clouds_container.point_clouds[0].gui_translation[1] = + (float)session.point_clouds_container.point_clouds[0].pose.py; + session.point_clouds_container.point_clouds[0].gui_translation[2] = + (float)session.point_clouds_container.point_clouds[0].pose.pz; + + session.point_clouds_container.point_clouds[0].gui_rotation[0] = + (float)(session.point_clouds_container.point_clouds[0].pose.om * RAD_TO_DEG); + session.point_clouds_container.point_clouds[0].gui_rotation[1] = + (float)(session.point_clouds_container.point_clouds[0].pose.fi * RAD_TO_DEG); + session.point_clouds_container.point_clouds[0].gui_rotation[2] = + (float)(session.point_clouds_container.point_clouds[0].pose.ka * RAD_TO_DEG); + + Eigen::Affine3d curr_m_pose2 = session.point_clouds_container.point_clouds[0].m_pose; + for (size_t j = 1; j < session.point_clouds_container.point_clouds.size(); j++) + { + curr_m_pose2 = curr_m_pose2 * (all_m_poses2[j - 1].inverse() * all_m_poses2[j]); + + // spdlog::info(curr_m_pose2.matrix() << std::endl; + session.point_clouds_container.point_clouds[j].m_pose = curr_m_pose2; + 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); + } + } + } + + ImGui::Separator(); + + ImGui::Text("Set offsets to export point cloud in global coordinate system [m]:"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("each local coordinate of the point += offset"); + ImGui::PushItemWidth(ImGuiNumberWidth); + ImGui::InputDouble("X##t", &session.point_clouds_container.offset.x(), 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(xText); + ImGui::SameLine(); + ImGui::InputDouble("Y##t", &session.point_clouds_container.offset.y(), 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(yText); + ImGui::SameLine(); + ImGui::InputDouble("Z##t", &session.point_clouds_container.offset.z(), 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(zText); + ImGui::PopItemWidth(); + + if (ImGui::Button("Set offset_to_apply --> from session (X, Y, Z)")) + session.point_clouds_container.offset = session.point_clouds_container.offset_to_apply; + /* + ImGui::NewLine(); + ImGui::NewLine(); + ImGui::NewLine(); + ImGui::NewLine(); + ImGui::NewLine(); + ImGui::NewLine(); + ImGui::Separator(); + ImGui::Text("Perform experiment on:"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Experiments to compare methods and approaches for multi-view TLS registration"); + ImGui::SameLine(); + if (ImGui::Button("WINDOWS")) + perform_experiment_on_windows(session, observation_picking, tls_registration.icp, tls_registration.ndt, + tls_registration.registration_plane_feature, tls_registration.pose_graph_slam); ImGui::SameLine(); if (ImGui::Button("LINUX")) + perform_experiment_on_linux(session, observation_picking, tls_registration.icp, tls_registration.ndt, + tls_registration.registration_plane_feature, tls_registration.pose_graph_slam); + */ + } + } + + ImGui::End(); +} + +void display() +{ + ImGuiIO& io = ImGui::GetIO(); + glViewport(0, 0, (GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); + + glClearColor(bg_color.x * bg_color.w, bg_color.y * bg_color.w, bg_color.z * bg_color.w, bg_color.w); + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + glEnable(GL_DEPTH_TEST); + + glMatrixMode(GL_PROJECTION); + glLoadIdentity(); + float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); + + updateCameraTransition(); + + viewLocal = Eigen::Affine3f::Identity(); + + if (!is_ortho) + { + reshape((GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); + + // janusz + if (is_loop_closure_gui) + { + if (new_loop_closure_index) + { + // if (index_loop_closure_source < session.point_clouds_container.point_clouds.size()) + //{ + // new_rotation_center.x() = + // session.point_clouds_container.point_clouds[index_loop_closure_source].m_pose.translation().x(); + // new_rotation_center.y() = + // session.point_clouds_container.point_clouds[index_loop_closure_source].m_pose.translation().y(); + // new_rotation_center.z() = + // session.point_clouds_container.point_clouds[index_loop_closure_source].m_pose.translation().z(); + // + // new_translate_x = -new_rotation_center.x(); + // new_translate_y = -new_rotation_center.y(); + // camera_transition_active = true; + //} + + if (session.pose_graph_loop_closure.manipulate_active_edge) + { + if (session.pose_graph_loop_closure.edges.size() > 0) + { + if (session.pose_graph_loop_closure.index_active_edge < session.pose_graph_loop_closure.edges.size()) + { + new_rotation_center.x() = + session.point_clouds_container + .point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge] + .index_from] + .m_pose.translation() + .x(); + new_rotation_center.y() = + session.point_clouds_container + .point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge] + .index_from] + .m_pose.translation() + .y(); + new_rotation_center.z() = + session.point_clouds_container + .point_clouds[session.pose_graph_loop_closure.edges[session.pose_graph_loop_closure.index_active_edge] + .index_from] + .m_pose.translation() + .z(); + } + } + + new_rotate_x = rotate_x; + new_rotate_y = rotate_y; + new_translate_x = -new_rotation_center.x(); + new_translate_y = -new_rotation_center.y(); + new_translate_z = translate_z; + camera_transition_active = true; + } + + new_loop_closure_index = false; + } + } + + viewLocal.translate(rotation_center); + + viewLocal.translate(Eigen::Vector3f(translate_x, translate_y, translate_z)); + if (!lock_z) + viewLocal.rotate(Eigen::AngleAxisf(rotate_x * DEG_TO_RAD, Eigen::Vector3f::UnitX())); + else + viewLocal.rotate(Eigen::AngleAxisf(-90.0 * DEG_TO_RAD, Eigen::Vector3f::UnitX())); + viewLocal.rotate(Eigen::AngleAxisf(rotate_y * DEG_TO_RAD, Eigen::Vector3f::UnitZ())); + + viewLocal.translate(-rotation_center); + + glLoadMatrixf(viewLocal.matrix().data()); + } + else + updateOrthoView(); + + showAxes(); + + if (session.control_points.is_imgui) + session.control_points.render(session.point_clouds_container, true); + else + { + if (is_loop_closure_gui) + session.pose_graph_loop_closure.Render( + session.point_clouds_container, + index_loop_closure_source, + index_loop_closure_target, + num_edge_extended_before, + num_edge_extended_after); + + tls_registration.gnss.render(session.point_clouds_container); + session.ground_control_points.render(session.point_clouds_container); + session.control_points.render(session.point_clouds_container, false); + } + + int prev_index_pose = session.control_points.index_pose; + + if (prev_index_pose != session.control_points.index_pose) + { + session.control_points.index_picked_point = -1; // reset picked point when pose changes + + new_rotation_center.x() = session.point_clouds_container.point_clouds[session.control_points.index_pose].m_pose.translation().x(); + new_rotation_center.y() = session.point_clouds_container.point_clouds[session.control_points.index_pose].m_pose.translation().y(); + new_rotation_center.z() = session.point_clouds_container.point_clouds[session.control_points.index_pose].m_pose.translation().z(); + + new_rotate_x = rotate_x; + new_rotate_y = rotate_y; + if (session.control_points.track_pose_with_camera) + { + new_translate_x = -new_rotation_center.x(); + new_translate_y = -new_rotation_center.y(); + } + else + { + new_translate_x = translate_x; + new_translate_y = translate_y; + } + new_translate_z = translate_z; + camera_transition_active = true; + } + + ImGui_ImplOpenGL2_NewFrame(); + ImGui_ImplGLUT_NewFrame(); + ImGui::NewFrame(); + + ShowMainDockSpace(); + + if (session.control_points.is_imgui) + session.control_points.imgui(session.point_clouds_container, rotation_center); + + if (session.ground_control_points.is_imgui) + session.ground_control_points.imgui(session.point_clouds_container); + + if (!session.control_points.is_imgui) + { + 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_decimate_point_cloud, 1, session_dims); + + // spdlog::info("session.point_clouds_container.xy_grid_10x10 " << (int)session.point_clouds_container.xy_grid_10x10 << + // std::endl; + + 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(); + } + } + + glColor3f(1, 0, 1); + glBegin(GL_POINTS); + for (auto p : picked_points) + { + glVertex3f(p.x(), p.y(), p.z()); + } + glEnd(); + } + else + { + // ImGuizmo ----------------------------------------------- + if (session.pose_graph_loop_closure.gizmo && session.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.pose_graph_loop_closure.edges[session.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.pose_graph_loop_closure.edges[session.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 (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_A, false)) + { + is_pca_gui = !is_pca_gui; + + // workaround + io.AddKeyEvent(ImGuiKey_A, false); + io.AddKeyEvent(ImGuiMod_Ctrl, false); + } + + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_C, false)) + { + session.control_points.is_imgui = !session.control_points.is_imgui; + + // workaround + io.AddKeyEvent(ImGuiKey_C, false); + io.AddKeyEvent(ImGuiMod_Ctrl, false); + } + + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_E, false)) + { + is_lio_segments_gui = !is_lio_segments_gui; + + // workaround + io.AddKeyEvent(ImGuiKey_E, false); + io.AddKeyEvent(ImGuiMod_Ctrl, false); + } + + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_G, false)) + { + session.ground_control_points.is_imgui = !session.ground_control_points.is_imgui; + + // workaround + io.AddKeyEvent(ImGuiKey_G, false); + io.AddKeyEvent(ImGuiMod_Ctrl, false); + } + + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_L, false)) + { + is_loop_closure_gui = !is_loop_closure_gui; + + // workaround + io.AddKeyEvent(ImGuiKey_L, false); + io.AddKeyEvent(ImGuiMod_Ctrl, false); + } + + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_O, false)) + { + openSession(); + + // workaround + io.AddKeyEvent(ImGuiKey_O, false); + io.AddKeyEvent(ImGuiMod_Ctrl, false); + } + + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_P, false)) + { + is_pose_graph_slam = !is_pose_graph_slam; + + // workaround + io.AddKeyEvent(ImGuiKey_P, false); + io.AddKeyEvent(ImGuiMod_Ctrl, false); + } + + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_R)) // random colors + { + for (auto& pc : session.point_clouds_container.point_clouds) + { + pc.render_color[0] = float(rand() % 255) / 255.0f; + pc.render_color[1] = float(rand() % 255) / 255.0f; + pc.render_color[2] = float(rand() % 255) / 255.0f; + + if (csTrajectory == CS_FOLLOW) + { + pc.traj_color[0] = pc.render_color[0]; + pc.traj_color[1] = pc.render_color[1]; + pc.traj_color[2] = pc.render_color[2]; + } + } + + // workaround + io.AddKeyEvent(ImGuiKey_R, false); + io.AddKeyEvent(ImGuiMod_Ctrl, false); + } + + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_S, false)) + { + if (io.KeyShift) + saveSubsession(); + else + saveSession(); + + // workaround + io.AddKeyEvent(ImGuiKey_S, false); + io.AddKeyEvent(ImGuiMod_Ctrl, false); + } + + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_T)) // solid colors + { + csPointCloud = CS_SOLID; + + float color[3]; + if (session_loaded) + { + color[0] = session.point_clouds_container.point_clouds[0].render_color[0]; + color[1] = session.point_clouds_container.point_clouds[0].render_color[1]; + color[2] = session.point_clouds_container.point_clouds[0].render_color[2]; + } + + for (auto& pc : session.point_clouds_container.point_clouds) + { + pc.render_color[0] = color[0]; + pc.render_color[1] = color[1]; + pc.render_color[2] = color[2]; + + if (csTrajectory == CS_FOLLOW) + { + pc.traj_color[0] = pc.render_color[0]; + pc.traj_color[1] = pc.render_color[1]; + pc.traj_color[2] = pc.render_color[2]; + } + } + + // workaround + io.AddKeyEvent(ImGuiKey_T, false); + io.AddKeyEvent(ImGuiMod_Ctrl, false); + } + + if (ImGui::BeginMainMenuBar()) + { + if (!session_loaded) + { + if (ImGui::Button("Open session")) + openSession(); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Select session to open (Ctrl+O)"); + + ImGui::SameLine(); + + if (ImGui::ArrowButton("##menuArrow", ImGuiDir_Down)) + ImGui::OpenPopup("OpenMenu"); + + if (ImGui::BeginPopup("OpenMenu")) + { + ImGui::MenuItem("Calculate_offset", nullptr, &tls_registration.calculate_offset); + ImGui::MenuItem("Fill in session", nullptr, &fillInSession); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Fill in data for trajectory and pose to create complete session"); + + if (ImGui::MenuItem("Open las/laz")) + openLaz(fillInSession); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Create session from las/laz file(s)"); + + ImGui::EndPopup(); + } + + ImGui::SameLine(); + ImGui::Dummy(ImVec2(20, 0)); + ImGui::SameLine(); + } + else + { + if (ImGui::BeginMenu("File")) + { + if (ImGui::MenuItem("Save session as", "Ctrl+S")) + saveSession(); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Save changes of full session with posibility to change filename"); + + // ImGui::BeginDisabled(!((index_begin > 0) || (index_end < + // static_cast(session.point_clouds_container.point_clouds.size() - 1)))); + //{ + if (ImGui::MenuItem("Save subsession", "Ctrl+Shift+S")) + saveSubsession(); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Save session according to selections from 'LIO segments editor' window"); + //} + // ImGui::EndDisabled(); + + ImGui::Separator(); + if (ImGui::BeginMenu("Save all marked scans")) + { + static bool skip_ts_0 = true; + ImGui::Checkbox("Skip points with zero ts", &skip_ts_0); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Invalid points with zero timestamp will be skipped during export"); + + if (ImGui::MenuItem("Local scan")) + { + const auto output_file_name = mandeye::fd::SaveFileDialog(out_fn.c_str(), mandeye::fd::LAS_LAZ_filter, ".laz"); + spdlog::info("laz file to save: '{}'", output_file_name); + + if (output_file_name.size() > 0) + save_all_to_las(session, output_file_name, true, skip_ts_0); + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("As one local scan transformed via inverse pose of first scan"); + + if (ImGui::MenuItem("Global scan")) + { + const auto output_file_name = mandeye::fd::SaveFileDialog(out_fn.c_str(), mandeye::fd::LAS_LAZ_filter, ".laz"); + spdlog::info("laz file to save: '{}'", output_file_name); + + if (output_file_name.size() > 0) + save_all_to_las(session, output_file_name, false, skip_ts_0); + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "To export in full resolution, close the program and open again and unmark 'downsample during " + "load' before loading session"); + ImGui::Separator(); + if (ImGui::MenuItem("Separate global scans (laz)")) + { + std::string output_folder_name_separately = ""; + output_folder_name_separately = mandeye::fd::SelectFolder("Choose folder"); + save_separately_to_las(session, output_folder_name_separately, ".laz"); + } + + if (ImGui::MenuItem("Separate global scans (las)")) + { + std::string output_folder_name_separately = ""; + output_folder_name_separately = mandeye::fd::SelectFolder("Choose folder"); + save_separately_to_las(session, output_folder_name_separately, ".las"); + } + + ImGui::EndMenu(); + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Save all marked scans as las/laz files"); + + if (ImGui::BeginMenu("Save all marked trajectories")) + { + ImGui::MenuItem("is_trajectory_export_downsampling", nullptr, &tls_registration.is_trajectory_export_downsampling); + ImGui::PushItemWidth(ImGuiNumberWidth); + ImGui::InputFloat("curve_consecutive_distance [m]", &tls_registration.curve_consecutive_distance_meters); + ImGui::InputFloat("not_curve_consecutive_distance [m]", &tls_registration.not_curve_consecutive_distance_meters); + ImGui::PopItemWidth(); + + if (ImGui::MenuItem("Save all as las/laz files")) + { + const auto output_file_name = mandeye::fd::SaveFileDialog(out_fn.c_str(), mandeye::fd::LAS_LAZ_filter, ".laz"); + spdlog::info("laz file to save: '{}'", output_file_name); + if (output_file_name.size() > 0) + save_trajectories_to_laz( + session, + output_file_name, + tls_registration.curve_consecutive_distance_meters, + tls_registration.not_curve_consecutive_distance_meters, + tls_registration.is_trajectory_export_downsampling); + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("As one global scan"); + + ImGui::Separator(); + + ImGui::Text("(x,y,z,r00,r01,r02,r10,r11,r12,r20,r21,r22)"); + if (ImGui::MenuItem("Save all as csv (timestamp Lidar)##1")) + { + const auto output_file_name = mandeye::fd::SaveFileDialog(out_fn.c_str(), mandeye::fd::Csv_filter, ".csv"); + spdlog::info("csv file to save: '{}'", output_file_name); + + if (output_file_name.size() > 0) + save_trajectories( + session, + output_file_name, + tls_registration.curve_consecutive_distance_meters, + tls_registration.not_curve_consecutive_distance_meters, + tls_registration.is_trajectory_export_downsampling, + true, + false, + false, + false); + } + if (ImGui::MenuItem("Save all as csv (timestamp Unix)##1")) + { + const auto output_file_name = mandeye::fd::SaveFileDialog(out_fn.c_str(), mandeye::fd::Csv_filter, ".csv"); + spdlog::info("csv file to save: '{}'", output_file_name); + + if (output_file_name.size() > 0) + save_trajectories( + session, + output_file_name, + tls_registration.curve_consecutive_distance_meters, + tls_registration.not_curve_consecutive_distance_meters, + tls_registration.is_trajectory_export_downsampling, + false, + true, + false, + false); + } + if (ImGui::MenuItem("Save all as csv (timestamp Lidar, Unix)##1")) + { + const auto output_file_name = mandeye::fd::SaveFileDialog(out_fn.c_str(), mandeye::fd::Csv_filter, ".csv"); + spdlog::info("csv file to save: '{}'", output_file_name); + + if (output_file_name.size() > 0) + save_trajectories( + session, + output_file_name, + tls_registration.curve_consecutive_distance_meters, + tls_registration.not_curve_consecutive_distance_meters, + tls_registration.is_trajectory_export_downsampling, + true, + true, + false, + false); + } + + ImGui::Separator(); + ImGui::Text("(x,y,z,qx,qy,qz,qw)"); + + if (ImGui::MenuItem("Save all as csv (timestamp Lidar)##2")) + { + const auto output_file_name = mandeye::fd::SaveFileDialog(out_fn.c_str(), mandeye::fd::Csv_filter, ".csv"); + spdlog::info("csv file to save: '{}'", output_file_name); + + if (output_file_name.size() > 0) + save_trajectories( + session, + output_file_name, + tls_registration.curve_consecutive_distance_meters, + tls_registration.not_curve_consecutive_distance_meters, + tls_registration.is_trajectory_export_downsampling, + true, + false, + true, + false); + } + if (ImGui::MenuItem("Save all as csv (timestamp Unix)##2")) + { + const auto output_file_name = mandeye::fd::SaveFileDialog(out_fn.c_str(), mandeye::fd::Csv_filter, ".csv"); + spdlog::info("csv file to save: '{}'", output_file_name); + + if (output_file_name.size() > 0) + save_trajectories( + session, + output_file_name, + tls_registration.curve_consecutive_distance_meters, + tls_registration.not_curve_consecutive_distance_meters, + tls_registration.is_trajectory_export_downsampling, + false, + true, + true, + false); + } + if (ImGui::MenuItem("Save all as csv (timestamp Lidar, Unix)##2")) + { + const auto output_file_name = mandeye::fd::SaveFileDialog(out_fn.c_str(), mandeye::fd::Csv_filter, ".csv"); + spdlog::info("csv file to save: '{}'", output_file_name); + + if (output_file_name.size() > 0) + save_trajectories( + session, + output_file_name, + tls_registration.curve_consecutive_distance_meters, + tls_registration.not_curve_consecutive_distance_meters, + tls_registration.is_trajectory_export_downsampling, + true, + true, + true, + false); + } + + ImGui::Separator(); + + if (ImGui::MenuItem("Save all as dxf as polyline")) + { + const auto output_file_name = mandeye::fd::SaveFileDialog("Ouput file name", mandeye::fd::Dxf_filter, ".dxf"); + spdlog::info("dxf file to save: '{}'", output_file_name); + + if (output_file_name.size() > 0) + save_trajectories( + session, + output_file_name, + tls_registration.curve_consecutive_distance_meters, + tls_registration.not_curve_consecutive_distance_meters, + tls_registration.is_trajectory_export_downsampling, + false, + false, + false, + true); + } + + ImGui::EndMenu(); + } + + if (ImGui::BeginMenu("Save scale board")) + { + ImGui::Text("For all marked trajectories as one global scan to laz"); + + if (ImGui::MenuItem("> dec 0.1")) + { + const auto output_file_name = mandeye::fd::SaveFileDialog(out_fn.c_str(), mandeye::fd::LAS_LAZ_filter, ".laz"); + spdlog::info("laz file to save: '{}'", output_file_name); + + if (output_file_name.size() > 0) + save_scale_board_to_laz(session, output_file_name, 0.1); + } + + if (ImGui::MenuItem("> dec 1.0")) + { + const auto output_file_name = mandeye::fd::SaveFileDialog(out_fn.c_str(), mandeye::fd::LAS_LAZ_filter, ".laz"); + spdlog::info("laz file to save: '{}'", output_file_name); + + if (output_file_name.size() > 0) + save_scale_board_to_laz(session, output_file_name, 1.0); + } + + if (ImGui::MenuItem("> dec 10.0")) + { + const auto output_file_name = mandeye::fd::SaveFileDialog(out_fn.c_str(), mandeye::fd::LAS_LAZ_filter, ".laz"); + spdlog::info("laz file to save: '{}'", output_file_name); + + if (output_file_name.size() > 0) + save_scale_board_to_laz(session, output_file_name, 10.0); + } + + ImGui::Separator(); + ImGui::Text("10km x 10km to laz"); + + if (ImGui::MenuItem("> 10m")) + { + const auto output_file_name = mandeye::fd::SaveFileDialog(out_fn.c_str(), mandeye::fd::LAS_LAZ_filter, ".laz"); + spdlog::info("laz file to save: '{}'", output_file_name); + + if (output_file_name.size() > 0) + save_scale_board_to_laz(session, output_file_name, 10.0, 10000.0); + } + + if (ImGui::MenuItem("> 100m")) + { + const auto output_file_name = mandeye::fd::SaveFileDialog(out_fn.c_str(), mandeye::fd::LAS_LAZ_filter, ".laz"); + spdlog::info("laz file to save: '{}'", output_file_name); + + if (output_file_name.size() > 0) + save_scale_board_to_laz(session, output_file_name, 100.0, 10000.0); + } + + if (ImGui::MenuItem("> 1000m")) + { + const auto output_file_name = mandeye::fd::SaveFileDialog(out_fn.c_str(), mandeye::fd::LAS_LAZ_filter, ".laz"); + spdlog::info("laz file to save: '{}'", output_file_name); + + if (output_file_name.size() > 0) + save_scale_board_to_laz(session, output_file_name, 1000.0, 10000.0); + } + + ImGui::EndMenu(); + } + + if (ImGui::BeginMenu("GNSS")) + { + ImGui::MenuItem("Load with offset -> move to (0,0,0)", nullptr, &gnssWithOffset); + + if (ImGui::MenuItem("Load GNSS files and convert WGS84 to PUWG92")) + { + std::vector input_file_names; + input_file_names = mandeye::fd::OpenFileDialog("Load gnss files", { "GNSS", "*.gnss" }, true); + + if (input_file_names.size() > 0) + { + Eigen::Vector3d out_offset(0.0, 0.0, 0.0); + if (!tls_registration.gnss.load_data_from_gnss_and_convert_to_92(input_file_names, out_offset, gnssWithOffset)) + { + spdlog::error("Error loading GNSS files!"); + } + else + { + session.point_clouds_container.offset_to_apply = out_offset; + } + } + } + + ImGui::MenuItem("Set WGS84 reference from 1st pose", nullptr, &tls_registration.gnss.setWGS84ReferenceFromFirstPose); + + ImGui::Text("Load & convert WGS84 to Cartesian by Mercator projection"); + + if (ImGui::MenuItem("Load GNSS (deprecated)")) + { + std::vector input_file_names; + input_file_names = mandeye::fd::OpenFileDialog("Load gnss files", { "GNSS", "*.gnss" }, true); + + if (input_file_names.size() > 0) + { + if (!tls_registration.gnss.load_raw_data_from_gnss(input_file_names)) + { + spdlog::error("Error loading GNSS files!"); + } + if (!tls_registration.gnss.project_to_mercator_projection()) + { + spdlog::error("Error converting WGS84 to Mercator projection!"); + } + } + } + if (ImGui::MenuItem("Load GNSS")) + { + std::vector input_file_names; + input_file_names = mandeye::fd::OpenFileDialog("Load gnss files", { "GNSS", "*.gnss" }, true); + + if (input_file_names.size() > 0) + { + if (!tls_registration.gnss.load_raw_data_from_gnss(input_file_names)) + { + spdlog::error("Error loading GNSS files!"); + } + if (!tls_registration.gnss.project_using_proj()) + { + spdlog::error("Error converting WGS84 to PROJ projection!"); + } + } + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Load structured GNSS dataset and decode it into coordinates, using PROJ library"); + + if (ImGui::MenuItem("Load NMEA (deprecated)")) + { + std::vector input_file_names; + input_file_names = mandeye::fd::OpenFileDialog("Load nmea files", { "NMEA", "*.nmea" }, true); + + if (input_file_names.size() > 0) + { + if (!tls_registration.gnss.load_raw_data_from_nmea(input_file_names)) + spdlog::error("Error loading NMEA files!"); + } + if (!tls_registration.gnss.project_to_mercator_projection()) + { + spdlog::error("Error converting WGS84 to Mercator projection!"); + } + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Load raw GNSS serial output and decode it into coordinates"); + if (ImGui::MenuItem("Load NMEA")) + { + std::vector input_file_names; + input_file_names = mandeye::fd::OpenFileDialog("Load nmea files", { "NMEA", "*.nmea" }, true); + + if (input_file_names.size() > 0) + { + if (!tls_registration.gnss.load_raw_data_from_nmea(input_file_names)) + spdlog::error("Error loading NMEA files!"); + } + if (!tls_registration.gnss.project_using_proj()) + { + spdlog::error("Error converting WGS84 to PROJ projection!"); + } + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Load raw GNSS serial output and decode it into coordinates, using PROJ library"); + + ImGui::Separator(); + + if (ImGui::MenuItem("Save GNSS data to las/laz file")) + { + const auto output_file_name = + mandeye::fd::SaveFileDialog("Save las or laz file", mandeye::fd::LAS_LAZ_filter, ".laz"); + spdlog::info("las or laz file to save: '{}'", output_file_name); + + if (output_file_name.size() > 0) + tls_registration.gnss.save_to_laz( + output_file_name, + session.point_clouds_container.offset.x(), + session.point_clouds_container.offset.y(), + session.point_clouds_container.offset.z()); + } + const auto prepareVisibleData = [&]() + { + std::vector pointcloud; + std::vector intensity; + std::vector timestamps; + + for (auto& p : session.point_clouds_container.point_clouds) + { + if (p.visible) + { + for (size_t i = 0; i < p.points_local.size(); i++) + { + const auto& pp = p.points_local[i]; + Eigen::Vector3d vp; + vp = p.m_pose * pp; + + 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]); + } + } + } + return std::tuple(pointcloud, intensity, timestamps); + }; + + if (ImGui::MenuItem("Save metascan points in PUWG92(dep!)")) + { + const auto output_file_name = mandeye::fd::SaveFileDialog(out_fn.c_str(), mandeye::fd::LAS_LAZ_filter, ".laz"); + const auto [pointcloud, intensity, timestamps] = prepareVisibleData(); + + const auto lat = tls_registration.gnss.WGS84ReferenceLatitude; + const auto lon = tls_registration.gnss.WGS84ReferenceLongitude; + const auto alt = tls_registration.gnss.gnss_poses[0].alt; + + double Xpuwg92 = 0.0; + double Ypuwg92 = 0.0; + wgs84_do_puwg92(lat, lon, &Xpuwg92, &Ypuwg92); + Eigen::Vector3d offset(Ypuwg92, Xpuwg92, alt); + exportLaz(output_file_name, pointcloud, intensity, timestamps, offset.x(), offset.y(), offset.z()); + } + ImGui::Separator(); + for (const auto& geoid : geoids) + { + if (ImGui::MenuItem(std::string("Set geoid to " + geoid).c_str(), nullptr, selected_geoid_model == geoid)) + { + selected_geoid_model = geoid; + } + } + ImGui::Separator(); + + for (const auto& crtName : CRTs::SupportedCRTs) + { + std::string itemName = "Save metascan points in " + crtName + " (PROJ)"; + if (ImGui::MenuItem(itemName.c_str())) + { + const auto output_file_name = mandeye::fd::SaveFileDialog(out_fn.c_str(), mandeye::fd::LAS_LAZ_filter, ".laz"); + + const auto [pointcloud, intensity, timestamps] = prepareVisibleData(); + const auto lla_points = tls_registration.gnss.unproject_using_proj(pointcloud); + auto crt_points = tls_registration.gnss.CRTConvert(lla_points, crtName, selected_geoid_model); + Eigen::Vector3d offset = crt_points.front(); + for (auto& p : crt_points) + { + p = p - offset; + } + exportLaz(output_file_name, crt_points, intensity, timestamps, offset.x(), offset.y(), offset.z()); + } + } + + for (const auto& crtName : CRTs::SupportedCRTs) + { + std::string itemName = "Save GNSS data to las/laz in " + crtName + " (PROJ)"; + if (ImGui::MenuItem(itemName.c_str())) + { + const auto output_file_name = mandeye::fd::SaveFileDialog(out_fn.c_str(), mandeye::fd::LAS_LAZ_filter, ".laz"); + + std::vector lla_points; + std::vector intensity; + std::vector timestamps; + for (const auto& gnss : tls_registration.gnss.gnss_poses) + { + lla_points.emplace_back(gnss.lat, gnss.lon, gnss.h_wgs84); + intensity.push_back(gnss.hdop); + timestamps.push_back(gnss.timestamp); + } + + auto crt_points = tls_registration.gnss.CRTConvert(lla_points, crtName, selected_geoid_model); + Eigen::Vector3d offset = crt_points.front(); + for (auto& p : crt_points) + { + p = p - offset; + } + exportLaz(output_file_name, crt_points, intensity, timestamps, offset.x(), offset.y(), offset.z()); + } + } + + ImGui::EndMenu(); + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("GNSS (GPS, etc.) related open/save commands"); + + ImGui::EndMenu(); + } + if (ImGui::IsItemHovered()) + { + ImGui::BeginTooltip(); + ImGui::Text("Loaded session:"); + ImGui::Text(std::string(session.session_file_name).c_str()); + ImGui::Separator(); + ImGui::Text("Total number of points: %zu", session_total_number_of_points); + + if (ImGui::BeginTable("Dimensions", 4)) + { + ImGui::TableSetupColumn("Coord [m]"); + ImGui::TableSetupColumn("min"); + ImGui::TableSetupColumn("max"); + ImGui::TableSetupColumn("size"); + ImGui::TableHeadersRow(); + + ImGui::TableNextRow(); + ImGui::TableSetColumnIndex(0); + + std::string text = "X"; + float centered = ImGui::GetColumnWidth() - ImGui::CalcTextSize(text.c_str()).x; + // Set cursor so text is centered + ImGui::SetCursorPosX(ImGui::GetCursorPosX() + centered * 0.5f); + + ImGui::Text("X"); + + ImGui::TableSetColumnIndex(1); + ImGui::Text("%.3f", session_dims.x_min); + ImGui::TableSetColumnIndex(2); + ImGui::Text("%.3f", session_dims.x_max); + ImGui::TableSetColumnIndex(3); + ImGui::Text("%.3f", session_dims.length); + + ImGui::TableNextRow(); + ImGui::TableSetColumnIndex(0); + ImGui::SetCursorPosX(ImGui::GetCursorPosX() + centered * 0.5f); + ImGui::Text("Y"); + + ImGui::TableSetColumnIndex(1); + ImGui::Text("%.3f", session_dims.y_min); + ImGui::TableSetColumnIndex(2); + ImGui::Text("%.3f", session_dims.y_max); + ImGui::TableSetColumnIndex(3); + ImGui::Text("%.3f", session_dims.width); + + ImGui::TableNextRow(); + ImGui::TableSetColumnIndex(0); + ImGui::SetCursorPosX(ImGui::GetCursorPosX() + centered * 0.5f); + ImGui::Text("Z"); + + ImGui::TableSetColumnIndex(1); + ImGui::Text("%.3f", session_dims.z_min); + ImGui::TableSetColumnIndex(2); + ImGui::Text("%.3f", session_dims.z_max); + ImGui::TableSetColumnIndex(3); + ImGui::Text("%.3f", session_dims.height); + + ImGui::EndTable(); + } + + ImGui::EndTooltip(); + } + + if (ImGui::BeginMenu("Tools")) + { + ImGui::MenuItem("Point Cloud Alignment", "Ctrl+A", &is_pca_gui); + if (ImGui::IsItemHovered()) + { + ImGui::BeginTooltip(); + ImGui::Text("Point cloud alignment (registration) algorithms:"); + ImGui::Text( + "(aligning two 3D point sets from LiDAR scans, by estimating\nthe relative pose(translation and rotation) " + "between them)"); + ImGui::Text("- Normal Distributions Transform"); + ImGui::Text("- Iterative Closest Point"); + ImGui::Text("- Registration Plane Feature"); + ImGui::EndTooltip(); + } + ImGui::Separator(); + ImGui::MenuItem("Pose Graph SLAM", "Ctrl+P", &is_pose_graph_slam); + ImGui::MenuItem("Observations", nullptr, &is_manual_analisys); + + ImGui::Separator(); + + ImGui::MenuItem("Control Points", "Ctrl+C", &session.control_points.is_imgui, !session.ground_control_points.is_imgui); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Known reference points used to align or verify the scan"); + + ImGui::MenuItem( + "Ground Control Points", "Ctrl+G", &session.ground_control_points.is_imgui, !session.control_points.is_imgui); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Accurate real-world points used to georeference the scan"); + + ImGui::MenuItem("Manual Loop Closure", "Ctrl+L", &is_loop_closure_gui, !is_lio_segments_gui); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Manually connect overlapping scan sections"); + + ImGui::Separator(); + ImGui::MenuItem("LIO segments editor", "Ctrl+E", &is_lio_segments_gui, !is_loop_closure_gui); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Manually adjust or review Lidar Inertial Odometry trajectory segments"); + + ImGui::Separator(); + ImGui::MenuItem("Translate", nullptr, &is_translate_gui); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Define new coordinate frame from 3 picked points and transform all clouds"); + + ImGui::EndMenu(); + } + + if (ImGui::BeginMenu("Intersections")) + { + ImGui::SetNextItemWidth(ImGuiNumberWidth); + ImGui::InputDouble("Intersection width [m]", &session.point_clouds_container.intersection_width, 0.0, 0.0, "%.2f"); + if (session.point_clouds_container.intersection_width < 0.001) + session.point_clouds_container.intersection_width = 0.001; + + ImGui::Separator(); + + ImGui::MenuItem("xz_intersection", nullptr, &session.point_clouds_container.xz_intersection); + ImGui::MenuItem("10m grid##xz", nullptr, &session.point_clouds_container.xz_grid_10x10); + ImGui::MenuItem("1m grid##xz", nullptr, &session.point_clouds_container.xz_grid_1x1); + ImGui::MenuItem("0.1m grid##xz", nullptr, &session.point_clouds_container.xz_grid_01x01); + + if (ImGui::MenuItem("Export xz intersection", nullptr, false, session.point_clouds_container.xz_intersection)) + { + const auto output_file_name = mandeye::fd::SaveFileDialog(out_fn.c_str(), mandeye::fd::LAS_LAZ_filter, ".laz"); + spdlog::info("laz file to save: '{}'", output_file_name); + + if (output_file_name.size() > 0) + { + save_intersection( + session, + output_file_name, + session.point_clouds_container.xz_intersection, + session.point_clouds_container.yz_intersection, + session.point_clouds_container.xy_intersection, + session.point_clouds_container.intersection_width); + } + } + + ImGui::Separator(); + + ImGui::MenuItem("yz_intersection", nullptr, &session.point_clouds_container.yz_intersection); + ImGui::MenuItem("10m grid##yz", nullptr, &session.point_clouds_container.yz_grid_10x10); + ImGui::MenuItem("1m grid##yz", nullptr, &session.point_clouds_container.yz_grid_1x1); + ImGui::MenuItem("0.1m grid##yz", nullptr, &session.point_clouds_container.yz_grid_01x01); + + if (ImGui::MenuItem("Export yz intersection", nullptr, false, session.point_clouds_container.yz_intersection)) + { + const auto output_file_name = mandeye::fd::SaveFileDialog(out_fn.c_str(), mandeye::fd::LAS_LAZ_filter, ".laz"); + spdlog::info("laz file to save: '{}'", output_file_name); + + if (output_file_name.size() > 0) + { + save_intersection( + session, + output_file_name, + session.point_clouds_container.xz_intersection, + session.point_clouds_container.yz_intersection, + session.point_clouds_container.xy_intersection, + session.point_clouds_container.intersection_width); + } + } + + ImGui::Separator(); + + ImGui::MenuItem("xy_intersection", nullptr, &session.point_clouds_container.xy_intersection); + ImGui::MenuItem("10m grid##xy", nullptr, &session.point_clouds_container.xy_grid_10x10); + ImGui::MenuItem("1m grid##xy", nullptr, &session.point_clouds_container.xy_grid_1x1); + ImGui::MenuItem("0.1m grid##xy", nullptr, &session.point_clouds_container.xy_grid_01x01); + + if (ImGui::MenuItem("Export xy intersection", nullptr, false, session.point_clouds_container.xy_intersection)) + { + const auto output_file_name = mandeye::fd::SaveFileDialog(out_fn.c_str(), mandeye::fd::LAS_LAZ_filter, ".laz"); + spdlog::info("laz file to save: '{}'", output_file_name); + + if (output_file_name.size() > 0) + { + save_intersection( + session, + output_file_name, + session.point_clouds_container.xz_intersection, + session.point_clouds_container.yz_intersection, + session.point_clouds_container.xy_intersection, + session.point_clouds_container.intersection_width); + } + } + + ImGui::EndMenu(); + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Intersection menu"); + } + + if (ImGui::BeginMenu("View")) + { + ImGui::BeginDisabled(!session_loaded); + { + if (ImGui::BeginMenu("Point cloud")) + { + auto tmp = point_size; + ImGui::SetNextItemWidth(ImGuiNumberWidth); + ImGui::InputInt("Points size", &point_size); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("keyboard 1-9 keys"); + if (point_size < 1) + point_size = 1; + else if (point_size > 10) + point_size = 10; + + if (tmp != point_size) + for (auto& point_cloud : session.point_clouds_container.point_clouds) + point_cloud.point_size = point_size; + + ImGui::Separator(); + + ImGui::Text("Color:"); + + float color[3]; + if (session_loaded) + { + color[0] = session.point_clouds_container.point_clouds[0].render_color[0]; + color[1] = session.point_clouds_container.point_clouds[0].render_color[1]; + color[2] = session.point_clouds_container.point_clouds[0].render_color[2]; + } + + if (ImGui::ColorEdit3("", (float*)&color, ImGuiColorEditFlags_NoInputs)) + { + csPointCloud = CS_SOLID; + + for (auto& pc : session.point_clouds_container.point_clouds) + { + pc.render_color[0] = color[0]; + pc.render_color[1] = color[1]; + pc.render_color[2] = color[2]; + + if (csTrajectory == CS_FOLLOW) + { + pc.traj_color[0] = pc.render_color[0]; + pc.traj_color[1] = pc.render_color[1]; + pc.traj_color[2] = pc.render_color[2]; + } + } + } + ImGui::SameLine(); + if (ImGui::MenuItem("> Solid", nullptr, (csPointCloud == CS_SOLID))) + csPointCloud = CS_SOLID; + + if (ImGui::MenuItem("> Random per segment", "Ctrl+R", (csPointCloud == CS_RANDOM))) + { + csPointCloud = CS_RANDOM; + + for (auto& pc : session.point_clouds_container.point_clouds) + { + pc.render_color[0] = float(rand() % 255) / 255.0f; + pc.render_color[1] = float(rand() % 255) / 255.0f; + pc.render_color[2] = float(rand() % 255) / 255.0f; + + if (csTrajectory == CS_FOLLOW) + { + pc.traj_color[0] = pc.render_color[0]; + pc.traj_color[1] = pc.render_color[1]; + pc.traj_color[2] = pc.render_color[2]; + } + } + } + + ImGui::EndMenu(); + } + + if (ImGui::BeginMenu("Trajectory")) + { + if (session_loaded) + { + auto tmp = session.point_clouds_container.point_clouds[0].line_width; + + ImGui::BeginDisabled(!glLineWidthSupport); + { + ImGui::SetNextItemWidth(ImGuiNumberWidth); + ImGui::InputInt("Line width", &tmp); + } + ImGui::EndDisabled(); + + if (tmp < 0) + tmp = 0; + else if (tmp > 5) + tmp = 5; + + if (tmp != session.point_clouds_container.point_clouds[0].line_width) + for (auto& point_cloud : session.point_clouds_container.point_clouds) + point_cloud.line_width = tmp; + } + + ImGui::MenuItem("Show IMU to LIO difference", nullptr, &session.point_clouds_container.show_imu_to_lio_diff); + + ImGui::Separator(); + + ImGui::Text("Color:"); + + float color[3]; + if (session_loaded) + { + color[0] = session.point_clouds_container.point_clouds[0].traj_color[0]; + color[1] = session.point_clouds_container.point_clouds[0].traj_color[1]; + color[2] = session.point_clouds_container.point_clouds[0].traj_color[2]; + } + + if (ImGui::ColorEdit3("", (float*)&color, ImGuiColorEditFlags_NoInputs)) + { + csTrajectory = CS_SOLID; + + for (auto& pc : session.point_clouds_container.point_clouds) + { + pc.traj_color[0] = color[0]; + pc.traj_color[1] = color[1]; + pc.traj_color[2] = color[2]; + } + } + ImGui::SameLine(); + if (ImGui::MenuItem("> Solid", nullptr, (csTrajectory == CS_SOLID))) + csTrajectory = CS_SOLID; + + if (ImGui::MenuItem("> Random per segment", nullptr, (csTrajectory == CS_RANDOM))) + { + csTrajectory = CS_RANDOM; + + for (auto& pc : session.point_clouds_container.point_clouds) + { + pc.traj_color[0] = float(rand() % 255) / 255.0f; + pc.traj_color[1] = float(rand() % 255) / 255.0f; + pc.traj_color[2] = float(rand() % 255) / 255.0f; + } + } + + if (ImGui::MenuItem("> Follow cloud color", nullptr, (csTrajectory == CS_FOLLOW))) + { + csTrajectory = CS_FOLLOW; + + for (auto& pc : session.point_clouds_container.point_clouds) + { + pc.traj_color[0] = pc.render_color[0]; + pc.traj_color[1] = pc.render_color[1]; + pc.traj_color[2] = pc.render_color[2]; + } + } + + ImGui::EndMenu(); + } + + ImGui::ColorEdit3("Background color", (float*)&bg_color, ImGuiColorEditFlags_NoInputs); + + ImGui::BeginDisabled(tls_registration.gnss.gnss_poses.size() <= 0); + { + ImGui::MenuItem("Show GNSS correspondences", nullptr, &tls_registration.gnss.show_correspondences); + } + ImGui::EndDisabled(); + + ImGui::Separator(); + } + ImGui::EndDisabled(); + + if (ImGui::MenuItem("Orthographic", "key O", &is_ortho)) + { + if (is_ortho) + { + new_rotation_center = rotation_center; + new_rotate_x = 0.0; + new_rotate_y = 0.0; + new_translate_x = translate_x; + new_translate_y = translate_y; + new_translate_z = translate_z; + camera_transition_active = true; + } + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Switch between perspective view (3D) and orthographic view (2D/flat)"); + + ImGui::MenuItem("Show axes", "key X", &show_axes); + ImGui::MenuItem("Show compass/ruler", "key C", &compass_ruler); + + ImGui::MenuItem("Lock Z", "Shift + Z", &lock_z, !is_ortho); + + ImGui::Separator(); + + ImGui::MenuItem("Settings", nullptr, &is_settings_gui); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Show power user settings window with more parameters"); + + if (ImGui::BeginMenu("Console")) + { +#ifdef _WIN32 + if (ImGui::MenuItem("Use Windows console", nullptr, &consWin)) + { + if (consWin) + { + AllocConsole(); + freopen("CONOUT$", "w", stdout); + freopen("CONOUT$", "w", stderr); + freopen("CONIN$", "r", stdin); + } + else + FreeConsole(); + } + if (ImGui::IsItemHovered()) + { + ImGui::BeginTooltip(); + ImGui::Text("!!! If not used.. !!!"); + ImGui::Text("- old console output is lost"); + ImGui::Text("- new console output can only be seen in subwindow"); + ImGui::Text("- app might run faster"); + ImGui::EndTooltip(); + } +#endif + // ImGui::MenuItem("Subwindow", nullptr, &consImGui); + // if (ImGui::IsItemHovered()) + // ImGui::SetTooltip("Show/hide console output as GUI window"); + + ImGui::EndMenu(); + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Control console output"); + + ImGui::EndMenu(); + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Scene view relevant parameters"); + + camMenu(); + + ImGui::BeginDisabled(session.point_clouds_container.point_clouds.size() <= 0); + { + ImGui::SameLine(); + ImGui::Dummy(ImVec2(20, 0)); + ImGui::SameLine(); + + ImGui::SetNextItemWidth(ImGuiNumberWidth); + ImGui::InputInt("Points render downsampling", &viewer_decimate_point_cloud, 10, 100); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("increase for better performance, decrease for rendering more points"); + ImGui::SameLine(); + + // fps_avg = fps_avg * 0.7f + ImGui::GetIO().Framerate * 0.3f; // exponential smoothing + + // double now = ImGui::GetTime(); // ImGui’s built-in timer (in seconds) + + // ImGui::Checkbox("dynamic", &dynamicSubsampling); + // if (ImGui::IsItemHovered()) + // ImGui::SetTooltip("automatically control subsampling vs FPS: increase bellow 10, decrease above 60"); + // if (dynamicSubsampling && (fps_avg < 15) && (now - lastAdjustTime > cooldownSeconds)) + //{ + // viewer_decimate_point_cloud += 1; + // lastAdjustTime = now; + //} + // ImGui::SameLine(); + // ImGui::Text("(avg %.1f)", fps_avg); + + if (viewer_decimate_point_cloud < 1) + viewer_decimate_point_cloud = 1; + + ImGui::SameLine(); + ImGui::Text("(%.1f FPS)", ImGui::GetIO().Framerate); + } + ImGui::EndDisabled(); + + ImGui::SameLine( + ImGui::GetWindowWidth() - ImGui::CalcTextSize("Info").x - ImGui::GetStyle().ItemSpacing.x * 2 - + ImGui::GetStyle().FramePadding.x * 2); + + ImGui::PushStyleVar(ImGuiStyleVar_FrameBorderSize, 0.0f); + ImGui::PushStyleVar(ImGuiStyleVar_FramePadding, ImVec2(4, 2)); + ImGui::PushStyleColor(ImGuiCol_Button, ImVec4(0, 0, 0, 0)); + ImGui::PushStyleColor(ImGuiCol_ButtonHovered, ImGui::GetStyleColorVec4(ImGuiCol_HeaderHovered)); + ImGui::PushStyleColor(ImGuiCol_ButtonActive, ImGui::GetStyleColorVec4(ImGuiCol_Header)); + + if (ImGui::SmallButton("Info")) + info_gui = !info_gui; + + ImGui::PopStyleVar(2); + ImGui::PopStyleColor(3); + + ImGui::EndMainMenuBar(); + } + + if (is_settings_gui) + settings_gui(); + + // my_display_code(); + + if (is_pca_gui) + pca_gui(); + + if (is_pose_graph_slam) + pose_graph_slam_gui(); + + if (is_manual_analisys) + observation_picking_gui(); + + if (is_loop_closure_gui) + loop_closure_gui(); + + if (is_lio_segments_gui) + lio_segments_gui(); + + if (is_translate_gui) + { + translate_gui(); + } + else if (translate_tool.step != TranslateTool::Step::Idle) + { + translate_tool.step = TranslateTool::Step::Idle; + translate_tool.has_transform = false; + translate_tool.transform = Eigen::Affine3d::Identity(); + glutSetCursor(GLUT_CURSOR_INHERIT); + } + + cor_window(); + + info_window(infoLines, appShortcuts); + + draw_translate_preview(); + + if (compass_ruler) + drawMiniCompassWithRuler(); + + ImGui::Render(); + ImGui_ImplOpenGL2_RenderDrawData(ImGui::GetDrawData()); + + glutSwapBuffers(); + glutPostRedisplay(); +} + +void draw_translate_preview() +{ + if (translate_tool.step != TranslateTool::Step::PickXAxis && translate_tool.step != TranslateTool::Step::PickYHint) + return; + + ImGuiIO& io = ImGui::GetIO(); + if (io.WantCaptureMouse) + return; + + const auto laser_beam = GetLaserBeam((int)io.MousePos.x, (int)io.MousePos.y); + RegistrationPlaneFeature::Plane pl; + pl.a = 0; + pl.b = 0; + pl.c = 1; + pl.d = -translate_tool.plane_z; + Eigen::Vector3d cursor_world = rayIntersection(laser_beam, pl); + + const Eigen::Vector3d O = translate_tool.origin; + + Eigen::Vector3d x_dir; + if (translate_tool.step == TranslateTool::Step::PickXAxis) + x_dir = cursor_world - O; + else + x_dir = translate_tool.x_point - O; + x_dir.z() = 0.0; + if (x_dir.norm() < 1e-9) + return; + double x_len = x_dir.norm(); + Eigen::Vector3d x_n = x_dir / x_len; + Eigen::Vector3d y_n(-x_n.y(), x_n.x(), 0.0); + + if (translate_tool.step == TranslateTool::Step::PickYHint) + { + Eigen::Vector3d v = cursor_world - O; + if (v.dot(y_n) < 0.0) + y_n = -y_n; + } + + double y_len = x_len * 0.5; + double z_len = x_len * 0.25; + + glLineWidth(3.0f); + glBegin(GL_LINES); + glColor3f(1.0f, 0.0f, 0.0f); + glVertex3d(O.x(), O.y(), O.z()); + glVertex3d(O.x() + x_n.x() * x_len, O.y() + x_n.y() * x_len, O.z()); + + glColor3f(0.0f, 1.0f, 0.0f); + glVertex3d(O.x(), O.y(), O.z()); + glVertex3d(O.x() + y_n.x() * y_len, O.y() + y_n.y() * y_len, O.z()); + + glColor3f(0.0f, 0.0f, 1.0f); + glVertex3d(O.x(), O.y(), O.z()); + glVertex3d(O.x(), O.y(), O.z() + z_len); + glEnd(); + glLineWidth(1.0f); +} + +Eigen::Affine3d compute_translate_matrix(const Eigen::Vector3d& O, const Eigen::Vector3d& X, const Eigen::Vector3d& Y_hint) +{ + Eigen::Vector3d dx = X - O; + dx.z() = 0.0; + if (dx.norm() < 1e-9) + return Eigen::Affine3d::Identity(); + dx.normalize(); + + double theta = -std::atan2(dx.y(), dx.x()); + + Eigen::Affine3d T = Eigen::Affine3d::Identity(); + T.prerotate(Eigen::AngleAxisd(theta, Eigen::Vector3d::UnitZ())); + T.pretranslate(-(T.linear() * O)); + + Eigen::Vector3d y_h_new = T * Y_hint; + if (y_h_new.y() < 0.0) + { + Eigen::Affine3d flip = Eigen::Affine3d::Identity(); + Eigen::Matrix3d R; + R << 1, 0, 0, 0, -1, 0, 0, 0, -1; + flip.linear() = R; + T = flip * T; + } + return T; +} + +void translate_gui() +{ + ImGui::Begin("Translate", &is_translate_gui); + + ImGui::PushItemWidth(ImGuiNumberWidth); + ImGui::InputFloat("Plane Z [m]", &translate_tool.plane_z); + ImGui::PopItemWidth(); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Height of the horizontal pick plane used to project mouse clicks into 3D"); + + if (ImGui::Button("Start picking")) + { + translate_tool.step = TranslateTool::Step::PickOrigin; + translate_tool.has_transform = false; + translate_tool.transform = Eigen::Affine3d::Identity(); + + is_ortho = true; + new_rotation_center = rotation_center; + new_rotate_x = 0.0; + new_rotate_y = 0.0; + new_translate_x = translate_x; + new_translate_y = translate_y; + new_translate_z = translate_z; + camera_transition_active = true; + + glutSetCursor(GLUT_CURSOR_CROSSHAIR); + } + ImGui::SameLine(); + if (ImGui::Button("Reset")) + { + translate_tool.step = TranslateTool::Step::Idle; + translate_tool.has_transform = false; + translate_tool.transform = Eigen::Affine3d::Identity(); + glutSetCursor(GLUT_CURSOR_INHERIT); + } + + const char* step_text = "Idle"; + switch (translate_tool.step) + { + case TranslateTool::Step::PickOrigin: + step_text = "Pick origin (new 0,0)"; + break; + case TranslateTool::Step::PickXAxis: + step_text = "Pick point on +X axis"; + break; + case TranslateTool::Step::PickYHint: + step_text = "Pick point on +Y side"; + break; + case TranslateTool::Step::Ready: + step_text = "Ready - press Translate"; + break; + default: + break; + } + ImGui::Text("Step: %s", step_text); + + ImGui::Text("Origin : %.3f %.3f %.3f", translate_tool.origin.x(), translate_tool.origin.y(), translate_tool.origin.z()); + ImGui::Text("X point: %.3f %.3f %.3f", translate_tool.x_point.x(), translate_tool.x_point.y(), translate_tool.x_point.z()); + ImGui::Text("Y hint : %.3f %.3f %.3f", translate_tool.y_hint.x(), translate_tool.y_hint.y(), translate_tool.y_hint.z()); + + if (translate_tool.has_transform) + { + ImGui::Separator(); + ImGui::Text("Transformation matrix:"); + const Eigen::Matrix4d M = translate_tool.transform.matrix(); + for (int r = 0; r < 4; r++) + ImGui::Text("%8.4f %8.4f %8.4f %8.4f", M(r, 0), M(r, 1), M(r, 2), M(r, 3)); + } + + ImGui::Separator(); + ImGui::BeginDisabled(!translate_tool.has_transform); + { + if (ImGui::Button("Translate")) + { + for (auto& pc : session.point_clouds_container.point_clouds) + pc.m_pose = translate_tool.transform * pc.m_pose; + + for (auto& cp : session.control_points.cps) + { + Eigen::Vector3d g(cp.x_target_global, cp.y_target_global, cp.z_target_global); + g = translate_tool.transform * g; + cp.x_target_global = g.x(); + cp.y_target_global = g.y(); + cp.z_target_global = g.z(); + } + + translate_tool.step = TranslateTool::Step::Idle; + translate_tool.has_transform = false; + translate_tool.transform = Eigen::Affine3d::Identity(); + glutSetCursor(GLUT_CURSOR_INHERIT); + } + } + ImGui::EndDisabled(); + + ImGui::End(); +} + +Eigen::Vector3d GLWidgetGetOGLPos(int x, int y, const ObservationPicking& observation_picking) +{ + const auto laser_beam = GetLaserBeam(x, y); + + RegistrationPlaneFeature::Plane pl; + + pl.a = 0; + pl.b = 0; + pl.c = 1; + pl.d = -observation_picking.picking_plane_height; + + Eigen::Vector3d pos = rayIntersection(laser_beam, pl); + + spdlog::info("intersection: {}, {}, {}", pos.x(), pos.y(), pos.z()); + + return pos; +} + +void mouse(int glut_button, int state, int x, int y) +{ + ImGuiIO& io = ImGui::GetIO(); + io.MousePos = ImVec2((float)x, (float)y); + + int button = -1; + if (glut_button == GLUT_LEFT_BUTTON) + button = 0; + if (glut_button == GLUT_RIGHT_BUTTON) + button = 1; + if (glut_button == GLUT_MIDDLE_BUTTON) + button = 2; + if (button != -1 && state == GLUT_DOWN) + io.MouseDown[button] = true; + if (button != -1 && state == GLUT_UP) + io.MouseDown[button] = false; + + static int glutMajorVersion = glutGet(GLUT_VERSION) / 10000; + if (state == GLUT_DOWN && (glut_button == 3 || glut_button == 4) && glutMajorVersion < 3) + wheel(glut_button, glut_button == 3 ? 1 : -1, x, y); + + if (!io.WantCaptureMouse) + { + if (glut_button == GLUT_LEFT_BUTTON && state == GLUT_DOWN && !io.KeyCtrl && !io.KeyShift && + translate_tool.step != TranslateTool::Step::Idle && translate_tool.step != TranslateTool::Step::Ready) + { + const auto laser_beam = GetLaserBeam(x, y); + RegistrationPlaneFeature::Plane pl; + pl.a = 0; + pl.b = 0; + pl.c = 1; + pl.d = -translate_tool.plane_z; + Eigen::Vector3d p = rayIntersection(laser_beam, pl); + + switch (translate_tool.step) + { + case TranslateTool::Step::PickOrigin: + translate_tool.origin = p; + translate_tool.step = TranslateTool::Step::PickXAxis; + break; + case TranslateTool::Step::PickXAxis: + translate_tool.x_point = p; + translate_tool.step = TranslateTool::Step::PickYHint; + break; + case TranslateTool::Step::PickYHint: + translate_tool.y_hint = p; + translate_tool.transform = compute_translate_matrix(translate_tool.origin, translate_tool.x_point, translate_tool.y_hint); + translate_tool.has_transform = true; + translate_tool.step = TranslateTool::Step::Ready; + break; + default: + break; + } + + mouse_old_x = x; + mouse_old_y = y; + return; + } + + if ((glut_button == GLUT_MIDDLE_BUTTON || glut_button == GLUT_LEFT_BUTTON) && state == GLUT_DOWN && (io.KeyCtrl || io.KeyShift)) + { + if (session.ground_control_points.is_imgui) + { + spdlog::info("GCP picking"); + int tmp; + getClosestTrajectoryPoint(session, x, y, true, tmp); + } + else if (session.control_points.is_imgui) + { + spdlog::info("Control point picking"); + const auto laser_beam = GetLaserBeam(x, y); + double min_distance = std::numeric_limits::max(); + + session.control_points.index_picked_point = -1; + + int i = session.control_points.index_pose; + if (session.control_points.index_pose >= 0 && + session.control_points.index_pose < session.point_clouds_container.point_clouds.size()) + { + for (size_t j = 0; j < session.point_clouds_container.point_clouds[i].points_local.size(); j++) + { + const auto& p = session.point_clouds_container.point_clouds[i].points_local[j]; + Eigen::Vector3d vp = session.point_clouds_container.point_clouds[i].m_pose * p; + + double dist = distance_point_to_line(vp, laser_beam); + + if (dist < min_distance && dist < 0.1) + { + min_distance = dist; + + new_rotation_center.x() = vp.x(); + new_rotation_center.y() = vp.y(); + new_rotation_center.z() = vp.z(); + + session.control_points.index_picked_point = j; + } + } + + new_rotate_x = rotate_x; + new_rotate_y = rotate_y; + new_translate_x = -new_rotation_center.x(); + new_translate_y = -new_rotation_center.y(); + new_translate_z = translate_z; + camera_transition_active = true; + } + } + else + { + if (glut_button == GLUT_MIDDLE_BUTTON) + if (session_loaded) + { + int tmp = -1; + getClosestTrajectoryPoint(session, x, y, false, tmp); + + if (io.KeyCtrl) + { + if (tmp != -1) + index_loop_closure_target = tmp; + } + else if (io.KeyShift) + { + if (tmp != -1) + index_loop_closure_source = tmp; + } + } + else + setNewRotationCenter(x, y); + } + } + + if (glut_button == GLUT_RIGHT_BUTTON && state == GLUT_DOWN && io.KeyCtrl) + { + int tmp; + if (session_loaded) + getClosestTrajectoryPoint(session, x, y, false, tmp); + else + setNewRotationCenter(x, y); + } + + if (state == GLUT_DOWN) + { + mouse_buttons |= 1 << glut_button; + + if (observation_picking.is_observation_picking_mode) + { + Eigen::Vector3d p = GLWidgetGetOGLPos(x, y, observation_picking); + int number_active_pcs = 0; + int index_picked = -1; + for (size_t i = 0; i < session.point_clouds_container.point_clouds.size(); i++) + { + if (session.point_clouds_container.point_clouds[i].visible) + { + number_active_pcs++; + index_picked = i; + } + } + if (number_active_pcs == 1) + observation_picking.add_picked_to_current_observation(index_picked, p); + } + } + else if (state == GLUT_UP) + mouse_buttons = 0; + + mouse_old_x = x; + mouse_old_y = y; + } +} + +int main(int argc, char* argv[]) +{ + try + { + if (checkClHelp(argc, argv)) + { + std::cout << winTitle << "\n\n" + << "USAGE:\n" + << std::filesystem::path(argv[0]).stem().string() << " /?\n\n" + << "where\n" + << " Path to Mandeye JSON Session file (*.mjs)\n" + << " -h, /h, --help, /? Show this help and exit\n\n"; + + return 0; + } + + // search for available geoid models in the system and populate the menu + geoids = GNSS::get_available_geoids(); + + initGL(&argc, argv, winTitle, display, mouse); + + if (argc > 1) + { + for (int i = 1; i < argc; i++) + { + std::string ext = fs::path(argv[i]).extension().string(); + std::transform(ext.begin(), ext.end(), ext.begin(), ::tolower); + + if (ext == ".mjs" || ext == ".json") + { + loadSession(argv[i]); + + break; + } + } + } + + glutMainLoop(); + + ImGui_ImplOpenGL2_Shutdown(); + ImGui_ImplGLUT_Shutdown(); + ImGui::DestroyContext(); + } catch (const std::bad_alloc& e) + { + spdlog::error("System is out of memory : {}", e.what()); + mandeye::fd::OutOfMemMessage(); + } catch (const std::exception& e) + { + spdlog::error(e.what()); + } catch (...) + { + spdlog::error("Unknown fatal error occurred!"); + } + + return 0; +} \ No newline at end of file diff --git a/apps/multi_view_tls_registration_legacy/perform_experiment.cpp b/apps/multi_view_tls_registration_legacy/perform_experiment.cpp new file mode 100644 index 00000000..3d09d804 --- /dev/null +++ b/apps/multi_view_tls_registration_legacy/perform_experiment.cpp @@ -0,0 +1,2428 @@ +#include +#include +#include +#include + +// clang-format off +#include +#include +// clang-format on + +#include +#include +#include +#include + +#include + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include + +namespace fs = std::filesystem; + +void export_result_to_folder(std::string output_folder_name, ObservationPicking& observation_picking, Session& session) +{ + fs::path path(output_folder_name); + std::string file_name_rms = "rms.csv"; + auto path_rms = path; + path_rms /= file_name_rms; + std::cout << "exporting to file: '" << path_rms.string() << "'" << std::endl; + std::ofstream outfile_rms; + outfile_rms.open(path_rms, std::ios_base::app); + outfile_rms << "index_roi, rms_initial, rms_result" << std::endl; + + for (int i = 0; i < observation_picking.intersections.size(); i++) + { + std::string file_name_initial = "intersection_" + std::to_string(i) + "_initial.csv"; + std::string file_name_result = "intersection_" + std::to_string(i) + "_result.csv"; + + auto path_initial = path; + auto path_result = path; + + path_initial /= file_name_initial; + path_result /= file_name_result; + + std::cout << "exporting to file: '" << path_initial.string() << "'" << std::endl; + std::cout << "exporting to file: '" << path_result.string() << "'" << std::endl; + + std::ofstream outfile_initial; + std::ofstream outfile_result; + + outfile_initial.open(path_initial, std::ios_base::app); + outfile_result.open(path_result, std::ios_base::app); + + const auto& intersection = observation_picking.intersections[i]; + TaitBryanPose pose; + pose.px = intersection.translation[0]; + pose.py = intersection.translation[1]; + pose.pz = intersection.translation[2]; + pose.om = intersection.rotation[0]; + pose.fi = intersection.rotation[1]; + pose.ka = intersection.rotation[2]; + Eigen::Affine3d m_pose_inv = affine_matrix_from_pose_tait_bryan(pose).inverse(); + + double w = intersection.width_length_height[0] * 0.5; + double l = intersection.width_length_height[1] * 0.5; + double h = intersection.width_length_height[2] * 0.5; + + outfile_initial << "x;y;z;pc_index;is_initial;index_intersection;file" << std::endl; + outfile_result << "x;y;z;pc_index;is_initial;index_intersection;file" << std::endl; + + for (int pc_index = 0; pc_index < session.point_clouds_container.point_clouds.size(); pc_index++) + { + const auto& pc = session.point_clouds_container.point_clouds[pc_index]; + for (const auto& p : pc.points_local) + { + Eigen::Vector3d vpi = pc.m_initial_pose * p; + Eigen::Vector3d vpr = pc.m_pose * p; + + Eigen::Vector3d vpit = m_pose_inv * vpi; + Eigen::Vector3d vprt = m_pose_inv * vpr; + + if (fabs(vpit.x()) < w) + { + if (fabs(vpit.y()) < l) + { + if (fabs(vpit.z()) < h) + { + outfile_initial << vpit.x() << ";" << vpit.y() << ";" << vpit.z() << ";" << pc_index << ";1;" << i << ";" + << pc.file_name << std::endl; + } + } + } + if (fabs(vprt.x()) < w) + { + if (fabs(vprt.y()) < l) + { + if (fabs(vprt.z()) < h) + { + outfile_result << vprt.x() << ";" << vprt.y() << ";" << vprt.z() << ";" << pc_index << ";0;" << i << ";" + << pc.file_name << std::endl; + } + } + } + } + } + outfile_initial.close(); + outfile_result.close(); + + const auto& obs = observation_picking.observations[i]; + double rms_initial = 0.0; + int sum = 0; + double rms_result = 0.0; + + for (const auto& [key1, value1] : obs) + { + for (const auto& [key2, value2] : obs) + { + if (key1 != key2) + { + Eigen::Vector3d p1, p2; + p1 = session.point_clouds_container.point_clouds[key1].m_initial_pose * value1; + p2 = session.point_clouds_container.point_clouds[key2].m_initial_pose * value2; + rms_initial += (p2.x() - p1.x()) * (p2.x() - p1.x()); + rms_initial += (p2.y() - p1.y()) * (p2.y() - p1.y()); + + p1 = session.point_clouds_container.point_clouds[key1].m_pose * value1; + p2 = session.point_clouds_container.point_clouds[key2].m_pose * value2; + rms_result += (p2.x() - p1.x()) * (p2.x() - p1.x()); + rms_result += (p2.y() - p1.y()) * (p2.y() - p1.y()); + + sum += 2; + } + } + } + std::cout << "sum: " << sum << std::endl; + if (sum > 0) + { + rms_initial = sqrt(rms_initial / sum); + rms_result = sqrt(rms_result / sum); + outfile_rms << i << ";" << rms_initial << ";" << rms_result << std::endl; + } + } + outfile_rms.close(); + + std::string file_name_poses = "poses_RESSO.reg"; + auto path_poses = path; + path_poses /= file_name_poses; + std::cout << "saving poses to: " << path_poses << std::endl; + session.point_clouds_container.save_poses(path_poses.string(), false); +} + +void export_result_to_folder(std::string output_folder_name, int method_id, ObservationPicking& observation_picking, Session& session) +{ + fs::path path(output_folder_name); + path /= std::to_string(method_id); + create_directory(path); + export_result_to_folder(path.string(), observation_picking, session); +} + +template +void append_to_result_file( + std::string file_name, std::string method, const T& t, float rms, int id_method, std::chrono::milliseconds elapsed) +{ + std::ofstream outfile; + outfile.open(file_name, std::ios_base::app); + outfile << method << ";" << id_method << ";" << int(t.is_gauss_newton) << ";" << int(t.is_levenberg_marguardt) << ";" << int(t.is_wc) + << ";" << int(t.is_cw) << ";" << int(t.is_tait_bryan_angles) << ";" << int(t.is_quaternion) << ";" << int(t.is_rodrigues) << ";" + << int(t.is_lie_algebra_left_jacobian) << ";" << int(t.is_lie_algebra_right_jacobian) << ";" << rms << ";" << elapsed.count() + << std::endl; + outfile.close(); +} + +void create_header(std::string file_name) +{ + std::ofstream outfile; + outfile.open(file_name); + outfile << "method;id_method;gauss_newton;levenberg_marguardt;wc;cw;tait_bryan_angles;quaternion;rodrigues;Lie_algebra_left_jacobian;" + "Lie_algebra_right_jacobian;rms;elapsed_time_miliseconds" + << std::endl; + outfile.close(); +} + +void add_initial_rms_to_file(std::string file_name, float rms) +{ + std::ofstream outfile; + outfile.open(file_name, std::ios_base::app); + // outfile << + // "method;is_gauss_newton;is_levenberg_marguardt;is_wc;is_cw;is_tait_bryan_angles;is_quaternion;is_rodrigues;is_Lie_algebra_left;Lie_algebra_right;rms;elapsed_time_miliseconds" + // << std::endl; + outfile << "initial_rms;0;0;0;0;0;0;0;0;0;0;" << rms << ";0" << std::endl; + outfile.close(); +} + +void reset_poses(Session& session) +{ + for (size_t i = 0; i < session.point_clouds_container.point_clouds.size(); i++) + { + session.point_clouds_container.point_clouds[i].m_pose = session.point_clouds_container.point_clouds[i].m_initial_pose; + 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)rad2deg(session.point_clouds_container.point_clouds[i].pose.om); + session.point_clouds_container.point_clouds[i].gui_rotation[1] = + (float)rad2deg(session.point_clouds_container.point_clouds[i].pose.fi); + session.point_clouds_container.point_clouds[i].gui_rotation[2] = + (float)rad2deg(session.point_clouds_container.point_clouds[i].pose.ka); + } +} + +double compute_rms(bool initial, Session& session, ObservationPicking& observation_picking) +{ + double rms = 0.0; + int sum = 0; + 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 (initial) + { + 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; + } + rms += (p2.x() - p1.x()) * (p2.x() - p1.x()); + sum++; + rms += (p2.y() - p1.y()) * (p2.y() - p1.y()); + sum++; + } + } + } + } + if (sum == 0) + { + std::cout << "sum == 0" << std::endl; + return -1; + } + else + { + rms = sqrt(rms / sum); + return rms; + } +} + +void perform_experiment_on_windows( + Session& session, + ObservationPicking& observation_picking, + ICP& icp, + NDT& ndt, + RegistrationPlaneFeature& registration_plane_feature, + PoseGraphSLAM& pose_graph_slam) +{ + bool compute_mean_and_cov_for_bucket = false; + session.point_clouds_container.show_with_initial_pose = false; + auto temp_data = session.point_clouds_container; + reset_poses(session); + double rms = 0.0f; + std::string result_file = session.working_directory + "/result_win.csv"; + float search_radius = 0.1f; + int number_of_threads = 16; + int number_of_iterations = 6; + int id_method = 0; + + create_header(result_file); + double initial_rms = compute_rms(false, session, observation_picking); + std::cout << "initial rms: " << initial_rms << std::endl; + add_initial_rms_to_file(result_file, initial_rms); + + // void export_result_to_folder(std::string output_folder_name, int method_id) { + // fs::path path(output_folder_name); + // path /= std::to_string(method_id); + // create_directory(path); + // export_result_to_folder(path.string()); + // } + + fs::path path_result(session.working_directory); + path_result /= "results_win"; + create_directory(path_result); + + //--0-- + icp.is_adaptive_robust_kernel = false; + icp.is_fix_first_node = false; + icp.search_radius = search_radius; + icp.number_of_threads = number_of_threads; + icp.number_of_iterations = number_of_iterations; + icp.is_adaptive_robust_kernel = false; + + icp.is_gauss_newton = true; + icp.is_levenberg_marguardt = false; + + icp.is_wc = true; + icp.is_cw = false; + + icp.is_tait_bryan_angles = true; + icp.is_quaternion = false; + icp.is_rodrigues = false; + icp.is_lie_algebra_left_jacobian = false; + icp.is_lie_algebra_right_jacobian = false; + + auto start = std::chrono::system_clock::now(); + icp.optimization_point_to_point_source_to_target(session.point_clouds_container); + auto end = std::chrono::system_clock::now(); + auto elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + std::cout << "final RMS: " << rms << std::endl; + + append_to_result_file(result_file, "point_to_point", icp, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + id_method++; + + //--1-- + icp.is_tait_bryan_angles = false; + icp.is_quaternion = true; + icp.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + icp.optimization_point_to_point_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 1; + + append_to_result_file(result_file, "point_to_point", icp, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + // id_method++; + //--2-- + icp.is_tait_bryan_angles = false; + icp.is_quaternion = false; + icp.is_rodrigues = true; + + start = std::chrono::system_clock::now(); + icp.optimization_point_to_point_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 2; + append_to_result_file(result_file, "point_to_point", icp, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--3-- + icp.is_wc = false; + icp.is_cw = true; + + icp.is_tait_bryan_angles = true; + icp.is_quaternion = false; + icp.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + icp.optimization_point_to_point_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 3; + append_to_result_file(result_file, "point_to_point", icp, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--4-- + icp.is_tait_bryan_angles = false; + icp.is_quaternion = true; + icp.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + icp.optimization_point_to_point_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 4; + append_to_result_file(result_file, "point_to_point", icp, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--5-- + icp.is_tait_bryan_angles = false; + icp.is_quaternion = false; + icp.is_rodrigues = true; + + start = std::chrono::system_clock::now(); + icp.optimization_point_to_point_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 5; + append_to_result_file(result_file, "point_to_point", icp, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--6-- + icp.is_gauss_newton = false; + icp.is_levenberg_marguardt = true; + + icp.is_wc = true; + icp.is_cw = false; + + icp.is_tait_bryan_angles = true; + icp.is_quaternion = false; + icp.is_rodrigues = false; + icp.is_lie_algebra_left_jacobian = false; + icp.is_lie_algebra_right_jacobian = false; + + start = std::chrono::system_clock::now(); + icp.optimization_point_to_point_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 6; + append_to_result_file(result_file, "point_to_point", icp, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--7-- + icp.is_tait_bryan_angles = false; + icp.is_quaternion = true; + icp.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + icp.optimization_point_to_point_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 7; + append_to_result_file(result_file, "point_to_point", icp, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--8-- + icp.is_tait_bryan_angles = false; + icp.is_quaternion = false; + icp.is_rodrigues = true; + + start = std::chrono::system_clock::now(); + icp.optimization_point_to_point_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 8; + append_to_result_file(result_file, "point_to_point", icp, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--9-- + icp.is_wc = false; + icp.is_cw = true; + + icp.is_tait_bryan_angles = true; + icp.is_quaternion = false; + icp.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + icp.optimization_point_to_point_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 9; + append_to_result_file(result_file, "point_to_point", icp, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--10-- + icp.is_wc = false; + icp.is_cw = true; + + icp.is_tait_bryan_angles = false; + icp.is_quaternion = true; + icp.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + icp.optimization_point_to_point_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 10; + append_to_result_file(result_file, "point_to_point", icp, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--11-- + icp.is_wc = false; + icp.is_cw = true; + + icp.is_tait_bryan_angles = false; + icp.is_quaternion = false; + icp.is_rodrigues = true; + + start = std::chrono::system_clock::now(); + icp.optimization_point_to_point_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 11; + append_to_result_file(result_file, "point_to_point", icp, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--12-- + icp.is_gauss_newton = true; + icp.is_levenberg_marguardt = false; + + icp.is_wc = true; + icp.is_cw = false; + + icp.is_tait_bryan_angles = false; + icp.is_quaternion = false; + icp.is_rodrigues = true; + + icp.is_lie_algebra_left_jacobian = true; + icp.is_lie_algebra_right_jacobian = false; + + start = std::chrono::system_clock::now(); + icp.optimize_source_to_target_lie_algebra_left_jacobian(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 12; + append_to_result_file(result_file, "point_to_point", icp, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--13-- + icp.is_lie_algebra_left_jacobian = false; + icp.is_lie_algebra_right_jacobian = true; + start = std::chrono::system_clock::now(); + icp.optimize_source_to_target_lie_algebra_right_jacobian(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 13; + append_to_result_file(result_file, "point_to_point", icp, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //---NDT--- + //--14-- + // + ndt.is_fix_first_node = false; + ndt.bucket_size[0] = 0.5; + ndt.bucket_size[1] = 0.5; + ndt.bucket_size[2] = 0.5; + ndt.number_of_threads = number_of_threads; + ndt.number_of_iterations = number_of_iterations; + + ndt.is_gauss_newton = true; + ndt.is_levenberg_marguardt = false; + + ndt.is_wc = true; + ndt.is_cw = false; + + ndt.is_tait_bryan_angles = true; + ndt.is_quaternion = false; + ndt.is_rodrigues = false; + ndt.is_lie_algebra_left_jacobian = false; + ndt.is_lie_algebra_right_jacobian = false; + + start = std::chrono::system_clock::now(); + ndt.optimize(session.point_clouds_container.point_clouds, true, compute_mean_and_cov_for_bucket); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + std::cout << "final rms: " << rms << std::endl; + + id_method = 14; + append_to_result_file(result_file, "normal_distributions_transform", ndt, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--15-- + ndt.is_tait_bryan_angles = false; + ndt.is_quaternion = true; + ndt.is_rodrigues = false; + start = std::chrono::system_clock::now(); + ndt.optimize(session.point_clouds_container.point_clouds, true, compute_mean_and_cov_for_bucket); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + + id_method = 15; + append_to_result_file(result_file, "normal_distributions_transform", ndt, rms, id_method, elapsed); + session.point_clouds_container = temp_data; + + //--16-- + ndt.is_tait_bryan_angles = false; + ndt.is_quaternion = false; + ndt.is_rodrigues = true; + start = std::chrono::system_clock::now(); + ndt.optimize(session.point_clouds_container.point_clouds, true, compute_mean_and_cov_for_bucket); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + + id_method = 16; + append_to_result_file(result_file, "normal_distributions_transform", ndt, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--17-- + ndt.is_wc = false; + ndt.is_cw = true; + + ndt.is_tait_bryan_angles = true; + ndt.is_quaternion = false; + ndt.is_rodrigues = false; + start = std::chrono::system_clock::now(); + ndt.optimize(session.point_clouds_container.point_clouds, true, compute_mean_and_cov_for_bucket); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 17; + append_to_result_file(result_file, "normal_distributions_transform", ndt, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--18-- + ndt.is_tait_bryan_angles = false; + ndt.is_quaternion = true; + ndt.is_rodrigues = false; + start = std::chrono::system_clock::now(); + ndt.optimize(session.point_clouds_container.point_clouds, true, compute_mean_and_cov_for_bucket); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + + id_method = 18; + append_to_result_file(result_file, "normal_distributions_transform", ndt, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--19-- + ndt.is_tait_bryan_angles = false; + ndt.is_quaternion = false; + ndt.is_rodrigues = true; + start = std::chrono::system_clock::now(); + ndt.optimize(session.point_clouds_container.point_clouds, true, compute_mean_and_cov_for_bucket); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + + id_method = 19; + append_to_result_file(result_file, "normal_distributions_transform", ndt, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--20-- + ndt.is_gauss_newton = false; + ndt.is_levenberg_marguardt = true; + + ndt.is_wc = true; + ndt.is_cw = false; + + ndt.is_tait_bryan_angles = true; + ndt.is_quaternion = false; + ndt.is_rodrigues = false; + start = std::chrono::system_clock::now(); + ndt.optimize(session.point_clouds_container.point_clouds, true, compute_mean_and_cov_for_bucket); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + + id_method = 20; + append_to_result_file(result_file, "normal_distributions_transform", ndt, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--21-- + ndt.is_tait_bryan_angles = false; + ndt.is_quaternion = true; + ndt.is_rodrigues = false; + start = std::chrono::system_clock::now(); + ndt.optimize(session.point_clouds_container.point_clouds, true, compute_mean_and_cov_for_bucket); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + + id_method = 21; + append_to_result_file(result_file, "normal_distributions_transform", ndt, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--22-- + ndt.is_tait_bryan_angles = false; + ndt.is_quaternion = false; + ndt.is_rodrigues = true; + start = std::chrono::system_clock::now(); + ndt.optimize(session.point_clouds_container.point_clouds, true, compute_mean_and_cov_for_bucket); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + + id_method = 22; + append_to_result_file(result_file, "normal_distributions_transform", ndt, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--23-- + ndt.is_wc = false; + ndt.is_cw = true; + + ndt.is_tait_bryan_angles = true; + ndt.is_quaternion = false; + ndt.is_rodrigues = false; + start = std::chrono::system_clock::now(); + ndt.optimize(session.point_clouds_container.point_clouds, true, compute_mean_and_cov_for_bucket); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + + id_method = 23; + append_to_result_file(result_file, "normal_distributions_transform", ndt, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--24-- + ndt.is_tait_bryan_angles = false; + ndt.is_quaternion = true; + ndt.is_rodrigues = false; + start = std::chrono::system_clock::now(); + ndt.optimize(session.point_clouds_container.point_clouds, true, compute_mean_and_cov_for_bucket); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + + id_method = 24; + + append_to_result_file(result_file, "normal_distributions_transform", ndt, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--25-- + ndt.is_tait_bryan_angles = false; + ndt.is_quaternion = false; + ndt.is_rodrigues = true; + start = std::chrono::system_clock::now(); + ndt.optimize(session.point_clouds_container.point_clouds, true, compute_mean_and_cov_for_bucket); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + + id_method = 25; + + append_to_result_file(result_file, "normal_distributions_transform", ndt, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--26-- + ndt.is_gauss_newton = true; + ndt.is_levenberg_marguardt = false; + + ndt.is_wc = true; + ndt.is_cw = false; + + ndt.is_tait_bryan_angles = false; + ndt.is_quaternion = false; + ndt.is_rodrigues = false; + + ndt.is_lie_algebra_left_jacobian = true; + ndt.is_lie_algebra_right_jacobian = false; + start = std::chrono::system_clock::now(); + ndt.optimize(session.point_clouds_container.point_clouds, true, compute_mean_and_cov_for_bucket); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + + id_method = 26; + + ndt.is_rodrigues = true; + + append_to_result_file(result_file, "normal_distributions_transform", ndt, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--27-- + ndt.is_gauss_newton = false; + ndt.is_levenberg_marguardt = true; + ndt.is_rodrigues = false; + start = std::chrono::system_clock::now(); + ndt.optimize(session.point_clouds_container.point_clouds, true, compute_mean_and_cov_for_bucket); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + + id_method = 27; + ndt.is_rodrigues = true; + append_to_result_file(result_file, "normal_distributions_transform", ndt, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--28-- + ndt.is_lie_algebra_left_jacobian = false; + ndt.is_lie_algebra_right_jacobian = true; + ndt.is_rodrigues = false; + + ndt.is_gauss_newton = true; + ndt.is_levenberg_marguardt = false; + start = std::chrono::system_clock::now(); + ndt.optimize(session.point_clouds_container.point_clouds, true, compute_mean_and_cov_for_bucket); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + + id_method = 28; + ndt.is_rodrigues = true; + append_to_result_file(result_file, "normal_distributions_transform", ndt, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--29-- + ndt.is_gauss_newton = false; + ndt.is_levenberg_marguardt = true; + ndt.is_rodrigues = false; + start = std::chrono::system_clock::now(); + ndt.optimize(session.point_clouds_container.point_clouds, true, compute_mean_and_cov_for_bucket); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + + id_method = 29; + ndt.is_rodrigues = true; + append_to_result_file(result_file, "normal_distributions_transform", ndt, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //---------------------------------------------------------------------------- + + registration_plane_feature.search_radius = search_radius; + registration_plane_feature.number_of_threads = number_of_threads; + registration_plane_feature.number_of_iterations = number_of_iterations; + registration_plane_feature.is_adaptive_robust_kernel = false; + registration_plane_feature.is_fix_first_node = false; + + //--30-- + registration_plane_feature.is_gauss_newton = true; + registration_plane_feature.is_levenberg_marguardt = false; + + registration_plane_feature.is_wc = true; + registration_plane_feature.is_cw = false; + + registration_plane_feature.is_tait_bryan_angles = true; + registration_plane_feature.is_quaternion = false; + registration_plane_feature.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 30; + append_to_result_file(result_file, "point_to_projection_onto_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--31-- + registration_plane_feature.is_tait_bryan_angles = false; + registration_plane_feature.is_quaternion = true; + registration_plane_feature.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 31; + append_to_result_file(result_file, "point_to_projection_onto_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--32-- + registration_plane_feature.is_tait_bryan_angles = false; + registration_plane_feature.is_quaternion = false; + registration_plane_feature.is_rodrigues = true; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 32; + append_to_result_file(result_file, "point_to_projection_onto_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--33-- + registration_plane_feature.is_wc = false; + registration_plane_feature.is_cw = true; + + registration_plane_feature.is_tait_bryan_angles = true; + registration_plane_feature.is_quaternion = false; + registration_plane_feature.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 33; + append_to_result_file(result_file, "point_to_projection_onto_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--34-- + registration_plane_feature.is_tait_bryan_angles = false; + registration_plane_feature.is_quaternion = true; + registration_plane_feature.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 34; + append_to_result_file(result_file, "point_to_projection_onto_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--35-- + registration_plane_feature.is_tait_bryan_angles = false; + registration_plane_feature.is_quaternion = false; + registration_plane_feature.is_rodrigues = true; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 35; + append_to_result_file(result_file, "point_to_projection_onto_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //------------------------------------------------------ + //--36-- + registration_plane_feature.is_gauss_newton = false; + registration_plane_feature.is_levenberg_marguardt = true; + + registration_plane_feature.is_wc = true; + registration_plane_feature.is_cw = false; + + registration_plane_feature.is_tait_bryan_angles = true; + registration_plane_feature.is_quaternion = false; + registration_plane_feature.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 36; + append_to_result_file(result_file, "point_to_projection_onto_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--37-- + registration_plane_feature.is_tait_bryan_angles = false; + registration_plane_feature.is_quaternion = true; + registration_plane_feature.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 37; + append_to_result_file(result_file, "point_to_projection_onto_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--38-- + registration_plane_feature.is_tait_bryan_angles = false; + registration_plane_feature.is_quaternion = false; + registration_plane_feature.is_rodrigues = true; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 38; + append_to_result_file(result_file, "point_to_projection_onto_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--39-- + registration_plane_feature.is_wc = false; + registration_plane_feature.is_cw = true; + + registration_plane_feature.is_tait_bryan_angles = true; + registration_plane_feature.is_quaternion = false; + registration_plane_feature.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 39; + append_to_result_file(result_file, "point_to_projection_onto_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--40-- + registration_plane_feature.is_tait_bryan_angles = false; + registration_plane_feature.is_quaternion = true; + registration_plane_feature.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 40; + append_to_result_file(result_file, "point_to_projection_onto_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--41-- + registration_plane_feature.is_tait_bryan_angles = false; + registration_plane_feature.is_quaternion = false; + registration_plane_feature.is_rodrigues = true; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 41; + append_to_result_file(result_file, "point_to_projection_onto_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--42-- Lie + registration_plane_feature.is_gauss_newton = true; + registration_plane_feature.is_levenberg_marguardt = false; + + registration_plane_feature.is_wc = true; + registration_plane_feature.is_cw = false; + + registration_plane_feature.is_tait_bryan_angles = false; + registration_plane_feature.is_quaternion = false; + registration_plane_feature.is_rodrigues = true; + + registration_plane_feature.is_lie_algebra_left_jacobian = true; + registration_plane_feature.is_lie_algebra_right_jacobian = false; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_left_jacobian( + session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 42; + append_to_result_file(result_file, "point_to_projection_onto_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--43-- + registration_plane_feature.is_gauss_newton = false; + registration_plane_feature.is_levenberg_marguardt = true; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_left_jacobian( + session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 43; + append_to_result_file(result_file, "point_to_projection_onto_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--44-- + registration_plane_feature.is_gauss_newton = true; + registration_plane_feature.is_levenberg_marguardt = false; + registration_plane_feature.is_lie_algebra_left_jacobian = false; + registration_plane_feature.is_lie_algebra_right_jacobian = true; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_right_jacobian( + session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 44; + append_to_result_file(result_file, "point_to_projection_onto_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--45-- + registration_plane_feature.is_gauss_newton = false; + registration_plane_feature.is_levenberg_marguardt = true; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_right_jacobian( + session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 45; + append_to_result_file(result_file, "point_to_projection_onto_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--46--using dot product + registration_plane_feature.is_lie_algebra_left_jacobian = false; + registration_plane_feature.is_lie_algebra_right_jacobian = false; + + registration_plane_feature.is_gauss_newton = true; + registration_plane_feature.is_levenberg_marguardt = false; + + registration_plane_feature.is_wc = true; + registration_plane_feature.is_cw = false; + + registration_plane_feature.is_tait_bryan_angles = true; + registration_plane_feature.is_quaternion = false; + registration_plane_feature.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_point_to_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 46; + append_to_result_file(result_file, "point_to_plane_using_dot_product", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--47 + registration_plane_feature.is_tait_bryan_angles = false; + registration_plane_feature.is_quaternion = true; + registration_plane_feature.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_point_to_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 47; + append_to_result_file(result_file, "point_to_plane_using_dot_product", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--48 + registration_plane_feature.is_tait_bryan_angles = false; + registration_plane_feature.is_quaternion = false; + registration_plane_feature.is_rodrigues = true; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_point_to_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 48; + append_to_result_file(result_file, "point_to_plane_using_dot_product", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--49 + registration_plane_feature.is_wc = false; + registration_plane_feature.is_cw = true; + + registration_plane_feature.is_tait_bryan_angles = true; + registration_plane_feature.is_quaternion = false; + registration_plane_feature.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_point_to_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 49; + append_to_result_file(result_file, "point_to_plane_using_dot_product", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--50 + registration_plane_feature.is_tait_bryan_angles = false; + registration_plane_feature.is_quaternion = true; + registration_plane_feature.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_point_to_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 50; + append_to_result_file(result_file, "point_to_plane_using_dot_product", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--51 + registration_plane_feature.is_tait_bryan_angles = false; + registration_plane_feature.is_quaternion = false; + registration_plane_feature.is_rodrigues = true; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_point_to_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 51; + append_to_result_file(result_file, "point_to_plane_using_dot_product", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--52 + registration_plane_feature.is_gauss_newton = false; + registration_plane_feature.is_levenberg_marguardt = true; + + registration_plane_feature.is_wc = true; + registration_plane_feature.is_cw = false; + + registration_plane_feature.is_tait_bryan_angles = true; + registration_plane_feature.is_quaternion = false; + registration_plane_feature.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_point_to_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 52; + append_to_result_file(result_file, "point_to_plane_using_dot_product", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--53 + registration_plane_feature.is_tait_bryan_angles = false; + registration_plane_feature.is_quaternion = true; + registration_plane_feature.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_point_to_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 53; + append_to_result_file(result_file, "point_to_plane_using_dot_product", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--54 + registration_plane_feature.is_tait_bryan_angles = false; + registration_plane_feature.is_quaternion = false; + registration_plane_feature.is_rodrigues = true; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_point_to_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 54; + append_to_result_file(result_file, "point_to_plane_using_dot_product", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--55 + registration_plane_feature.is_wc = false; + registration_plane_feature.is_cw = true; + + registration_plane_feature.is_tait_bryan_angles = true; + registration_plane_feature.is_quaternion = false; + registration_plane_feature.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_point_to_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 55; + append_to_result_file(result_file, "point_to_plane_using_dot_product", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--56 + registration_plane_feature.is_tait_bryan_angles = false; + registration_plane_feature.is_quaternion = true; + registration_plane_feature.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_point_to_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 56; + append_to_result_file(result_file, "point_to_plane_using_dot_product", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--57 + registration_plane_feature.is_tait_bryan_angles = false; + registration_plane_feature.is_quaternion = false; + registration_plane_feature.is_rodrigues = true; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_point_to_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 57; + append_to_result_file(result_file, "point_to_plane_using_dot_product", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--58 optimize_distance_point_to_plane_source_to_target + registration_plane_feature.is_gauss_newton = true; + registration_plane_feature.is_levenberg_marguardt = false; + + registration_plane_feature.is_wc = true; + registration_plane_feature.is_cw = false; + + registration_plane_feature.is_tait_bryan_angles = true; + registration_plane_feature.is_quaternion = false; + registration_plane_feature.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_distance_point_to_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 58; + append_to_result_file(result_file, "distance_point_to_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--59 + registration_plane_feature.is_tait_bryan_angles = false; + registration_plane_feature.is_quaternion = true; + registration_plane_feature.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_distance_point_to_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 59; + append_to_result_file(result_file, "distance_point_to_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--60 + registration_plane_feature.is_tait_bryan_angles = false; + registration_plane_feature.is_quaternion = false; + registration_plane_feature.is_rodrigues = true; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_distance_point_to_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 60; + append_to_result_file(result_file, "distance_point_to_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--61 + registration_plane_feature.is_wc = false; + registration_plane_feature.is_cw = true; + + registration_plane_feature.is_tait_bryan_angles = true; + registration_plane_feature.is_quaternion = false; + registration_plane_feature.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_distance_point_to_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 61; + append_to_result_file(result_file, "distance_point_to_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--62 + registration_plane_feature.is_tait_bryan_angles = false; + registration_plane_feature.is_quaternion = true; + registration_plane_feature.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_distance_point_to_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 62; + append_to_result_file(result_file, "distance_point_to_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--63 + registration_plane_feature.is_tait_bryan_angles = false; + registration_plane_feature.is_quaternion = false; + registration_plane_feature.is_rodrigues = true; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_distance_point_to_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 63; + append_to_result_file(result_file, "distance_point_to_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--64 + registration_plane_feature.is_gauss_newton = false; + registration_plane_feature.is_levenberg_marguardt = true; + + registration_plane_feature.is_wc = true; + registration_plane_feature.is_cw = false; + + registration_plane_feature.is_tait_bryan_angles = true; + registration_plane_feature.is_quaternion = false; + registration_plane_feature.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_distance_point_to_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 64; + append_to_result_file(result_file, "distance_point_to_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--65 + registration_plane_feature.is_tait_bryan_angles = false; + registration_plane_feature.is_quaternion = true; + registration_plane_feature.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_distance_point_to_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 65; + append_to_result_file(result_file, "distance_point_to_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--66 + registration_plane_feature.is_tait_bryan_angles = false; + registration_plane_feature.is_quaternion = false; + registration_plane_feature.is_rodrigues = true; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_distance_point_to_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 66; + append_to_result_file(result_file, "distance_point_to_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--67 + registration_plane_feature.is_wc = false; + registration_plane_feature.is_cw = true; + + registration_plane_feature.is_tait_bryan_angles = true; + registration_plane_feature.is_quaternion = false; + registration_plane_feature.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_distance_point_to_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 67; + append_to_result_file(result_file, "distance_point_to_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--68 + registration_plane_feature.is_tait_bryan_angles = false; + registration_plane_feature.is_quaternion = true; + registration_plane_feature.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_distance_point_to_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 68; + append_to_result_file(result_file, "distance_point_to_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--69 + registration_plane_feature.is_tait_bryan_angles = false; + registration_plane_feature.is_quaternion = false; + registration_plane_feature.is_rodrigues = true; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_distance_point_to_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 69; + append_to_result_file(result_file, "distance_point_to_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--70 optimize_plane_to_plane_source_to_target + registration_plane_feature.is_adaptive_robust_kernel = true; + + registration_plane_feature.is_gauss_newton = true; + registration_plane_feature.is_levenberg_marguardt = false; + + registration_plane_feature.is_wc = true; + registration_plane_feature.is_cw = false; + + registration_plane_feature.is_tait_bryan_angles = true; + registration_plane_feature.is_quaternion = false; + registration_plane_feature.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_plane_to_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 70; + append_to_result_file(result_file, "plane_to_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--71 + registration_plane_feature.is_tait_bryan_angles = false; + registration_plane_feature.is_quaternion = true; + registration_plane_feature.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_plane_to_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 71; + append_to_result_file(result_file, "plane_to_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--72 + registration_plane_feature.is_tait_bryan_angles = false; + registration_plane_feature.is_quaternion = false; + registration_plane_feature.is_rodrigues = true; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_plane_to_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 72; + append_to_result_file(result_file, "plane_to_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--73 + registration_plane_feature.is_wc = false; + registration_plane_feature.is_cw = true; + + registration_plane_feature.is_tait_bryan_angles = true; + registration_plane_feature.is_quaternion = false; + registration_plane_feature.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_plane_to_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 73; + append_to_result_file(result_file, "plane_to_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--74 + registration_plane_feature.is_tait_bryan_angles = false; + registration_plane_feature.is_quaternion = true; + registration_plane_feature.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_plane_to_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 74; + append_to_result_file(result_file, "plane_to_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--75 + registration_plane_feature.is_tait_bryan_angles = false; + registration_plane_feature.is_quaternion = false; + registration_plane_feature.is_rodrigues = true; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_plane_to_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 75; + append_to_result_file(result_file, "plane_to_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--76 + registration_plane_feature.is_gauss_newton = false; + registration_plane_feature.is_levenberg_marguardt = true; + + registration_plane_feature.is_wc = true; + registration_plane_feature.is_cw = false; + + registration_plane_feature.is_tait_bryan_angles = true; + registration_plane_feature.is_quaternion = false; + registration_plane_feature.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_plane_to_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 76; + append_to_result_file(result_file, "plane_to_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--77 + registration_plane_feature.is_tait_bryan_angles = false; + registration_plane_feature.is_quaternion = true; + registration_plane_feature.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_plane_to_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 77; + append_to_result_file(result_file, "plane_to_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--78 + registration_plane_feature.is_tait_bryan_angles = false; + registration_plane_feature.is_quaternion = false; + registration_plane_feature.is_rodrigues = true; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_plane_to_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 78; + append_to_result_file(result_file, "plane_to_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--79 + registration_plane_feature.is_wc = false; + registration_plane_feature.is_cw = true; + + registration_plane_feature.is_tait_bryan_angles = true; + registration_plane_feature.is_quaternion = false; + registration_plane_feature.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_plane_to_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 79; + append_to_result_file(result_file, "plane_to_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--80 + registration_plane_feature.is_tait_bryan_angles = false; + registration_plane_feature.is_quaternion = true; + registration_plane_feature.is_rodrigues = false; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_plane_to_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 80; + append_to_result_file(result_file, "plane_to_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--81 + registration_plane_feature.is_tait_bryan_angles = false; + registration_plane_feature.is_quaternion = false; + registration_plane_feature.is_rodrigues = true; + + start = std::chrono::system_clock::now(); + registration_plane_feature.optimize_plane_to_plane_source_to_target(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 81; + append_to_result_file(result_file, "plane_to_plane", registration_plane_feature, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + // pose graph slam + //--82-- + pose_graph_slam.overlap_threshold = 0.3; + pose_graph_slam.iterations = 6; + + pose_graph_slam.search_radius = search_radius; + pose_graph_slam.number_of_threads = number_of_threads; + pose_graph_slam.number_of_iterations_pair_wise_matching = number_of_iterations; + + //-- + pose_graph_slam.is_adaptive_robust_kernel = false; + pose_graph_slam.is_fix_first_node = true; + pose_graph_slam.is_gauss_newton = true; + pose_graph_slam.is_levenberg_marguardt = false; + pose_graph_slam.is_cw = false; + pose_graph_slam.is_wc = true; + pose_graph_slam.is_tait_bryan_angles = true; + pose_graph_slam.is_quaternion = false; + pose_graph_slam.is_rodrigues = false; + + session.point_clouds_container = temp_data; + pose_graph_slam.set_all_to_false(); + pose_graph_slam.is_ndt = true; + pose_graph_slam.pair_wise_matching_type = PoseGraphSLAM::PairWiseMatchingType::general; + pose_graph_slam.ndt_bucket_size[0] = ndt.bucket_size[0]; + pose_graph_slam.ndt_bucket_size[1] = ndt.bucket_size[1]; + pose_graph_slam.ndt_bucket_size[2] = ndt.bucket_size[2]; + start = std::chrono::system_clock::now(); + pose_graph_slam.optimize(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 82; + append_to_result_file(result_file, "pose_graph_slam (normal_distributions_transform)", pose_graph_slam, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + + //--83 + session.point_clouds_container = temp_data; + pose_graph_slam.set_all_to_false(); + pose_graph_slam.is_optimization_point_to_point_source_to_target = true; + pose_graph_slam.pair_wise_matching_type = PoseGraphSLAM::PairWiseMatchingType::general; + start = std::chrono::system_clock::now(); + pose_graph_slam.optimize(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 83; + append_to_result_file(result_file, "pose_graph_slam (point_to_point)", pose_graph_slam, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--84 + session.point_clouds_container = temp_data; + pose_graph_slam.set_all_to_false(); + pose_graph_slam.is_optimize_point_to_projection_onto_plane_source_to_target = true; + pose_graph_slam.pair_wise_matching_type = PoseGraphSLAM::PairWiseMatchingType::general; + start = std::chrono::system_clock::now(); + pose_graph_slam.optimize(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 84; + append_to_result_file(result_file, "pose_graph_slam (point_to_projection_onto_plane)", pose_graph_slam, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + + //--85 + session.point_clouds_container = temp_data; + pose_graph_slam.set_all_to_false(); + pose_graph_slam.is_optimize_point_to_plane_source_to_target = true; + pose_graph_slam.pair_wise_matching_type = PoseGraphSLAM::PairWiseMatchingType::general; + start = std::chrono::system_clock::now(); + pose_graph_slam.optimize(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 85; + append_to_result_file(result_file, "pose_graph_slam (point_to_plane_using_dot_product)", pose_graph_slam, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + + //--86 + session.point_clouds_container = temp_data; + pose_graph_slam.set_all_to_false(); + pose_graph_slam.is_optimize_distance_point_to_plane_source_to_target = true; + pose_graph_slam.pair_wise_matching_type = PoseGraphSLAM::PairWiseMatchingType::general; + start = std::chrono::system_clock::now(); + pose_graph_slam.optimize(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 86; + append_to_result_file(result_file, "pose_graph_slam (distance_point_to_plane)", pose_graph_slam, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--87 + pose_graph_slam.is_adaptive_robust_kernel = true; + + pose_graph_slam.set_all_to_false(); + pose_graph_slam.is_optimize_plane_to_plane_source_to_target = true; + pose_graph_slam.pair_wise_matching_type = PoseGraphSLAM::PairWiseMatchingType::general; + start = std::chrono::system_clock::now(); + pose_graph_slam.optimize(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 87; + append_to_result_file(result_file, "pose_graph_slam (plane_to_plane)", pose_graph_slam, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + + //--88 + pose_graph_slam.set_all_to_false(); + session.point_clouds_container = temp_data; + pose_graph_slam.is_adaptive_robust_kernel = false; + pose_graph_slam.is_ndt_lie_algebra_left_jacobian = true; + pose_graph_slam.is_lie_algebra_left_jacobian = true; + pose_graph_slam.pair_wise_matching_type = PoseGraphSLAM::PairWiseMatchingType::general; + start = std::chrono::system_clock::now(); + pose_graph_slam.optimize(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 88; + append_to_result_file(result_file, "pose_graph_slam (ndt_lie_algebra_left_jacobian)", pose_graph_slam, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + + //--89 + pose_graph_slam.set_all_to_false(); + session.point_clouds_container = temp_data; + pose_graph_slam.is_ndt_lie_algebra_right_jacobian = true; + pose_graph_slam.is_lie_algebra_right_jacobian = true; + pose_graph_slam.pair_wise_matching_type = PoseGraphSLAM::PairWiseMatchingType::general; + start = std::chrono::system_clock::now(); + pose_graph_slam.optimize(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 89; + append_to_result_file(result_file, "pose_graph_slam (ndt_lie_algebra_right_jacobian)", pose_graph_slam, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + + //--90 + pose_graph_slam.set_all_to_false(); + session.point_clouds_container = temp_data; + pose_graph_slam.is_optimize_point_to_point_source_to_target_lie_algebra_left_jacobian = true; + pose_graph_slam.is_lie_algebra_left_jacobian = true; + pose_graph_slam.pair_wise_matching_type = PoseGraphSLAM::PairWiseMatchingType::general; + start = std::chrono::system_clock::now(); + pose_graph_slam.optimize(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 90; + append_to_result_file( + result_file, "pose_graph_slam (point_to_point_lie_algebra_left_jacobian)", pose_graph_slam, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + + //--91 + pose_graph_slam.set_all_to_false(); + session.point_clouds_container = temp_data; + pose_graph_slam.is_optimize_point_to_point_source_to_target_lie_algebra_right_jacobian = true; + pose_graph_slam.is_lie_algebra_right_jacobian = true; + pose_graph_slam.pair_wise_matching_type = PoseGraphSLAM::PairWiseMatchingType::general; + start = std::chrono::system_clock::now(); + pose_graph_slam.optimize(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 91; + append_to_result_file( + result_file, "pose_graph_slam (point_to_point_lie_algebra_right_jacobian)", pose_graph_slam, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + + //--92 + pose_graph_slam.set_all_to_false(); + session.point_clouds_container = temp_data; + pose_graph_slam.is_optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_left_jacobian = true; + pose_graph_slam.is_lie_algebra_left_jacobian = true; + pose_graph_slam.pair_wise_matching_type = PoseGraphSLAM::PairWiseMatchingType::general; + start = std::chrono::system_clock::now(); + pose_graph_slam.optimize(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 92; + append_to_result_file( + result_file, + "pose_graph_slam (point_to_projection_onto_plane_lie_algebra_left_jacobian)", + pose_graph_slam, + rms, + id_method, + elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + + //--93 + pose_graph_slam.set_all_to_false(); + session.point_clouds_container = temp_data; + pose_graph_slam.is_optimize_point_to_projection_onto_plane_source_to_target_lie_algebra_right_jacobian = true; + pose_graph_slam.is_lie_algebra_right_jacobian = true; + pose_graph_slam.pair_wise_matching_type = PoseGraphSLAM::PairWiseMatchingType::general; + start = std::chrono::system_clock::now(); + pose_graph_slam.optimize(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 93; + append_to_result_file( + result_file, + "pose_graph_slam (point_to_projection_onto_plane_lie_algebra_right_jacobian)", + pose_graph_slam, + rms, + id_method, + elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); +} + +void perform_experiment_on_linux( + Session& session, + ObservationPicking& observation_picking, + ICP& icp, + NDT& ndt, + RegistrationPlaneFeature& registration_plane_feature, + PoseGraphSLAM& pose_graph_slam) +{ + fs::path path_result(session.working_directory); + path_result /= "results_linux"; + create_directory(path_result); + + session.point_clouds_container.show_with_initial_pose = false; + auto temp_data = session.point_clouds_container; + // reset_poses(); + float rms = 0.0f; + std::string result_file = session.working_directory + "/result_linux.csv"; + create_header(result_file); + double initial_rms = compute_rms(false, session, observation_picking); + std::cout << "initial rms: " << initial_rms << std::endl; + add_initial_rms_to_file(result_file, initial_rms); + + float search_radius = 0.1f; + int number_of_threads = 16; + int number_of_iterations = 6; + int id_method = 0; + + // pose graph slam + //--94-- + pose_graph_slam.overlap_threshold = 0.3; + pose_graph_slam.iterations = 6; + + pose_graph_slam.search_radius = search_radius; + pose_graph_slam.number_of_threads = number_of_threads; + pose_graph_slam.number_of_iterations_pair_wise_matching = number_of_iterations; + + //-- + pose_graph_slam.is_adaptive_robust_kernel = false; + pose_graph_slam.is_fix_first_node = true; + pose_graph_slam.is_gauss_newton = true; + pose_graph_slam.is_levenberg_marguardt = false; + pose_graph_slam.is_cw = false; + pose_graph_slam.is_wc = true; + pose_graph_slam.is_tait_bryan_angles = false; + pose_graph_slam.is_quaternion = false; + pose_graph_slam.is_rodrigues = true; + + session.point_clouds_container = temp_data; + + // pose_graph_slam.is_ndt = true; + // pose_graph_slam.pair_wise_matching_type = PoseGraphSLAM::PairWiseMatchingType::general; + // pose_graph_slam.ndt_bucket_size[0] = ndt.bucket_size[0]; + // pose_graph_slam.ndt_bucket_size[1] = ndt.bucket_size[1]; + // pose_graph_slam.ndt_bucket_size[2] = ndt.bucket_size[2]; + // pose_graph_slam.optimize(point_clouds_container); + // rms = compute_rms(); + // id_method = 94; + // append_to_result_file(result_file, "pose_graph_slam (normal_distributions_transform)", pose_graph_slam, rms, id_method); + +#ifdef WITH_PCL + pose_graph_slam.set_all_to_false(); + pose_graph_slam.is_optimize_pcl_ndt = true; + pose_graph_slam.pair_wise_matching_type = PoseGraphSLAM::PairWiseMatchingType::pcl_ndt; + + auto start = std::chrono::system_clock::now(); + pose_graph_slam.optimize(session.point_clouds_container); + auto end = std::chrono::system_clock::now(); + auto elapsed = std::chrono::duration_cast(end - start); + + rms = compute_rms(false, session, observation_picking); + id_method = 94; + append_to_result_file(result_file, "pose_graph_slam (pcl_ndt)", pose_graph_slam, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--95-- + pose_graph_slam.set_all_to_false(); + pose_graph_slam.is_optimize_pcl_icp = true; + pose_graph_slam.pair_wise_matching_type = PoseGraphSLAM::PairWiseMatchingType::pcl_icp; + + start = std::chrono::system_clock::now(); + pose_graph_slam.optimize(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + + rms = compute_rms(false, session, observation_picking); + id_method = 95; + append_to_result_file(result_file, "pose_graph_slam (pcl_icp)", pose_graph_slam, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; +#endif + +#if WITH_GTSAM + //--96-- + try + { + pose_graph_slam.ndt_bucket_size[0] = ndt.bucket_size[0]; + pose_graph_slam.ndt_bucket_size[1] = ndt.bucket_size[1]; + pose_graph_slam.ndt_bucket_size[2] = ndt.bucket_size[2]; + + pose_graph_slam.set_all_to_false(); + pose_graph_slam.is_optimize_pcl_ndt = true; + pose_graph_slam.is_lie_algebra_right_jacobian = true; + pose_graph_slam.pair_wise_matching_type = PoseGraphSLAM::PairWiseMatchingType::pcl_ndt; + + auto start = std::chrono::system_clock::now(); + pose_graph_slam.optimize_with_GTSAM(session.point_clouds_container); + auto end = std::chrono::system_clock::now(); + auto elapsed = std::chrono::duration_cast(end - start); + rms = compute_rms(false, session, observation_picking); + id_method = 96; + append_to_result_file(result_file, "pose_graph_slam (GTSAM pcl_ndt)", pose_graph_slam, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + } catch (std::exception& e) + { + std::cout << e.what() << std::endl; + rms = compute_rms(false, session, observation_picking); + // append_to_result_file(result_file, "pose_graph_slam (GTSAM pcl_ndt)", pose_graph_slam, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + } + //--97-- + try + { + pose_graph_slam.set_all_to_false(); + pose_graph_slam.is_optimize_pcl_icp = true; + pose_graph_slam.is_lie_algebra_right_jacobian = true; + pose_graph_slam.pair_wise_matching_type = PoseGraphSLAM::PairWiseMatchingType::pcl_icp; + + auto start = std::chrono::system_clock::now(); + pose_graph_slam.optimize_with_GTSAM(session.point_clouds_container); + auto end = std::chrono::system_clock::now(); + auto elapsed = std::chrono::duration_cast(end - start); + + rms = compute_rms(false, session, observation_picking); + id_method = 97; + append_to_result_file(result_file, "pose_graph_slam (GTSAM pcl_icp)", pose_graph_slam, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + } catch (std::exception& e) + { + std::cout << e.what() << std::endl; + rms = compute_rms(false, session, observation_picking); + // append_to_result_file(result_file, "pose_graph_slam (GTSAM pcl_icp)", pose_graph_slam, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + } +#endif +#if WITH_MANIF + //--98-- + { + pose_graph_slam.set_all_to_false(); + pose_graph_slam.is_optimize_pcl_ndt = true; + pose_graph_slam.is_lie_algebra_right_jacobian = true; + pose_graph_slam.pair_wise_matching_type = PoseGraphSLAM::PairWiseMatchingType::pcl_ndt; + + auto start = std::chrono::system_clock::now(); + pose_graph_slam.optimize_with_manif(session.point_clouds_container); + auto end = std::chrono::system_clock::now(); + auto elapsed = std::chrono::duration_cast(end - start); + + rms = compute_rms(false, session, observation_picking); + id_method = 98; + append_to_result_file(result_file, "pose_graph_slam (manif pcl_ndt)", pose_graph_slam, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + + //--99-- + pose_graph_slam.set_all_to_false(); + pose_graph_slam.is_optimize_pcl_icp = true; + pose_graph_slam.is_lie_algebra_right_jacobian = true; + pose_graph_slam.pair_wise_matching_type = PoseGraphSLAM::PairWiseMatchingType::pcl_icp; + + start = std::chrono::system_clock::now(); + pose_graph_slam.optimize_with_manif(session.point_clouds_container); + end = std::chrono::system_clock::now(); + elapsed = std::chrono::duration_cast(end - start); + + rms = compute_rms(false, session, observation_picking); + id_method = 99; + append_to_result_file(result_file, "pose_graph_slam (manif pcl_icp)", pose_graph_slam, rms, id_method, elapsed); + export_result_to_folder(path_result.string(), id_method, observation_picking, session); + session.point_clouds_container = temp_data; + } +#endif +} + +#if 0 +bool exportLaz(const std::string &filename, + const std::vector &pointcloud, + const std::vector &intensity, double offset_x, double offset_y, double offset_alt) +{ + + constexpr float scale = 0.0001f; // one tenth of milimeter + // find max + Eigen::Vector3d max(std::numeric_limits::lowest(), std::numeric_limits::lowest(), std::numeric_limits::lowest()); + Eigen::Vector3d min(std::numeric_limits::max(), std::numeric_limits::max(), std::numeric_limits::max()); + for (auto &p : pointcloud) + { + max.x() = std::max(max.x(), p.x()); + max.y() = std::max(max.y(), p.y()); + max.z() = std::max(max.z(), p.z()); + + min.x() = std::min(min.x(), p.x()); + min.y() = std::min(min.y(), p.y()); + min.z() = std::min(min.z(), p.z()); + } + + std::cout << "exportLaz to file: '" << filename << "'" << std::endl; + std::cout << std::setprecision(20) << "min.x " << min.x() << " " << max.x() << std::endl; + std::cout << "min.y " << min.y() << " " << max.y() << std::endl; + std::cout << "min.z " << min.z() << " " << max.z() << std::endl; + + // create the writer + laszip_POINTER laszip_writer; + if (laszip_create(&laszip_writer)) + { + spdlog::error("DLL ERROR: creating laszip writer"); + return false; + } + + // get a pointer to the header of the writer so we can populate it + + laszip_header *header; + + if (laszip_get_header_pointer(laszip_writer, &header)) + { + spdlog::error("DLL ERROR: getting header pointer from laszip writer"); + return false; + } + + // populate the header + + header->file_source_ID = 4711; + header->global_encoding = (1 << 0); // see LAS specification for details + header->version_major = 1; + header->version_minor = 2; + // header->file_creation_day = 120; + // header->file_creation_year = 2013; + header->point_data_format = 1; + header->point_data_record_length = 0; + header->number_of_point_records = pointcloud.size(); + header->number_of_points_by_return[0] = pointcloud.size(); + header->number_of_points_by_return[1] = 0; + header->point_data_record_length = 28; + header->x_scale_factor = scale; + header->y_scale_factor = scale; + header->z_scale_factor = scale; + + header->max_x = max.x() + offset_x; + header->min_x = min.x() + offset_x; + header->max_y = max.y() + offset_y; + header->min_y = min.y() + offset_y; + header->max_z = max.z() + offset_alt; + header->min_z = min.z() + offset_alt; + + header->x_offset = offset_x; + header->y_offset = offset_y; + header->z_offset = offset_alt; + + // optional: use the bounding box and the scale factor to create a "good" offset + // open the writer + laszip_BOOL compress = (strstr(filename.c_str(), ".laz") != 0); + + if (laszip_open_writer(laszip_writer, filename.c_str(), compress)) + { + spdlog::error("DLL ERROR: opening laszip writer for '{}'", filename ); + return false; + } + + spdlog::info("writing file '{}' {}compressed", filename, (compress ? "" : "un")); + + // get a pointer to the point of the writer that we will populate and write + + laszip_point *point; + if (laszip_get_point_pointer(laszip_writer, &point)) + { + spdlog::error("DLL ERROR: getting point pointer from laszip writer"); + return false; + } + + laszip_I64 p_count = 0; + laszip_F64 coordinates[3]; + + for (int i = 0; i < pointcloud.size(); i++) + { + point->intensity = intensity[i]; + + const auto &p = pointcloud[i]; + p_count++; + coordinates[0] = p.x();// + offset_x; + coordinates[1] = p.y();// + offset_y; + coordinates[2] = p.z();// + offset_alt; + if (laszip_set_coordinates(laszip_writer, coordinates)) + { + spdlog::error("DLL ERROR: setting coordinates for point {}", p_count); + return false; + } + + // p.SetIntensity(pp.intensity); + + // if (i < intensity.size()) { + // point->intensity = intensity[i]; + // } + // laszip_set_point + + if (laszip_write_point(laszip_writer)) + { + spdlog::error("DLL ERROR: writing point {}", p_count); + return false; + } + } + + if (laszip_get_point_count(laszip_writer, &p_count)) + { + spdlog::error("DLL ERROR: getting point count"); + return false; + } + + spdlog::info("successfully written {} points", p_count); + + // close the writer + + if (laszip_close_writer(laszip_writer)) + { + spdlog::error("DLL ERROR: closing laszip writer"); + return false; + } + + // destroy the writer + + if (laszip_destroy(laszip_writer)) + { + spdlog::error("DLL ERROR: destroying laszip writer"); + return false; + } + + std::cout << "exportLaz DONE" << std::endl; + return true; +} +#endif + +#if 0 +//void export_result_to_folder(std::string output_folder_name, ObservationPicking &observation_picking, Session &session); +void export_result_to_folder(std::string output_folder_name, ObservationPicking &observation_picking, Session &session) +{ + fs::path path(output_folder_name); + std::string file_name_rms = "rms.csv"; + auto path_rms = path; + path_rms /= file_name_rms; + std::cout << "exporting to file: '" << path_rms.string() << "'" << std::endl; + std::ofstream outfile_rms; + outfile_rms.open(path_rms, std::ios_base::app); + outfile_rms << "index_roi, rms_initial, rms_result" << std::endl; + + for (int i = 0; i < observation_picking.intersections.size(); i++) + { + std::string file_name_initial = "intersection_" + std::to_string(i) + "_initial.csv"; + std::string file_name_result = "intersection_" + std::to_string(i) + "_result.csv"; + + auto path_initial = path; + auto path_result = path; + + path_initial /= file_name_initial; + path_result /= file_name_result; + + std::cout << "exporting to file: '" << path_initial.string() << "'" << std::endl; + std::cout << "exporting to file: '" << path_result.string() << "'" << std::endl; + + std::ofstream outfile_initial; + std::ofstream outfile_result; + + outfile_initial.open(path_initial, std::ios_base::app); + outfile_result.open(path_result, std::ios_base::app); + + const auto &intersection = observation_picking.intersections[i]; + TaitBryanPose pose; + pose.px = intersection.translation[0]; + pose.py = intersection.translation[1]; + pose.pz = intersection.translation[2]; + pose.om = intersection.rotation[0]; + pose.fi = intersection.rotation[1]; + pose.ka = intersection.rotation[2]; + Eigen::Affine3d m_pose_inv = affine_matrix_from_pose_tait_bryan(pose).inverse(); + + double w = intersection.width_length_height[0] * 0.5; + double l = intersection.width_length_height[1] * 0.5; + double h = intersection.width_length_height[2] * 0.5; + + outfile_initial << "x;y;z;pc_index;is_initial;index_intersection;file" << std::endl; + outfile_result << "x;y;z;pc_index;is_initial;index_intersection;file" << std::endl; + + for (int pc_index = 0; pc_index < session.point_clouds_container.point_clouds.size(); pc_index++) + { + const auto &pc = session.point_clouds_container.point_clouds[pc_index]; + for (const auto &p : pc.points_local) + { + Eigen::Vector3d vpi = pc.m_initial_pose * p; + Eigen::Vector3d vpr = pc.m_pose * p; + + Eigen::Vector3d vpit = m_pose_inv * vpi; + Eigen::Vector3d vprt = m_pose_inv * vpr; + + if (fabs(vpit.x()) < w) + { + if (fabs(vpit.y()) < l) + { + if (fabs(vpit.z()) < h) + { + outfile_initial << vpit.x() << ";" << vpit.y() << ";" << vpit.z() << ";" << pc_index << ";1;" << i << ";" << pc.file_name << std::endl; + } + } + } + if (fabs(vprt.x()) < w) + { + if (fabs(vprt.y()) < l) + { + if (fabs(vprt.z()) < h) + { + outfile_result << vprt.x() << ";" << vprt.y() << ";" << vprt.z() << ";" << pc_index << ";0;" << i << ";" << pc.file_name << std::endl; + } + } + } + } + } + outfile_initial.close(); + outfile_result.close(); + + const auto &obs = observation_picking.observations[i]; + double rms_initial = 0.0; + int sum = 0; + double rms_result = 0.0; + + for (const auto &[key1, value1] : obs) + { + for (const auto &[key2, value2] : obs) + { + if (key1 != key2) + { + Eigen::Vector3d p1, p2; + p1 = session.point_clouds_container.point_clouds[key1].m_initial_pose * value1; + p2 = session.point_clouds_container.point_clouds[key2].m_initial_pose * value2; + rms_initial += (p2.x() - p1.x()) * (p2.x() - p1.x()); + rms_initial += (p2.y() - p1.y()) * (p2.y() - p1.y()); + + p1 = session.point_clouds_container.point_clouds[key1].m_pose * value1; + p2 = session.point_clouds_container.point_clouds[key2].m_pose * value2; + rms_result += (p2.x() - p1.x()) * (p2.x() - p1.x()); + rms_result += (p2.y() - p1.y()) * (p2.y() - p1.y()); + + sum += 2; + } + } + } + std::cout << "sum: " << sum << std::endl; + if (sum > 0) + { + rms_initial = sqrt(rms_initial / sum); + rms_result = sqrt(rms_result / sum); + outfile_rms << i << ";" << rms_initial << ";" << rms_result << std::endl; + } + } + outfile_rms.close(); + + std::string file_name_poses = "poses_RESSO.reg"; + auto path_poses = path; + path_poses /= file_name_poses; + std::cout << "saving poses to: " << path_poses << std::endl; + session.point_clouds_container.save_poses(path_poses.string(), false); +} + + +//void export_result_to_folder(std::string output_folder_name, ObservationPicking &observation_picking, Session &session) +void export_result_to_folder(std::string output_folder_name, int method_id, ObservationPicking &observation_picking) +{ + fs::path path(output_folder_name); + path /= std::to_string(method_id); + create_directory(path); + export_result_to_folder(path.string(), observation_picking); +} + +#endif \ No newline at end of file diff --git a/apps/multi_view_tls_registration_legacy/resource.h b/apps/multi_view_tls_registration_legacy/resource.h new file mode 100644 index 00000000..4f3204ca --- /dev/null +++ b/apps/multi_view_tls_registration_legacy/resource.h @@ -0,0 +1,17 @@ +//{{NO_DEPENDENCIES}} +// Microsoft Visual C++ generated include file. +// Used by resource.rc +// +#define IDI_ICON1 101 // application icon +#define VS_VERSION_INFO 1 // version info + +// Next default values for new objects +// +#ifdef APSTUDIO_INVOKED +#ifndef APSTUDIO_READONLY_SYMBOLS +#define _APS_NEXT_RESOURCE_VALUE 106 +#define _APS_NEXT_COMMAND_VALUE 40001 +#define _APS_NEXT_CONTROL_VALUE 1000 +#define _APS_NEXT_SYMED_VALUE 101 +#endif +#endif diff --git a/apps/multi_view_tls_registration_legacy/resource.rc b/apps/multi_view_tls_registration_legacy/resource.rc new file mode 100644 index 00000000..77dce10d --- /dev/null +++ b/apps/multi_view_tls_registration_legacy/resource.rc @@ -0,0 +1,49 @@ +// Microsoft Visual C++ generated resource script. +// +#include "resource.h" +///////////////////////////////////////////////////////////////////////////// +// English (United States) resources + +///////////////////////////////////////////////////////////////////////////// +// +// Icon +// + +// Icon with lowest ID value placed first to ensure application icon +// remains consistent on all systems. +IDI_ICON1 ICON "icon.ico" + + +///////////////////////////////////////////////////////////////////////////// +// +// Version +// + +VS_VERSION_INFO VERSIONINFO +FILEVERSION 0, 0, 100, 1 +PRODUCTVERSION 0, 0, 100, 1 +FILEFLAGSMASK 0x3fL +FILEOS 0x40004 +FILETYPE 0x1 +BEGIN +BLOCK "StringFileInfo" +BEGIN +BLOCK "040904B0" +BEGIN +VALUE "CompanyName", "Mandeye\0" +VALUE "FileDescription", "HDMapping Step 2\0" +VALUE "FileVersion", "0.100.1\0" +VALUE "InternalName", "Multi view TLS registration\0" +VALUE "LegalCopyright", "(c) 2026 github.com/MapsHD/HDMapping\0" +VALUE "OriginalFilename", "multi_view_tls_registration_step_2.exe\0" +VALUE "ProductVersion", "0.100.1\0" +VALUE "ProgramID", "github.com/MapsHD/HDMapping\0" +VALUE "ProductName", "HDMapping\0" +END +END +BLOCK "VarFileInfo" +BEGIN +VALUE "Translation", 0x409, 0x04B0 +END +END +///////////////////////////////////////////////////////////////////////////// \ No newline at end of file From 3ce91ff76da8899eb73c2dc7ed98eea92b817b64 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Sun, 26 Jul 2026 00:27:23 +0200 Subject: [PATCH 09/17] Drop FLAG_MSAA_4X_HINT to fix a GLX-driven crash On some GPU/driver combinations (seen with an NVIDIA PRIME-offloaded context) GLFW's GLX request for a 4x-multisample framebuffer fails outright ("GLX: Failed to create context: BadValue"), and raylib/GLFW then segfaults using the broken context instead of degrading gracefully. Not worth the crash risk for a cosmetic antialiasing hint. Co-Authored-By: Claude Sonnet 5 Claude-Session: https://claude.ai/code/session_01XgxCVLnEEAftKKRvZHPUB9 --- .../multi_view_tls_registration_gui.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) 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 a016959a..a92e3f04 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 @@ -4692,7 +4692,13 @@ bool initGL(int* argc, char** argv, const std::string& winTitleArg, void (*)(), (void)argc; (void)argv; - SetConfigFlags(FLAG_WINDOW_RESIZABLE | FLAG_MSAA_4X_HINT); + // No FLAG_MSAA_4X_HINT: on some GPU/driver combinations (seen with an + // NVIDIA PRIME-offloaded context) GLFW's GLX context request for a + // 4x-multisample framebuffer fails outright ("GLX: Failed to create + // context: BadValue"), and raylib/GLFW then segfaults using the broken + // context instead of falling back cleanly. Not worth the crash risk for + // a cosmetic antialiasing hint. + SetConfigFlags(FLAG_WINDOW_RESIZABLE); InitWindow(static_cast(window_width), static_cast(window_height), winTitleArg.c_str()); SetTargetFPS(60); From 484ccb36af47c6927cadc5c1b9a8bebf9e25cdbe Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Sun, 26 Jul 2026 00:27:45 +0200 Subject: [PATCH 10/17] Add GL_RENDERER, draw-call and vertex counts to the app bar Surfaces GPU renderer info (raylib has no wrapper for it) plus per-frame draw-call/vertex counts from ScanRenderer::draw(), replacing the plain FPS counter. ScanRenderer tracks its own counts since neither raylib nor rlgl expose draw-call/vertex stats for custom (non-batched) glDrawArrays calls. Co-Authored-By: Claude Sonnet 5 Claude-Session: https://claude.ai/code/session_01XgxCVLnEEAftKKRvZHPUB9 --- .../multi_view_tls_registration_gui.cpp | 16 ++++++++++++++- core/include/Core/raylib_render.hpp | 20 +++++++++++++++++++ core/src/raylib_render.cpp | 5 +++++ 3 files changed, 40 insertions(+), 1 deletion(-) 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 a92e3f04..1f9471dd 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 @@ -4224,10 +4224,24 @@ void display() viewer_decimate_point_cloud = 1; ImGui::SameLine(); - ImGui::Text("(%.1f FPS)", ImGui::GetIO().Framerate); + // GetFPS()/point-cloud draw-call/vertex count via raylib/ScanRenderer, + // rather than ImGui's own Framerate tracker -- raylib doesn't + // expose a general "draw calls" counter (rlgl's own internal one + // only tracks its immediate-mode batch renderer, not custom + // glDrawArrays calls like ScanRenderer's), so these are scan_renderer's + // own per-frame counts of the calls/points it issued in draw(). + ImGui::Text("(%d FPS, %d draw calls, %d vertices)", GetFPS(), scan_renderer.lastDrawCallCount(), + scan_renderer.lastVertexCount()); } ImGui::EndDisabled(); + ImGui::SameLine(); + // GL_RENDERER has no raylib wrapper (raylib doesn't expose GPU + // vendor/renderer strings), so this is the same plain GL query + // info_window()'s tooltip already uses, just surfaced directly in + // the bar instead of hidden behind a hover. + ImGui::TextDisabled("| %s", reinterpret_cast(glGetString(GL_RENDERER))); + ImGui::SameLine( ImGui::GetWindowWidth() - ImGui::CalcTextSize("Info").x - ImGui::GetStyle().ItemSpacing.x * 2 - ImGui::GetStyle().FramePadding.x * 2); diff --git a/core/include/Core/raylib_render.hpp b/core/include/Core/raylib_render.hpp index 2b940628..9406ae53 100644 --- a/core/include/Core/raylib_render.hpp +++ b/core/include/Core/raylib_render.hpp @@ -107,6 +107,24 @@ class ScanRenderer float distanceMax = 1.f, int decimateStride = 1) const; + // Number of glDrawArrays calls draw() issued the last time it ran (one + // per visible scan) -- raylib/rlgl don't expose a draw-call counter for + // custom, non-batched GL calls like these (rlgl's own internal + // drawCounter only tracks its immediate-mode batch renderer), so this + // app-level count is the closest equivalent for a "draw calls" stat. + int lastDrawCallCount() const + { + return lastDrawCallCount_; + } + + // Total points actually submitted to the GPU across all draw() calls + // last frame (i.e. sum of each visible scan's decimated drawCount) -- + // the vertex-count counterpart to lastDrawCallCount() above. + int lastVertexCount() const + { + return lastVertexCount_; + } + // Overrides a scan's draw() color with a flat mark color -- e.g. to // highlight the loop-closure source/target scans in red/blue -- drawn // from the same cached full-resolution GPU buffer as a normal scan @@ -172,4 +190,6 @@ class ScanRenderer int locElevMax_ = -1; int locDistCenter_ = -1; int locDistMax_ = -1; + mutable int lastDrawCallCount_ = 0; + mutable int lastVertexCount_ = 0; }; diff --git a/core/src/raylib_render.cpp b/core/src/raylib_render.cpp index 57f74d1c..8231ab90 100644 --- a/core/src/raylib_render.cpp +++ b/core/src/raylib_render.cpp @@ -271,6 +271,9 @@ void ScanRenderer::draw( float distanceMax, int decimateStride) const { + lastDrawCallCount_ = 0; + lastVertexCount_ = 0; + if (!shaderValid_) { return; @@ -350,6 +353,8 @@ void ScanRenderer::draw( const int drawCount = (gpu.count + stride - 1) / stride; glDrawArrays(GL_POINTS, 0, drawCount); + ++lastDrawCallCount_; + lastVertexCount_ += drawCount; rlDisableVertexArray(); } From febac13bf9ba17f0f5d3184165685737a076bc14 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Sun, 26 Jul 2026 00:28:16 +0200 Subject: [PATCH 11/17] Hint hybrid-GPU laptops to prefer the discrete GPU Windows: force GLFW_USE_HYBRID_HPG ON so the raylib build exports NvOptimusEnablement/AmdPowerXpressRequestHighPerformance, the standard symbol hint NVIDIA/AMD drivers read from an EXE to pick the discrete GPU on Optimus/PowerXpress laptops. No build-time equivalent exists on Linux (GPU selection there is a runtime PRIME choice), so add a README note pointing Linux users at prime-run instead. Co-Authored-By: Claude Sonnet 5 Claude-Session: https://claude.ai/code/session_01XgxCVLnEEAftKKRvZHPUB9 --- README.md | 6 ++++++ cmake/raylib.cmake | 10 ++++++++++ 2 files changed, 16 insertions(+) diff --git a/README.md b/README.md index 422e0362..98773092 100644 --- a/README.md +++ b/README.md @@ -61,6 +61,12 @@ cmake -B build -S . -DCMAKE_BUILD_TYPE=Release cmake --build build --config Release -j ``` +**Laptops with hybrid NVIDIA/Intel graphics:** the raylib-based apps (e.g. `multi_view_tls_registration_step_2`) may default to the integrated GPU. If `nvidia-prime` is installed, launch with `prime-run` to force the discrete NVIDIA GPU instead, e.g.: + +```bash +prime-run ./build/bin/multi_view_tls_registration_step_2 +``` + ## Quick Start (macOS) **Prerequisites:** diff --git a/cmake/raylib.cmake b/cmake/raylib.cmake index 752c1b9d..42147c52 100644 --- a/cmake/raylib.cmake +++ b/cmake/raylib.cmake @@ -13,6 +13,16 @@ include(FetchContent) set(BUILD_EXAMPLES OFF CACHE BOOL "" FORCE) set(BUILD_GAMES OFF CACHE BOOL "" FORCE) +if(WIN32) + # Makes GLFW's Win32 backend export NvOptimusEnablement/ + # AmdPowerXpressRequestHighPerformance (see glfw/src/win32_init.c), the + # standard hint NVIDIA Optimus/AMD PowerXpress drivers read from an EXE's + # exports to prefer the discrete GPU on hybrid-graphics laptops. Win32-only + # (GLFW_USE_HYBRID_HPG is a no-op on X11/Wayland/macOS) -- Linux hybrid + # offload is a runtime env-var choice instead (__NV_PRIME_RENDER_OFFLOAD=1 + # __GLX_VENDOR_LIBRARY_NAME=nvidia, or prime-run), not a build-time one. + set(GLFW_USE_HYBRID_HPG ON CACHE BOOL "" FORCE) +endif() FetchContent_Declare( raylib GIT_REPOSITORY https://github.com/raysan5/raylib.git From a19f62bd2130553eaa0b81699183b6b900fe9043 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Sun, 26 Jul 2026 00:36:49 +0200 Subject: [PATCH 12/17] Apply clang-format to the FPS/draw-call/vertex text line Co-Authored-By: Claude Sonnet 5 Claude-Session: https://claude.ai/code/session_01XgxCVLnEEAftKKRvZHPUB9 --- .../multi_view_tls_registration_gui.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp b/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp index 1f9471dd..dc494a1f 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 @@ -4230,8 +4230,8 @@ void display() // only tracks its immediate-mode batch renderer, not custom // glDrawArrays calls like ScanRenderer's), so these are scan_renderer's // own per-frame counts of the calls/points it issued in draw(). - ImGui::Text("(%d FPS, %d draw calls, %d vertices)", GetFPS(), scan_renderer.lastDrawCallCount(), - scan_renderer.lastVertexCount()); + ImGui::Text( + "(%d FPS, %d draw calls, %d vertices)", GetFPS(), scan_renderer.lastDrawCallCount(), scan_renderer.lastVertexCount()); } ImGui::EndDisabled(); From 9338472287f3e2a81fcd3ca69924fde3bd348a0e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Sun, 26 Jul 2026 00:38:55 +0200 Subject: [PATCH 13/17] Lead with raw PRIME env vars over prime-run in the README prime-run isn't always installed even when prime-select is, and prime-select nvidia alone doesn't force offload mode per-launch -- give the underlying env vars as the primary instructions, prime-run as a shorthand when available. Co-Authored-By: Claude Sonnet 5 Claude-Session: https://claude.ai/code/session_01XgxCVLnEEAftKKRvZHPUB9 --- README.md | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 98773092..f350e52e 100644 --- a/README.md +++ b/README.md @@ -61,7 +61,13 @@ cmake -B build -S . -DCMAKE_BUILD_TYPE=Release cmake --build build --config Release -j ``` -**Laptops with hybrid NVIDIA/Intel graphics:** the raylib-based apps (e.g. `multi_view_tls_registration_step_2`) may default to the integrated GPU. If `nvidia-prime` is installed, launch with `prime-run` to force the discrete NVIDIA GPU instead, e.g.: +**Laptops with hybrid NVIDIA/Intel graphics:** the raylib-based apps (e.g. `multi_view_tls_registration_step_2`) may default to the integrated GPU even with `prime-select nvidia` set, since PRIME's on-demand/offload mode is a per-launch choice, not a system default. Force the discrete NVIDIA GPU with: + +```bash +__NV_PRIME_RENDER_OFFLOAD=1 __GLX_VENDOR_LIBRARY_NAME=nvidia ./build/bin/multi_view_tls_registration_step_2 +``` + +If the `nvidia-prime` package is installed, `prime-run` wraps the same env vars: ```bash prime-run ./build/bin/multi_view_tls_registration_step_2 From 3b1b81134806fd72b43f9ee12c2dd69234d339f8 Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Mon, 27 Jul 2026 02:11:15 +0200 Subject: [PATCH 14/17] Fix 3D picking and port GCP/GNSS/ControlPoints rendering to raylib GetLaserBeam() queried rlgl's matrix stack live, but mouse() runs before display() each frame, so it was reading the previous frame's post-end3DMatrixStack() state (identity modelview, 2D ortho projection) instead of the 3D camera -- every pick ray was garbage, so Ctrl+click picking (GCP, loop closure, translate tool) would grab an arbitrary point and the camera-transition would fly there, looking like the scene exploding. display() now caches the 3D view/projection into frame_view_3d/frame_proj_3d right before end3DMatrixStack() resets the stack, and GetLaserBeam() reads those instead. Also ports the three remaining legacy-GL render() functions (shared with the GLUT apps in `core`, so they can't be touched) to raylib: GroundControlPoints, GNSS, and ControlPoints -- markers/lines via DrawLine3D/rlBegin(RL_LINES), text labels via 2D screen-space DrawText projected through frame_mvp_3d. ControlPoints additionally restricts scan_renderer.draw() to the active scan while editing, since the bulk multi-scan draw is skipped entirely in that mode. Fixes a label-overlap bug found along the way (GCP/Control Points labels sitting centimeters apart in world space collapsed to the same screen pixels) by giving drawOutlinedText() a `line` param so nearby labels stack instead of overlapping, and restores full scan visibility when Manual Loop Closure closes (nothing previously undid the source/target-only filtering it applies while open). Co-Authored-By: Claude Sonnet 5 --- .../multi_view_tls_registration_gui.cpp | 476 ++++++++++++++++-- apps/multi_view_tls_registration/rl_utils.cpp | 8 +- apps/multi_view_tls_registration/rl_utils.h | 13 + 3 files changed, 464 insertions(+), 33 deletions(-) diff --git a/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp b/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp index dc494a1f..920fdf00 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 @@ -1,3 +1,4 @@ +#include #include // This app used to be built on GLUT + legacy immediate-mode OpenGL @@ -140,6 +141,11 @@ void observationPickingRender(const ObservationPicking& observation_picking); void renderLoopClosure( PointClouds& point_clouds_container, int index_loop_closure_source, int index_loop_closure_target, int before, int after); void renderLoopClosureLabels(PointClouds& point_clouds_container); +void renderGroundControlPoints(const GroundControlPoints& ground_control_points, const PointClouds& point_clouds_container); +void renderGroundControlPointsLabels(const GroundControlPoints& ground_control_points, const PointClouds& point_clouds_container); +void renderGNSS(const GNSS& gnss, const PointClouds& point_clouds_container); +void renderControlPoints(const ControlPoints& control_points, PointClouds& point_clouds_container); +void renderControlPointsLabels(const ControlPoints& control_points, const PointClouds& point_clouds_container); void display(); void mouse(int glut_button, int state, int x, int y); @@ -2546,6 +2552,291 @@ void renderLoopClosure( } } +// Was GroundControlPoints::draw_ellipse() (core/src/ground_control_points.cpp) +// -- a GL_LINE_LOOP quad per lat/long grid cell. rlgl's rlBegin() has no +// LOOP/STRIP mode (only RL_LINES/RL_TRIANGLES/RL_QUADS -- see +// renderLoopClosureLabels()'s comment for the same constraint elsewhere in +// this file), so each cell's 4-vertex loop is emitted as 4 independent line +// segments instead. nstd is dropped: the only caller always passed 1.0. +void drawUncertaintyEllipse(const Eigen::Matrix3d& covar, const Eigen::Vector3d& mean, Color color) +{ + Eigen::LLT> cholSolver(covar); + Eigen::Matrix3d transform = cholSolver.matrixL(); + + const double pi = 3.141592; + const double di = 0.02; + const double dj = 0.04; + const double du = di * 2 * pi; + const double dv = dj * pi; + + rlBegin(RL_LINES); + rlColor4ub(color.r, color.g, color.b, color.a); + + for (double i = 0; i < 1.0; i += di) // horizontal + { + for (double j = 0; j < 1.0; j += dj) // vertical + { + double u = i * 2 * pi; // 0 to 2pi + double v = (j - 0.5) * pi; //-pi/2 to pi/2 + + const Eigen::Vector3d pp0(cos(v) * cos(u), cos(v) * sin(u), sin(v)); + const Eigen::Vector3d pp1(cos(v) * cos(u + du), cos(v) * sin(u + du), sin(v)); + const Eigen::Vector3d pp2(cos(v + dv) * cos(u + du), cos(v + dv) * sin(u + du), sin(v + dv)); + const Eigen::Vector3d pp3(cos(v + dv) * cos(u), cos(v + dv) * sin(u), sin(v + dv)); + Eigen::Vector3d tp0 = transform * pp0 + mean; + Eigen::Vector3d tp1 = transform * pp1 + mean; + Eigen::Vector3d tp2 = transform * pp2 + mean; + Eigen::Vector3d tp3 = transform * pp3 + mean; + + rlVertex3f(static_cast(tp0.x()), static_cast(tp0.y()), static_cast(tp0.z())); + rlVertex3f(static_cast(tp1.x()), static_cast(tp1.y()), static_cast(tp1.z())); + + rlVertex3f(static_cast(tp1.x()), static_cast(tp1.y()), static_cast(tp1.z())); + rlVertex3f(static_cast(tp2.x()), static_cast(tp2.y()), static_cast(tp2.z())); + + rlVertex3f(static_cast(tp2.x()), static_cast(tp2.y()), static_cast(tp2.z())); + rlVertex3f(static_cast(tp3.x()), static_cast(tp3.y()), static_cast(tp3.z())); + + rlVertex3f(static_cast(tp3.x()), static_cast(tp3.y()), static_cast(tp3.z())); + rlVertex3f(static_cast(tp0.x()), static_cast(tp0.y()), static_cast(tp0.z())); + } + } + + rlEnd(); +} + +// Was GroundControlPoints::render() (core/src/ground_control_points.cpp) -- +// legacy-GL, compiled once into `core` and shared with the remaining GLUT +// apps, so it can't be touched; reimplemented here with raylib's own +// DrawLine3D (works against whatever rlgl projection/modelview is currently +// active, same as renderLoopClosure()'s DrawSphere/DrawCylinderEx calls -- +// no BeginMode3D needed). Name/height text labels are handled separately by +// renderGroundControlPointsLabels() (2D screen-space DrawText, same +// reasoning as renderLoopClosureLabels()). +void renderGroundControlPoints(const GroundControlPoints& ground_control_points, const PointClouds& point_clouds_container) +{ + for (const auto& gcp : ground_control_points.gpcs) + { + if (gcp.index_to_node_inner < 0 || + static_cast(gcp.index_to_node_inner) >= point_clouds_container.point_clouds.size()) + { + continue; + } + const auto& pc = point_clouds_container.point_clouds[gcp.index_to_node_inner]; + if (gcp.index_to_node_outer < 0 || static_cast(gcp.index_to_node_outer) >= pc.local_trajectory.size()) + { + continue; + } + + Eigen::Vector3d c = pc.m_pose * pc.local_trajectory[gcp.index_to_node_outer].m_pose.translation(); + float h = static_cast(gcp.lidar_height_above_ground); + Vector3 g{ static_cast(gcp.x), static_cast(gcp.y), static_cast(gcp.z) }; + + const Color markColor{ 179, 77, 128, 255 }; // was glColor3f(0.7f, 0.3f, 0.5f) + + DrawLine3D(Vector3{ g.x - 0.05f, g.y, g.z }, Vector3{ g.x + 0.05f, g.y, g.z }, markColor); + DrawLine3D(Vector3{ g.x, g.y - 0.05f, g.z }, Vector3{ g.x, g.y + 0.05f, g.z }, markColor); + + DrawLine3D(Vector3{ g.x - 0.01f, g.y, g.z + h }, Vector3{ g.x + 0.01f, g.y, g.z + h }, markColor); + DrawLine3D(Vector3{ g.x, g.y - 0.01f, g.z + h }, Vector3{ g.x, g.y + 0.01f, g.z + h }, markColor); + + DrawLine3D(g, Vector3{ g.x, g.y, g.z + h }, markColor); + + const Color connectorColor{ 0, 77, 153, 255 }; // was glColor3f(0.0f, 0.3f, 0.6f) + DrawLine3D( + Vector3{ static_cast(c.x()), static_cast(c.y()), static_cast(c.z()) }, + Vector3{ g.x, g.y, g.z + h }, + connectorColor); + + if (ground_control_points.draw_uncertainty) + { + Eigen::Matrix3d covar = Eigen::Matrix3d::Zero(); + covar(0, 0) = gcp.sigma_x * gcp.sigma_x; + covar(1, 1) = gcp.sigma_y * gcp.sigma_y; + covar(2, 2) = gcp.sigma_z * gcp.sigma_z; + + Eigen::Vector3d mean(gcp.x, gcp.y, gcp.z + h); + drawUncertaintyEllipse(covar, mean, GRAY); // was Eigen::Vector3f(0.5, 0.5, 0.5) + } + } +} + +// Was GNSS::render() (core/src/gnss.cpp) -- legacy-GL, compiled once into +// `core` and shared with the remaining GLUT apps, so it can't be touched; +// reimplemented here with rlgl's rl*() legacy-emulation API. GL_LINE_STRIP +// has no rlBegin() equivalent (only RL_LINES/RL_TRIANGLES/RL_QUADS -- see +// drawUncertaintyEllipse()'s comment for the same constraint elsewhere in +// this file), so the polyline is emitted as one RL_LINES segment per +// consecutive pair of poses instead. No text labels here (the original had +// none), so unlike GroundControlPoints this needs no separate 2D-pass +// function. +void renderGNSS(const GNSS& gnss, const PointClouds& point_clouds_container) +{ + if (gnss.gnss_poses.size() >= 2) + { + rlBegin(RL_LINES); + rlColor3f(1.0f, 1.0f, 1.0f); + for (size_t i = 0; i + 1 < gnss.gnss_poses.size(); ++i) + { + const auto& a = gnss.gnss_poses[i]; + const auto& b = gnss.gnss_poses[i + 1]; + rlVertex3f( + static_cast(a.enu_x - point_clouds_container.offset.x()), + static_cast(a.enu_y - point_clouds_container.offset.y()), + static_cast(a.enu_z - point_clouds_container.offset.z())); + rlVertex3f( + static_cast(b.enu_x - point_clouds_container.offset.x()), + static_cast(b.enu_y - point_clouds_container.offset.y()), + static_cast(b.enu_z - point_clouds_container.offset.z())); + } + rlEnd(); + } + + if (gnss.show_correspondences) + { + rlBegin(RL_LINES); + rlColor3f(1.0f, 0.0f, 0.0f); + for (const auto& pc : point_clouds_container.point_clouds) + { + for (size_t i = 0; i < gnss.gnss_poses.size(); ++i) + { + double time_stamp = gnss.gnss_poses[i].timestamp; + + auto it = std::lower_bound( + pc.local_trajectory.begin(), + pc.local_trajectory.end(), + time_stamp, + [](const PointCloud::LocalTrajectoryNode& lhs, const double& time) -> bool + { return lhs.timestamps.first < time; }); + + size_t index = static_cast(it - pc.local_trajectory.begin()); + + if (index > 0 && index < pc.local_trajectory.size()) + { + if (fabs(time_stamp - pc.local_trajectory[index].timestamps.first) < 10e12) + { + auto m = pc.m_pose * pc.local_trajectory[index].m_pose; + rlVertex3f(static_cast(m(0, 3)), static_cast(m(1, 3)), static_cast(m(2, 3))); + + rlVertex3f( + static_cast(gnss.gnss_poses[i].enu_x - point_clouds_container.offset.x()), + static_cast(gnss.gnss_poses[i].enu_y - point_clouds_container.offset.y()), + static_cast(gnss.gnss_poses[i].enu_z - point_clouds_container.offset.z())); + } + } + } + } + rlEnd(); + } +} + +// Was ControlPoints::render() (core/src/control_points.cpp) -- legacy-GL, +// compiled once into `core` and shared with the remaining GLUT apps, so it +// can't be touched; reimplemented here. Two parts, like the original's +// show_pc flag: +// - While editing (control_points.is_imgui), the bulk multi-scan +// scan_renderer.draw() call in display() is skipped entirely (see this +// function's caller), so the original's per-point GL_POINTS draw of just +// the active (index_pose) scan, colored by intensity, is replaced here +// by restricting a scan_renderer.draw() call to that one scan instead of +// reimplementing per-point immediate-mode drawing (rlBegin() has no +// RL_POINTS mode -- see drawUncertaintyEllipse()'s comment for the same +// rlBegin() constraint elsewhere in this file). Its GL_LINE_STRIP +// trajectory becomes RL_LINES segments, same reasoning. +// - The per-control-point crosshair/connector/ellipse markers, drawn +// unconditionally in the original (both show_pc branches called this), +// are always drawn regardless of is_imgui -- text labels are handled +// separately by renderControlPointsLabels(). +void renderControlPoints(const ControlPoints& control_points, PointClouds& point_clouds_container) +{ + auto& pointClouds = point_clouds_container.point_clouds; + + if (control_points.is_imgui && control_points.index_pose >= 0 && + static_cast(control_points.index_pose) < pointClouds.size()) + { + std::vector wasVisible(pointClouds.size()); + for (size_t i = 0; i < pointClouds.size(); ++i) + { + wasVisible[i] = pointClouds[i].visible; + pointClouds[i].visible = (static_cast(i) == control_points.index_pose); + } + + scan_renderer.draw( + pointClouds, + static_cast(point_size), + ScanColorMode::Intensity, + static_cast(session_dims.z_min), + static_cast(session_dims.z_max), + Eigen::Vector3d(rotation_center.x(), rotation_center.y(), rotation_center.z()), + static_cast(std::max({ session_dims.length, session_dims.width, session_dims.height, 1.0 })), + 1); + + for (size_t i = 0; i < pointClouds.size(); ++i) + { + pointClouds[i].visible = wasVisible[i]; + } + + const auto& activePc = pointClouds[control_points.index_pose]; + if (activePc.local_trajectory.size() >= 2) + { + rlBegin(RL_LINES); + rlColor3f(0.0f, 1.0f, 0.0f); + for (size_t i = 0; i + 1 < activePc.local_trajectory.size(); ++i) + { + auto poseA = activePc.m_pose * activePc.local_trajectory[i].m_pose; + auto poseB = activePc.m_pose * activePc.local_trajectory[i + 1].m_pose; + rlVertex3f(static_cast(poseA(0, 3)), static_cast(poseA(1, 3)), static_cast(poseA(2, 3))); + rlVertex3f(static_cast(poseB(0, 3)), static_cast(poseB(1, 3)), static_cast(poseB(2, 3))); + } + rlEnd(); + } + } + + const Color markColor{ 179, 77, 128, 255 }; // was glColor3f(0.7f, 0.3f, 0.5f) + const Color connectorColor{ 0, 77, 153, 255 }; // was glColor3f(0.0f, 0.3f, 0.6f) + + for (const auto& cp : control_points.cps) + { + if (cp.index_to_pose < 0 || static_cast(cp.index_to_pose) >= pointClouds.size()) + { + continue; + } + + Eigen::Vector3d p(cp.x_source_local, cp.y_source_local, cp.z_source_local); + Eigen::Vector3d c = pointClouds[cp.index_to_pose].m_pose * p; + Vector3 g{ static_cast(cp.x_target_global), static_cast(cp.y_target_global), + static_cast(cp.z_target_global) }; + + DrawLine3D(Vector3{ g.x - 0.05f, g.y, g.z }, Vector3{ g.x + 0.05f, g.y, g.z }, markColor); + DrawLine3D(Vector3{ g.x, g.y - 0.05f, g.z }, Vector3{ g.x, g.y + 0.05f, g.z }, markColor); + DrawLine3D(Vector3{ g.x - 0.01f, g.y, g.z }, Vector3{ g.x + 0.01f, g.y, g.z }, markColor); + DrawLine3D(Vector3{ g.x, g.y - 0.01f, g.z }, Vector3{ g.x, g.y + 0.01f, g.z }, markColor); + // Original's 5th line pair was glVertex3f(g,g) twice -- a + // degenerate zero-length segment that draws nothing. Dropped. + + DrawLine3D(Vector3{ static_cast(c.x()), static_cast(c.y()), static_cast(c.z()) }, g, connectorColor); + + if (control_points.draw_uncertainty) + { + Eigen::Matrix3d covar = Eigen::Matrix3d::Zero(); + if (cp.is_z_0) + { + covar(0, 0) = 0.01 * 0.01; + covar(1, 1) = 0.01 * 0.01; + } + else + { + covar(0, 0) = cp.sigma_x * cp.sigma_x; + covar(1, 1) = cp.sigma_y * cp.sigma_y; + } + covar(2, 2) = cp.sigma_z * cp.sigma_z; + + Eigen::Vector3d mean(cp.x_target_global, cp.y_target_global, cp.z_target_global); + drawUncertaintyEllipse(covar, mean, GRAY); // was Eigen::Vector3f(0.5, 0.5, 0.5) + } + } +} + // Was ManualPoseGraphLoopClosure::Render()'s per-pose glRasterPos3f + // glutBitmapString(std::to_string(i)) labels (one per point cloud, plus one // per edge at its flagpole top) -- neither has an rlgl/raylib equivalent @@ -2559,11 +2850,15 @@ namespace // Plain DrawText at a point sitting exactly on top of a same-size, often // same-color trajectory marker is easy to lose visually -- outlined in // black and nudged up-right of the anchor so it reads clearly regardless - // of what's directly underneath. - void drawOutlinedText(const char* text, Vector2 anchor, int fontSize, Color color) + // of what's directly underneath. `line` stacks additional labels above + // the same anchor (one line height per unit) -- needed wherever several + // labels sit at world points too close together to separate on screen + // by their 3D position alone (e.g. GCP's LiDAR-center/ground-plane/name + // labels, which differ by only lidar_height_above_ground/0.1m). + void drawOutlinedText(const char* text, Vector2 anchor, int fontSize, Color color, int line = 0) { int x = static_cast(anchor.x) + 6; - int y = static_cast(anchor.y) - fontSize - 6; + int y = static_cast(anchor.y) - fontSize - 6 - line * (fontSize + 4); for (int dx = -1; dx <= 1; ++dx) { for (int dy = -1; dy <= 1; ++dy) @@ -2576,25 +2871,12 @@ namespace } DrawText(text, x, y, fontSize, color); } -} // namespace -void renderLoopClosureLabels(PointClouds& point_clouds_container) -{ - auto& pointClouds = point_clouds_container.point_clouds; - - // io.DisplaySize, not GetScreenWidth()/GetScreenHeight(): must match - // whatever reshape() last set the actual GL viewport to (see - // end3DMatrixStack()'s comment) -- the two can differ under DPI - // scaling, which would silently throw this off-screen. - ImGuiIO& io = ImGui::GetIO(); - float screenW = io.DisplaySize.x; - float screenH = io.DisplaySize.y; - - auto worldToScreen = [screenW, screenH](const Eigen::Vector3d& world) -> Vector2 + // Manual clip-space transform (mat * [x,y,z,1]^T) -- raymath's + // Vector3Transform computes the same x/y/z but drops w, which the + // perspective divide below needs, so it can't be reused here. + Vector2 worldToScreen(const Eigen::Vector3d& world, float screenW, float screenH) { - // Manual clip-space transform (mat * [x,y,z,1]^T) -- raymath's - // Vector3Transform computes the same x/y/z but drops w, which the - // perspective divide below needs, so it can't be reused here. const Matrix& m = frame_mvp_3d; float x = static_cast(world.x()); float y = static_cast(world.y()); @@ -2609,11 +2891,24 @@ void renderLoopClosureLabels(PointClouds& point_clouds_container) float ndcX = clipX / clipW; float ndcY = clipY / clipW; return Vector2{ (ndcX * 0.5f + 0.5f) * screenW, (1.0f - (ndcY * 0.5f + 0.5f)) * screenH }; - }; + } +} // namespace + +void renderLoopClosureLabels(PointClouds& point_clouds_container) +{ + auto& pointClouds = point_clouds_container.point_clouds; + + // io.DisplaySize, not GetScreenWidth()/GetScreenHeight(): must match + // whatever reshape() last set the actual GL viewport to (see + // end3DMatrixStack()'s comment) -- the two can differ under DPI + // scaling, which would silently throw this off-screen. + ImGuiIO& io = ImGui::GetIO(); + float screenW = io.DisplaySize.x; + float screenH = io.DisplaySize.y; for (size_t i = 0; i < pointClouds.size(); ++i) { - Vector2 screen = worldToScreen(pointClouds[i].m_pose.translation()); + Vector2 screen = worldToScreen(pointClouds[i].m_pose.translation(), screenW, screenH); drawOutlinedText(TextFormat("%d", static_cast(i)), screen, 20, WHITE); } @@ -2630,12 +2925,103 @@ void renderLoopClosureLabels(PointClouds& point_clouds_container) Eigen::Vector3d worldTrg = pointClouds[edge.index_to].m_pose.translation(); Eigen::Vector3d midUp = (worldSrc + worldTrg) * 0.5 + Eigen::Vector3d(0, 0, 10); - Vector2 screen = worldToScreen(midUp); + Vector2 screen = worldToScreen(midUp, screenW, screenH); Color c = (static_cast(i) == session.pose_graph_loop_closure.index_active_edge) ? RED : SKYBLUE; drawOutlinedText(TextFormat("%d", static_cast(i)), screen, 22, c); } } +// Was GroundControlPoints::render()'s glRasterPos3f + glutBitmapString +// calls (core/src/ground_control_points.cpp) -- same reasoning as +// renderLoopClosureLabels() (no matrix-anchored bitmap fonts under core +// profile), projected to screen space and drawn with DrawText instead. +// Must run after end3DMatrixStack(), like renderLoopClosureLabels(). +void renderGroundControlPointsLabels(const GroundControlPoints& ground_control_points, const PointClouds& point_clouds_container) +{ + ImGuiIO& io = ImGui::GetIO(); + float screenW = io.DisplaySize.x; + float screenH = io.DisplaySize.y; + + const Color markColor{ 179, 77, 128, 255 }; // was glColor3f(0.7f, 0.3f, 0.5f) + const Color connectorColor{ 0, 77, 153, 255 }; // was glColor3f(0.0f, 0.3f, 0.6f) + + for (size_t i = 0; i < ground_control_points.gpcs.size(); ++i) + { + const auto& gcp = ground_control_points.gpcs[i]; + + // LiDAR center (z+h), ground plane (z) and name (z+h+0.1) sit + // world-space centimeters apart -- at any normal zoom that's the + // same handful of screen pixels, so unlike renderLoopClosureLabels() + // (one label per anchor) these three share a single screen anchor + // and stack via drawOutlinedText's `line` instead of relying on + // their (invisible-on-screen) 3D separation. + Vector2 anchor = worldToScreen(Eigen::Vector3d(gcp.x, gcp.y, gcp.z), screenW, screenH); + + // was glColor3f(0, 0, 0) -- plain black text with no outline in the + // GLUT original; drawOutlinedText always outlines in black, so + // black text would vanish. WHITE instead, matching + // renderLoopClosureLabels()'s index labels. + drawOutlinedText(gcp.name, anchor, 22, WHITE, 2); + drawOutlinedText(TextFormat("GCP_%d: LiDAR center", static_cast(i)), anchor, 14, markColor, 1); + drawOutlinedText(TextFormat("GCP_%d: 'plane on the ground'", static_cast(i)), anchor, 14, markColor, 0); + + if (gcp.index_to_node_inner < 0 || + static_cast(gcp.index_to_node_inner) >= point_clouds_container.point_clouds.size()) + { + continue; + } + const auto& pc = point_clouds_container.point_clouds[gcp.index_to_node_inner]; + if (gcp.index_to_node_outer < 0 || static_cast(gcp.index_to_node_outer) >= pc.local_trajectory.size()) + { + continue; + } + + Eigen::Vector3d c = pc.m_pose * pc.local_trajectory[gcp.index_to_node_outer].m_pose.translation(); + Vector2 nodeScreen = worldToScreen(c, screenW, screenH); + drawOutlinedText(TextFormat("GCP_%d: assigned trajectory node", static_cast(i)), nodeScreen, 14, connectorColor); + } +} + +// Was ControlPoints::render()'s glRasterPos3f + glutBitmapString calls +// (core/src/control_points.cpp) -- same reasoning as +// renderGroundControlPointsLabels() (no matrix-anchored bitmap fonts under +// core profile, and name/"CP_i" sit only 0.1m apart in world space -- too +// close to separate on screen at normal zoom -- so they share one anchor +// and stack via drawOutlinedText's `line`). Must run after +// end3DMatrixStack(), like renderGroundControlPointsLabels(). +void renderControlPointsLabels(const ControlPoints& control_points, const PointClouds& point_clouds_container) +{ + ImGuiIO& io = ImGui::GetIO(); + float screenW = io.DisplaySize.x; + float screenH = io.DisplaySize.y; + + const Color markColor{ 179, 77, 128, 255 }; // was glColor3f(0.7f, 0.3f, 0.5f) + + for (size_t i = 0; i < control_points.cps.size(); ++i) + { + const auto& cp = control_points.cps[i]; + + Vector2 anchor = worldToScreen(Eigen::Vector3d(cp.x_target_global, cp.y_target_global, cp.z_target_global), screenW, screenH); + + // was glColor3f(0, 0, 0) -- plain black text with no outline in the + // GLUT original; drawOutlinedText always outlines in black, so + // black text would vanish. WHITE instead, matching + // renderGroundControlPointsLabels()'s name label. + drawOutlinedText(cp.name, anchor, 22, WHITE, 1); + drawOutlinedText(TextFormat("CP_%d", static_cast(i)), anchor, 14, WHITE, 0); + + if (cp.index_to_pose < 0 || static_cast(cp.index_to_pose) >= point_clouds_container.point_clouds.size()) + { + continue; + } + + Eigen::Vector3d p(cp.x_source_local, cp.y_source_local, cp.z_source_local); + Eigen::Vector3d c = point_clouds_container.point_clouds[cp.index_to_pose].m_pose * p; + Vector2 sourceScreen = worldToScreen(c, screenW, screenH); + drawOutlinedText(TextFormat("CP_%d: initial location", static_cast(i)), sourceScreen, 14, markColor); + } +} + void display() { // Safety net: rebuilds any scan whose m_pose no longer matches its @@ -2737,18 +3123,41 @@ void display() else updateOrthoView(); - frame_mvp_3d = MatrixMultiply(rlGetMatrixModelview(), rlGetMatrixProjection()); + frame_view_3d = rlGetMatrixModelview(); + frame_proj_3d = rlGetMatrixProjection(); + frame_mvp_3d = MatrixMultiply(frame_view_3d, frame_proj_3d); showAxes(); - // GNSS/GroundControlPoints/ControlPoints 3D rendering (.render()) is - // legacy-GL, compiled once into `core` and shared with the remaining - // GLUT apps -- not yet ported to raylib (their ImGui panels below, - // .imgui()/.Gui(), are pure ImGui/data and still fully work). Loop - // closure's 3D rendering (below) is ported since it was this session's - // actual focus. + // renderLoopClosure() hides every scan except the current source/target + // range while loop closure editing is active (see its comment) -- + // restore full visibility the moment the panel closes (X button, menu + // toggle, or Ctrl+L), or every scan but that last-shown range stays + // hidden. Checked unconditionally (not just when the below block runs) + // so this still fires even if control_points.is_imgui happens to be + // true on the closing frame. + static bool was_loop_closure_gui = false; + if (was_loop_closure_gui && !is_loop_closure_gui) + { + for (auto& pc : session.point_clouds_container.point_clouds) + { + pc.visible = true; + } + } + was_loop_closure_gui = is_loop_closure_gui; + + // renderControlPoints() draws its markers regardless of is_imgui (like + // the original's render() -- both its show_pc=true/false call sites + // drew them) and, while is_imgui is true, also substitutes for the bulk + // scan_renderer.draw() call below (skipped entirely in that case -- see + // its own comment). + renderControlPoints(session.control_points, session.point_clouds_container); + if (!session.control_points.is_imgui) { + renderGroundControlPoints(session.ground_control_points, session.point_clouds_container); + renderGNSS(tls_registration.gnss, session.point_clouds_container); + if (is_loop_closure_gui) renderLoopClosure( session.point_clouds_container, @@ -4307,6 +4716,11 @@ void display() if (is_loop_closure_gui) renderLoopClosureLabels(session.point_clouds_container); + if (!session.control_points.is_imgui) + renderGroundControlPointsLabels(session.ground_control_points, session.point_clouds_container); + + renderControlPointsLabels(session.control_points, session.point_clouds_container); + if (compass_ruler) drawMiniCompassWithRuler(); @@ -4609,7 +5023,7 @@ void mouse(int glut_button, int state, int x, int y) double dist = distance_point_to_line(vp, laser_beam); - if (dist < min_distance && dist < 0.1) + if (dist < min_distance) { min_distance = dist; diff --git a/apps/multi_view_tls_registration/rl_utils.cpp b/apps/multi_view_tls_registration/rl_utils.cpp index 9d78ac11..902fec9b 100644 --- a/apps/multi_view_tls_registration/rl_utils.cpp +++ b/apps/multi_view_tls_registration/rl_utils.cpp @@ -101,6 +101,9 @@ bool glLineWidthSupport = true; float m_ortho_projection[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; float m_ortho_gizmo_view[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; +Matrix frame_view_3d = MatrixIdentity(); +Matrix frame_proj_3d = MatrixIdentity(); + // ============================================================================ // Formerly core/src/utils.cpp -- local now (see the big comment at the top // of this file for why). Everywhere the original was pure ImGui/Eigen/GLM @@ -1089,8 +1092,8 @@ LaserBeam GetLaserBeam(int x, int y) float ndcX = (2.0f * (float)x) / (float)width - 1.0f; float ndcY = 1.0f - (2.0f * (float)y) / (float)height; - Matrix matView = rlGetMatrixModelview(); - Matrix matProj = rlGetMatrixProjection(); + Matrix matView = frame_view_3d; + Matrix matProj = frame_proj_3d; Vector3 nearPoint = Vector3Unproject(Vector3{ ndcX, ndcY, 0.0f }, matProj, matView); Vector3 farPoint = Vector3Unproject(Vector3{ ndcX, ndcY, 1.0f }, matProj, matView); @@ -1098,6 +1101,7 @@ LaserBeam GetLaserBeam(int x, int y) LaserBeam laser_beam; laser_beam.position = Eigen::Vector3d(nearPoint.x, nearPoint.y, nearPoint.z); laser_beam.direction = Eigen::Vector3d(farPoint.x - nearPoint.x, farPoint.y - nearPoint.y, farPoint.z - nearPoint.z); + return laser_beam; } diff --git a/apps/multi_view_tls_registration/rl_utils.h b/apps/multi_view_tls_registration/rl_utils.h index f483f3af..32d1a323 100644 --- a/apps/multi_view_tls_registration/rl_utils.h +++ b/apps/multi_view_tls_registration/rl_utils.h @@ -15,6 +15,8 @@ // is multi_view_tls_registration's own local header, analogous to how // core/src/utils.cpp served the same role for the GLUT apps. +#include "raylib.h" + #include #include @@ -109,6 +111,17 @@ extern float new_translate_z; // Transition timing extern bool camera_transition_active; +// The 3D view/projection rlgl had active during this frame's scene render, +// cached by display() right before end3DMatrixStack() resets rlgl's matrix +// stack to the 2D screen-space ortho used for the mini-compass/ImGui pass. +// GetLaserBeam() (called from mouse(), which runs *before* display() each +// frame -- see main()) needs these: querying rlGetMatrixModelview()/ +// rlGetMatrixProjection() live at that point would still see the previous +// frame's post-end3DMatrixStack() state (identity modelview, 2D ortho +// projection), not the 3D camera, producing a meaningless pick ray. +extern Matrix frame_view_3d; +extern Matrix frame_proj_3d; + // Unlike the original (which probed GL_LINE_WIDTH_RANGE), rlgl's line width // support is uniform enough here not to need a runtime check -- always true. extern bool glLineWidthSupport; From 48a5acb2d7a46b88d0f69e1df0c5003f4a734e26 Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Mon, 27 Jul 2026 02:21:55 +0200 Subject: [PATCH 15/17] Apply clang-format to the raylib picking/GCP/GNSS/ControlPoints port Co-Authored-By: Claude Sonnet 5 --- .../multi_view_tls_registration_gui.cpp | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp b/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp index 920fdf00..390e3187 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 @@ -2617,8 +2617,7 @@ void renderGroundControlPoints(const GroundControlPoints& ground_control_points, { for (const auto& gcp : ground_control_points.gpcs) { - if (gcp.index_to_node_inner < 0 || - static_cast(gcp.index_to_node_inner) >= point_clouds_container.point_clouds.size()) + if (gcp.index_to_node_inner < 0 || static_cast(gcp.index_to_node_inner) >= point_clouds_container.point_clouds.size()) { continue; } @@ -2707,7 +2706,9 @@ void renderGNSS(const GNSS& gnss, const PointClouds& point_clouds_container) pc.local_trajectory.end(), time_stamp, [](const PointCloud::LocalTrajectoryNode& lhs, const double& time) -> bool - { return lhs.timestamps.first < time; }); + { + return lhs.timestamps.first < time; + }); size_t index = static_cast(it - pc.local_trajectory.begin()); @@ -2751,8 +2752,7 @@ void renderControlPoints(const ControlPoints& control_points, PointClouds& point { auto& pointClouds = point_clouds_container.point_clouds; - if (control_points.is_imgui && control_points.index_pose >= 0 && - static_cast(control_points.index_pose) < pointClouds.size()) + if (control_points.is_imgui && control_points.index_pose >= 0 && static_cast(control_points.index_pose) < pointClouds.size()) { std::vector wasVisible(pointClouds.size()); for (size_t i = 0; i < pointClouds.size(); ++i) @@ -2804,8 +2804,7 @@ void renderControlPoints(const ControlPoints& control_points, PointClouds& point Eigen::Vector3d p(cp.x_source_local, cp.y_source_local, cp.z_source_local); Eigen::Vector3d c = pointClouds[cp.index_to_pose].m_pose * p; - Vector3 g{ static_cast(cp.x_target_global), static_cast(cp.y_target_global), - static_cast(cp.z_target_global) }; + Vector3 g{ static_cast(cp.x_target_global), static_cast(cp.y_target_global), static_cast(cp.z_target_global) }; DrawLine3D(Vector3{ g.x - 0.05f, g.y, g.z }, Vector3{ g.x + 0.05f, g.y, g.z }, markColor); DrawLine3D(Vector3{ g.x, g.y - 0.05f, g.z }, Vector3{ g.x, g.y + 0.05f, g.z }, markColor); @@ -2965,8 +2964,7 @@ void renderGroundControlPointsLabels(const GroundControlPoints& ground_control_p drawOutlinedText(TextFormat("GCP_%d: LiDAR center", static_cast(i)), anchor, 14, markColor, 1); drawOutlinedText(TextFormat("GCP_%d: 'plane on the ground'", static_cast(i)), anchor, 14, markColor, 0); - if (gcp.index_to_node_inner < 0 || - static_cast(gcp.index_to_node_inner) >= point_clouds_container.point_clouds.size()) + if (gcp.index_to_node_inner < 0 || static_cast(gcp.index_to_node_inner) >= point_clouds_container.point_clouds.size()) { continue; } From 05899cf8c1ccc8882822d9db0bcaef5d8a32c385 Mon Sep 17 00:00:00 2001 From: Michal Date: Thu, 30 Jul 2026 00:02:12 +0200 Subject: [PATCH 16/17] Fixes on MacOs Signed-off-by: Michal --- .../multi_view_tls_registration_gui.cpp | 42 ++++++++++++++++++- apps/multi_view_tls_registration/rl_utils.cpp | 5 ++- 2 files changed, 44 insertions(+), 3 deletions(-) diff --git a/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp b/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp index 390e3187..1800976d 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 @@ -3030,7 +3030,13 @@ void display() scan_renderer.syncPoses(session.point_clouds_container.point_clouds); ImGuiIO& io = ImGui::GetIO(); - rlViewport(0, 0, (int)io.DisplaySize.x, (int)io.DisplaySize.y); + // GetRenderWidth/Height(), not io.DisplaySize: the GL viewport must be + // sized in actual framebuffer pixels, which on a Retina Mac are a + // multiple of io.DisplaySize's logical points (see initGL()'s + // FLAG_WINDOW_HIGHDPI comment) -- using io.DisplaySize directly here + // left the 3D scene rendered only into the bottom-left quarter of the + // window, the rest showing just the clear color. + rlViewport(0, 0, GetRenderWidth(), GetRenderHeight()); ClearBackground(ColorFromNormalized(Vector4{ bg_color.x * bg_color.w, bg_color.y * bg_color.w, bg_color.z * bg_color.w, bg_color.w })); rlEnableDepthTest(); @@ -5124,10 +5130,42 @@ bool initGL(int* argc, char** argv, const std::string& winTitleArg, void (*)(), // context: BadValue"), and raylib/GLFW then segfaults using the broken // context instead of falling back cleanly. Not worth the crash risk for // a cosmetic antialiasing hint. - SetConfigFlags(FLAG_WINDOW_RESIZABLE); + // + // FLAG_WINDOW_HIGHDPI: macOS/GLFW always backs the window with a + // full-resolution Retina framebuffer (rglfw.c's _GLFW_USE_RETINA), + // regardless of this flag -- but rlImGui only reads that real scale + // factor into io.DisplayFramebufferScale (and scales its font atlas + // and mouse coordinates to match) when FLAG_WINDOW_HIGHDPI is set. + // Without it, on a Retina display ImGui assumes 1:1 and everything it + // draws ends up scaled/positioned for a framebuffer a quarter the + // actual size -- menus stretched into oversized black bars, widgets + // rendered as solid blocks from a mis-sampled font atlas. + SetConfigFlags(FLAG_WINDOW_RESIZABLE | FLAG_WINDOW_HIGHDPI); InitWindow(static_cast(window_width), static_cast(window_height), winTitleArg.c_str()); SetTargetFPS(60); + // The hardcoded window_width/window_height default (1600x900) can be + // wider and/or taller than the actual screen (e.g. a 13" MacBook's + // 1470x956-point default-scaled display) -- when it doesn't fit, macOS + // shifts the window up/left to keep it on screen, which can tuck the + // very top of the content area (where ImGui's main menu bar lives) + // behind the OS menu bar/title bar instead of below it. Shrinking to + // fit the monitor's work area and recentering avoids that; on screens + // that already fit the default size this is a no-op. + { + const int monitor = GetCurrentMonitor(); + const int monitorWidth = GetMonitorWidth(monitor); + const int monitorHeight = GetMonitorHeight(monitor); + const int margin = 100; // room for the OS title bar, menu bar and dock + const int fitWidth = (monitorWidth > 0) ? std::min(static_cast(window_width), monitorWidth - margin) : static_cast(window_width); + const int fitHeight = (monitorHeight > 0) ? std::min(static_cast(window_height), monitorHeight - margin) : static_cast(window_height); + if (fitWidth != static_cast(window_width) || fitHeight != static_cast(window_height)) + { + SetWindowSize(fitWidth, fitHeight); + SetWindowPosition((monitorWidth - fitWidth) / 2, (monitorHeight - fitHeight) / 2); + } + } + rlImGuiSetup(true); ImGuiIO& io = ImGui::GetIO(); io.ConfigFlags |= ImGuiConfigFlags_NavEnableKeyboard | ImGuiConfigFlags_NavEnableGamepad | ImGuiConfigFlags_DockingEnable; diff --git a/apps/multi_view_tls_registration/rl_utils.cpp b/apps/multi_view_tls_registration/rl_utils.cpp index 902fec9b..10841d91 100644 --- a/apps/multi_view_tls_registration/rl_utils.cpp +++ b/apps/multi_view_tls_registration/rl_utils.cpp @@ -205,7 +205,10 @@ void wheel(int button, int dir, int x, int y) // rlFrustum() call by hand (standard fovy -> frustum-bounds formula). void reshape(int w, int h) { - rlViewport(0, 0, (int)w, (int)h); + // GetRenderWidth/Height(), not w/h: see display()'s matching comment -- + // w/h are logical points (window size), the GL viewport needs actual + // framebuffer pixels. + rlViewport(0, 0, GetRenderWidth(), GetRenderHeight()); rlMatrixMode(RL_PROJECTION); rlLoadIdentity(); if (!is_ortho) From 0a1469e3ae016ac685fd26ff9d5ee2d4ed520baf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Thu, 30 Jul 2026 13:48:23 +0200 Subject: [PATCH 17/17] Fix clion MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Michał Pełka --- .../multi_view_tls_registration_gui.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp b/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp index 1800976d..76ee6e65 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 @@ -5157,8 +5157,10 @@ bool initGL(int* argc, char** argv, const std::string& winTitleArg, void (*)(), const int monitorWidth = GetMonitorWidth(monitor); const int monitorHeight = GetMonitorHeight(monitor); const int margin = 100; // room for the OS title bar, menu bar and dock - const int fitWidth = (monitorWidth > 0) ? std::min(static_cast(window_width), monitorWidth - margin) : static_cast(window_width); - const int fitHeight = (monitorHeight > 0) ? std::min(static_cast(window_height), monitorHeight - margin) : static_cast(window_height); + const int fitWidth = + (monitorWidth > 0) ? std::min(static_cast(window_width), monitorWidth - margin) : static_cast(window_width); + const int fitHeight = + (monitorHeight > 0) ? std::min(static_cast(window_height), monitorHeight - margin) : static_cast(window_height); if (fitWidth != static_cast(window_width) || fitHeight != static_cast(window_height)) { SetWindowSize(fitWidth, fitHeight);