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: diff --git a/CMakeLists.txt b/CMakeLists.txt index 1fe58065..4a19ba7f 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,10 +113,11 @@ 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) +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/README.md b/README.md index 422e0362..f350e52e 100644 --- a/README.md +++ b/README.md @@ -61,6 +61,18 @@ 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 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 +``` + ## Quick Start (macOS) **Prerequisites:** diff --git a/apps/multi_view_tls_registration/CMakeLists.txt b/apps/multi_view_tls_registration/CMakeLists.txt index 495300ff..58e7e19e 100644 --- a/apps/multi_view_tls_registration/CMakeLists.txt +++ b/apps/multi_view_tls_registration/CMakeLists.txt @@ -3,11 +3,27 @@ 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: 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), 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* +# 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 + rl_utils.cpp rl_utils.h ../lidar_odometry_step_1/lidar_odometry_utils.cpp - "../../core/src/utils.cpp" ) # Windows: add resource file @@ -25,12 +41,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 +60,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 +94,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..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 @@ -1,10 +1,29 @@ +#include #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 "external/glad.h" +#include "raylib.h" +#include "raymath.h" +#include "rlImGui.h" +#include "rlgl.h" #include -#include -#include #include #include @@ -25,12 +44,35 @@ #include #include #include +#include #include #include +#include #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 @@ -49,13 +91,66 @@ #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 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" -#include +#endif +// 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" + +#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 +// 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 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); +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); + +/////////////////////////////////////////////////////////////////////////////////// + #ifdef _WIN32 bool consWin = true; #endif @@ -228,9 +323,30 @@ 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; +// 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 +1644,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 +1786,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,17 +2335,714 @@ void settings_gui() ImGui::End(); } -void display() +// 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 +// 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.005f, fabsf(translate_z) * 0.001f); + 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 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 +// (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). +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. `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 - line * (fontSize + 4); + 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); + } + + // 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) + { + 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) * 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(), screenW, screenH); + drawOutlinedText(TextFormat("%d", static_cast(i)), screen, 20, WHITE); + } + + 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, 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(); - glViewport(0, 0, (GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); + 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]; - 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); + Vector2 anchor = worldToScreen(Eigen::Vector3d(cp.x_target_global, cp.y_target_global, cp.z_target_global), screenW, screenH); - glMatrixMode(GL_PROJECTION); - glLoadIdentity(); + // 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 + // 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(); + // 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(); + + rlMatrixMode(RL_PROJECTION); + rlLoadIdentity(); float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); updateCameraTransition(); @@ -2307,28 +3122,53 @@ void display() viewLocal.translate(-rotation_center); - glLoadMatrixf(viewLocal.matrix().data()); + rlMultMatrixf(viewLocal.matrix().data()); } else updateOrthoView(); + frame_view_3d = rlGetMatrixModelview(); + frame_proj_3d = rlGetMatrixProjection(); + frame_mvp_3d = MatrixMultiply(frame_view_3d, frame_proj_3d); + showAxes(); - if (session.control_points.is_imgui) - session.control_points.render(session.point_clouds_container, true); - else + // 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) - session.pose_graph_loop_closure.Render( + renderLoopClosure( 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; @@ -2357,9 +3197,15 @@ void display() camera_transition_active = true; } - ImGui_ImplOpenGL2_NewFrame(); - ImGui_ImplGLUT_NewFrame(); - ImGui::NewFrame(); + // 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(); @@ -2391,11 +3237,17 @@ void display() if (!is_ortho) { - GLfloat projection[16]; - glGetFloatv(GL_PROJECTION_MATRIX, projection); - - GLfloat modelview[16]; - glGetFloatv(GL_MODELVIEW_MATRIX, modelview); + // 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], @@ -2475,15 +3327,24 @@ 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; - - observation_picking.render(); + // 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); + + 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 +3364,23 @@ 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 +3395,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 +3428,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 +4423,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(); } @@ -3723,7 +4613,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(); @@ -3747,10 +4637,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); @@ -3799,7 +4703,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 +4712,23 @@ 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 (!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(); - ImGui::Render(); - ImGui_ImplOpenGL2_RenderDrawData(ImGui::GetDrawData()); - - glutSwapBuffers(); - glutPostRedisplay(); + rlImGuiEnd(); } void draw_translate_preview() @@ -3859,21 +4772,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 +4840,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 +4848,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 +4904,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 +4930,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 +4958,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) { @@ -4103,7 +5027,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; @@ -4185,6 +5109,77 @@ 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; + + // 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. + // + // 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; + 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 +5217,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/apps/multi_view_tls_registration/rl_utils.cpp b/apps/multi_view_tls_registration/rl_utils.cpp new file mode 100644 index 00000000..10841d91 --- /dev/null +++ b/apps/multi_view_tls_registration/rl_utils.cpp @@ -0,0 +1,1284 @@ +#include "rl_utils.h" + +#include "external/glad.h" +#include "raylib.h" +#include "raymath.h" +#include "rlgl.h" + +#include + +#include + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +#ifdef _WIN32 +// 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 + +/////////////////////////////////////////////////////////////////////////////////// +// 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 }; + +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 +// (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) +{ + // 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) + { + 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, 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()); +#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 = 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); + + 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..32d1a323 --- /dev/null +++ b/apps/multi_view_tls_registration/rl_utils.h @@ -0,0 +1,180 @@ +#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 "raylib.h" + +#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; + +// 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; + +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(); 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 00000000..310689ec Binary files /dev/null and b/apps/multi_view_tls_registration_legacy/icon.ico differ diff --git a/apps/multi_view_tls_registration_legacy/multi_view_tls_registration.cpp b/apps/multi_view_tls_registration_legacy/multi_view_tls_registration.cpp new file mode 100644 index 00000000..a9473ea7 --- /dev/null +++ b/apps/multi_view_tls_registration_legacy/multi_view_tls_registration.cpp @@ -0,0 +1,878 @@ +#include "multi_view_tls_registration.h" +#include +#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 diff --git a/cmake/raylib.cmake b/cmake/raylib.cmake new file mode 100644 index 00000000..42147c52 --- /dev/null +++ b/cmake/raylib.cmake @@ -0,0 +1,79 @@ +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) +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 + 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..9406ae53 --- /dev/null +++ b/core/include/Core/raylib_render.hpp @@ -0,0 +1,195 @@ +#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; + + // 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 + // (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; + mutable int lastDrawCallCount_ = 0; + mutable int lastVertexCount_ = 0; +}; diff --git a/core/src/raylib_render.cpp b/core/src/raylib_render.cpp new file mode 100644 index 00000000..8231ab90 --- /dev/null +++ b/core/src/raylib_render.cpp @@ -0,0 +1,586 @@ +#include +#include + +#include "external/glad.h" +#include "raymath.h" +#include "rlgl.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 +{ + lastDrawCallCount_ = 0; + lastVertexCount_ = 0; + + 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); + ++lastDrawCallCount_; + lastVertexCount_ += 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.