From 8203aac61886d0e11fac920c61cf33da1f67e919 Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Thu, 22 Oct 2026 09:10:22 +0200 Subject: [PATCH 1/3] Port step 1 and the raw data viewer to raylib lidar_odometry_step_1 and mandeye_raw_data_viewer now run on raylib + rlImGui instead of GLUT + legacy OpenGL. The GLUT builds are kept as lidar_odometry_step_1_legacy and mandeye_raw_data_viewer_legacy, like step 2 and step 3. Step 3's raylib replacement for (same names, camera in raylib_widgets::OrbitCamera) moves into a shared core_raylib_utils library (Core/raylib_utils.hpp), so the ported apps keep their code and only their GL calls change. Step 3 keeps only its session-rendering helpers. New in the shared module: a per-vertex-color point batch (rlgl has no GL_POINTS), draw_ellipse, an ortho-aware reshape, intensity shading matching ScanRenderer's FlatIntensity mode, dropped-file handling, and the Windows taskbar progress (own TU, windows.h clashes with raylib.h). Both apps accept drag and drop (step 1: a data folder starts processing; raw viewer: folders/files load) and shade points by intensity (View menu toggle, on by default). implot_raylib builds ImPlot against imgui_raylib for the raw viewer's IMU plots. Wide lines are drawn 1 px (core profile). Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01UXL9MhpTVJ1uyhtztVmAoK --- CMakeLists.txt | 2 + apps/lidar_odometry_step_1/CMakeLists.txt | 26 +- .../lidar_odometry_gui.cpp | 327 +- .../CMakeLists.txt | 85 + apps/lidar_odometry_step_1_legacy/icon.ico | Bin 0 -> 20645 bytes .../lidar_odometry_gui.cpp | 2833 +++++++++++++++++ apps/lidar_odometry_step_1_legacy/resource.h | 17 + apps/lidar_odometry_step_1_legacy/resource.rc | 54 + apps/mandeye_raw_data_viewer/CMakeLists.txt | 20 +- .../mandeye_raw_data_viewer.cpp | 189 +- .../CMakeLists.txt | 79 + apps/mandeye_raw_data_viewer_legacy/icon.ico | Bin 0 -> 25645 bytes .../mandeye_raw_data_viewer.cpp | 2080 ++++++++++++ .../mandeye_raw_data_viewer_legacy/resource.h | 17 + .../resource.rc | 50 + .../multi_session_registration/CMakeLists.txt | 4 +- .../multi_session_registration.cpp | 1 + .../raylib_utils.cpp | 475 +-- .../multi_session_registration/raylib_utils.h | 115 +- cmake/raylib.cmake | 13 + core/CMakeLists.txt | 22 +- core/include/Core/raylib_utils.hpp | 155 + core/src/raylib_taskbar_win.cpp | 49 + core/src/raylib_utils.cpp | 686 ++++ deploy_mandeye.bat | 2 +- 25 files changed, 6435 insertions(+), 866 deletions(-) create mode 100644 apps/lidar_odometry_step_1_legacy/CMakeLists.txt create mode 100644 apps/lidar_odometry_step_1_legacy/icon.ico create mode 100644 apps/lidar_odometry_step_1_legacy/lidar_odometry_gui.cpp create mode 100644 apps/lidar_odometry_step_1_legacy/resource.h create mode 100644 apps/lidar_odometry_step_1_legacy/resource.rc create mode 100644 apps/mandeye_raw_data_viewer_legacy/CMakeLists.txt create mode 100644 apps/mandeye_raw_data_viewer_legacy/icon.ico create mode 100644 apps/mandeye_raw_data_viewer_legacy/mandeye_raw_data_viewer.cpp create mode 100644 apps/mandeye_raw_data_viewer_legacy/resource.h create mode 100644 apps/mandeye_raw_data_viewer_legacy/resource.rc create mode 100644 core/include/Core/raylib_utils.hpp create mode 100644 core/src/raylib_taskbar_win.cpp create mode 100644 core/src/raylib_utils.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 4428b2a8..cd0adb0d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -140,7 +140,9 @@ endif() # Applications for typical workflow - see images/workflow.png add_subdirectory(apps/mandeye_raw_data_viewer) +add_subdirectory(apps/mandeye_raw_data_viewer_legacy) add_subdirectory(apps/lidar_odometry_step_1) +add_subdirectory(apps/lidar_odometry_step_1_legacy) add_subdirectory(apps/mandeye_single_session_viewer) add_subdirectory(apps/multi_view_tls_registration) add_subdirectory(apps/multi_view_tls_registration_legacy) diff --git a/apps/lidar_odometry_step_1/CMakeLists.txt b/apps/lidar_odometry_step_1/CMakeLists.txt index 60a1a956..30446b3b 100644 --- a/apps/lidar_odometry_step_1/CMakeLists.txt +++ b/apps/lidar_odometry_step_1/CMakeLists.txt @@ -24,7 +24,6 @@ set(SOURCES points_global_spill.h points_global_spill.cpp raw_cloud_loader.h raw_cloud_loader.cpp toml_io.h toml_io.cpp - "../../core/src/utils.cpp" ) # Windows: add resource file @@ -39,6 +38,9 @@ add_executable( target_compile_definitions(lidar_odometry_step_1 PRIVATE WITH_GUI=1) target_apply_hdmapping_profiler(lidar_odometry_step_1) +# lidar_odometry_gui.cpp is raylib-based (ported off GLUT; the GLUT build is kept as +# apps/lidar_odometry_step_1_legacy): (core_raylib_utils) replaces +# with the same names, so only the GL calls changed. target_include_directories( lidar_odometry_step_1 PRIVATE include @@ -48,11 +50,6 @@ target_include_directories( ${THIRDPARTY_DIRECTORY}/glm ${EIGEN3_INCLUDE_DIR} ${THIRDPARTY_DIRECTORY}/tomlplusplus/include - ${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 @@ -62,19 +59,18 @@ target_include_directories( target_link_libraries( lidar_odometry_step_1 - PRIVATE - vqf + PRIVATE + # core_raylib_utils brings in core_raylib (core, raylib), raylib_widgets and rlImGui. + core_raylib_utils + imgui_raylib + imguizmo_raylib + vqf Fusion unordered_dense::unordered_dense spdlog::spdlog UTL::include - OpenGL::GLU - ${FREEGLUT_LIBRARY} - ${OPENGL_gl_LIBRARY} ${PLATFORM_LASZIP_LIB} - ${PLATFORM_MISCELLANEOUS_LIBS} - ${CORE_LIBRARIES} - ${GUI_LIBRARIES}) + ${PLATFORM_MISCELLANEOUS_LIBS}) if(WIN32) add_custom_command( @@ -92,6 +88,8 @@ endif() add_executable( drag_folder_with_mandeye_data_and_drop_here-precision_forestry drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp ${SOURCES} + # Still GLUT-based. + "../../core/src/utils.cpp" ) target_compile_definitions(drag_folder_with_mandeye_data_and_drop_here-precision_forestry PRIVATE WITH_GUI=1) diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp index 03bb2aff..6a2cfda6 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp @@ -1,12 +1,19 @@ #include -#include -#include #include +#include #include -#include +#ifdef _WIN32 +// windows.h (pulled in by portable-file-dialogs.h) declares CloseWindow/ShowCursor like raylib.h does. +#define CloseWindow CloseWindow_win32 +#define ShowCursor ShowCursor_win32 +#endif #include +#ifdef _WIN32 +#undef CloseWindow +#undef ShowCursor +#endif #include "lidar_odometry.h" #include "lidar_odometry_utils.h" @@ -15,8 +22,8 @@ #include #include #include +#include #include -#include #include "toml_io.h" #include @@ -28,7 +35,6 @@ #ifdef _WIN32 #include "resource.h" -#include #endif /////////////////////////////////////////////////////////////////////////////////// @@ -155,6 +161,7 @@ bool calculations_failed = false; bool show_without_filtered_buckets = false; bool show_normal_vectors_indoor = false; bool show_normal_vectors_outdoor = false; +bool intensity_shading = true; int lastPar = 1; @@ -619,18 +626,16 @@ Eigen::Vector3d GLWidgetGetOGLPos(int x, int y, const ObservationPicking& observ return pos; } -void step1(const std::atomic& loPause) +void step1(const std::string& input_folder_name, const std::atomic& loPause) { - std::string input_folder_name; std::vector input_file_names; - input_folder_name = mandeye::fd::SelectFolder("Select Mandeye data folder"); std::cout << "Selected folder: '" << input_folder_name << std::endl; if (fs::exists(input_folder_name)) { std::string newTitle = winTitle + " - ..\\" + std::filesystem::path(input_folder_name).filename().string(); - glutSetWindowTitle(newTitle.c_str()); + SetWindowTitle(newTitle.c_str()); for (const auto& entry : fs::directory_iterator(input_folder_name)) if (entry.is_regular_file()) @@ -1150,7 +1155,7 @@ void settings_gui() { loStartTime = std::chrono::system_clock::now(); - step1(loPause); + step1(mandeye::fd::SelectFolder("Select Mandeye data folder"), loPause); std::cout << "Load data done please click 'Compute all' to continue calculations" << std::endl; std::chrono::duration elapsed = std::chrono::system_clock::now() - loStartTime; @@ -1761,6 +1766,7 @@ void progress_window() ImGui::Checkbox("Show reference buckets indoor", &show_reference_buckets_indoor); ImGui::Checkbox("Show reference buckets outdoor", &show_reference_buckets_outdoor); ImGui::Checkbox("Show initial points", &show_initial_points); + ImGui::Checkbox("Intensity shading", &intensity_shading); ImGui::Checkbox("Show trajectory", &show_trajectory); ImGui::Checkbox("Show trajectory as axes", &show_trajectory_as_axes); ImGui::Checkbox("Show prediction vectors", &show_prediction_vectors); @@ -1774,8 +1780,11 @@ void progress_window() ImGui::End(); } -void openData() +//! folder: Mandeye data folder to process; empty asks for one with a folder dialog. +void openData(const std::string& folder = "") { + const std::string input_folder_name = folder.empty() ? mandeye::fd::SelectFolder("Select Mandeye data folder") : folder; + is_settings_gui = false; info_gui = false; @@ -1785,7 +1794,7 @@ void openData() loEstimatedTimeRemaining.store(0.0); loStartTime = std::chrono::system_clock::now(); - step1(loPause); + step1(input_folder_name, loPause); if (step_1_done) { @@ -1881,19 +1890,17 @@ void save_results( void display() { ImGuiIO& io = ImGui::GetIO(); - glViewport(0, 0, (GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); + rlViewport(0, 0, GetRenderWidth(), GetRenderHeight()); - 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); + 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(); - glMatrixMode(GL_PROJECTION); - glLoadIdentity(); - float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); + rlMatrixMode(RL_PROJECTION); + rlLoadIdentity(); updateCameraTransition(); - reshape((GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); + reshape((int)io.DisplaySize.x, (int)io.DisplaySize.y); viewLocal = Eigen::Affine3f::Identity(); @@ -1908,22 +1915,24 @@ void display() viewLocal.translate(-rotation_center); - glMatrixMode(GL_MODELVIEW); - glLoadMatrixf(viewLocal.matrix().data()); + rlMultMatrixf(viewLocal.matrix().data()); + captureFrameMatrices(); showAxes(); if (show_initial_points) { - glColor3d(0.0, 1.0, 0.0); - glPointSize(point_size); - glBegin(GL_POINTS); + const IntensityRange range = intensityRange(params.initial_points); + color3f(0.0f, 1.0f, 0.0f); + beginPoints(point_size); for (const auto& p : params.initial_points) { + if (intensity_shading) + color3fIntensityShaded(0.0f, 1.0f, 0.0f, p.intensity, range); auto pp = params.m_g * p.point; - glVertex3d(pp.x(), pp.y(), pp.z()); + pointVertex3f(pp.x(), pp.y(), pp.z()); } - glEnd(); + endPoints(); } #if 0 // ToDo @@ -1960,42 +1969,48 @@ void display() if (show_reference_points) { - glColor3f(1, 0, 0); - glBegin(GL_POINTS); + const IntensityRange range = intensityRange(params.reference_points, dec_reference_points); + color3f(1, 0, 0); + beginPoints(point_size); for (size_t i = 0; i < params.reference_points.size(); i += dec_reference_points) - glVertex3f(params.reference_points[i].point.x(), params.reference_points[i].point.y(), params.reference_points[i].point.z()); - glEnd(); + { + if (intensity_shading) + color3fIntensityShaded(1, 0, 0, params.reference_points[i].intensity, range); + pointVertex3f(params.reference_points[i].point.x(), params.reference_points[i].point.y(), params.reference_points[i].point.z()); + } + endPoints(); } if (show_trajectory_as_axes) { - // glBegin(GL_LINE_STRIP); - glBegin(GL_LINES); + rlBegin(RL_LINES); for (const auto& wd : worker_data) { for (const auto& it : wd.intermediate_trajectory) { - glColor3f(1, 0, 0); - glVertex3f(it(0, 3), it(1, 3), it(2, 3)); - glVertex3f(it(0, 3) + it(0, 0) * 0.1, it(1, 3) + it(1, 0) * 0.1, it(2, 3) + it(2, 0) * 0.1); + rlColor3f(1, 0, 0); + rlVertex3f(it(0, 3), it(1, 3), it(2, 3)); + rlVertex3f(it(0, 3) + it(0, 0) * 0.1, it(1, 3) + it(1, 0) * 0.1, it(2, 3) + it(2, 0) * 0.1); - glColor3f(0, 1, 0); - glVertex3f(it(0, 3), it(1, 3), it(2, 3)); - glVertex3f(it(0, 3) + it(0, 1) * 0.1, it(1, 3) + it(1, 1) * 0.1, it(2, 3) + it(2, 1) * 0.1); + rlColor3f(0, 1, 0); + rlVertex3f(it(0, 3), it(1, 3), it(2, 3)); + rlVertex3f(it(0, 3) + it(0, 1) * 0.1, it(1, 3) + it(1, 1) * 0.1, it(2, 3) + it(2, 1) * 0.1); - glColor3f(0, 0, 1); - glVertex3f(it(0, 3), it(1, 3), it(2, 3)); - glVertex3f(it(0, 3) + it(0, 2) * 0.1, it(1, 3) + it(1, 2) * 0.1, it(2, 3) + it(2, 2) * 0.1); + rlColor3f(0, 0, 1); + rlVertex3f(it(0, 3), it(1, 3), it(2, 3)); + rlVertex3f(it(0, 3) + it(0, 2) * 0.1, it(1, 3) + it(1, 2) * 0.1, it(2, 3) + it(2, 2) * 0.1); } } - glEnd(); + rlEnd(); } if (show_prediction_vectors) { - glDisable(GL_DEPTH_TEST); - glLineWidth(3.0f); - glBegin(GL_LINES); + // Depth test state applies when the batch is drawn, so flush around the toggle. Line widths + // other than 1 aren't supported by the core-profile context, so these are drawn 1 px wide. + rlDrawRenderBatchActive(); + rlDisableDepthTest(); + rlBegin(RL_LINES); for (const auto& wd : worker_data) { if (wd.intermediate_trajectory.empty() || wd.imu_prediction_vector.norm() < 1e-6) @@ -2004,19 +2019,18 @@ void display() Eigen::Vector3d start = wd.intermediate_trajectory.front().translation(); Eigen::Vector3d end = start + wd.imu_prediction_vector; - glColor3f(1.0f, 0.0f, 1.0f); // magenta - glVertex3d(start.x(), start.y(), start.z()); - glVertex3d(end.x(), end.y(), end.z()); + rlColor3f(1.0f, 0.0f, 1.0f); // magenta + rlVertex3f(start.x(), start.y(), start.z()); + rlVertex3f(end.x(), end.y(), end.z()); } - glEnd(); - glLineWidth(1.0f); - glEnable(GL_DEPTH_TEST); + rlEnd(); + rlDrawRenderBatchActive(); + rlEnableDepthTest(); } if (intermediate_trajectory_prediction_axes) { - glLineWidth(1.0f); - glBegin(GL_LINES); + rlBegin(RL_LINES); for (const auto& wd : worker_data) { if (wd.intermediate_trajectory.empty() || wd.intermediate_trajectory_prediction.empty()) @@ -2027,38 +2041,35 @@ void display() const auto& it = wd.intermediate_trajectory[i]; const auto& pred_axis = wd.intermediate_trajectory_prediction[i]; - glColor3f(1, 0, 0); - glVertex3f(it(0, 3), it(1, 3), it(2, 3)); - glVertex3f(it(0, 3) + pred_axis(0, 0) * 0.05, it(1, 3) + pred_axis(1, 0) * 0.05, it(2, 3) + pred_axis(2, 0) * 0.05); + rlColor3f(1, 0, 0); + rlVertex3f(it(0, 3), it(1, 3), it(2, 3)); + rlVertex3f(it(0, 3) + pred_axis(0, 0) * 0.05, it(1, 3) + pred_axis(1, 0) * 0.05, it(2, 3) + pred_axis(2, 0) * 0.05); - glColor3f(0, 1, 0); - glVertex3f(it(0, 3), it(1, 3), it(2, 3)); - glVertex3f(it(0, 3) + pred_axis(0, 1) * 0.05, it(1, 3) + pred_axis(1, 1) * 0.05, it(2, 3) + pred_axis(2, 1) * 0.05); + rlColor3f(0, 1, 0); + rlVertex3f(it(0, 3), it(1, 3), it(2, 3)); + rlVertex3f(it(0, 3) + pred_axis(0, 1) * 0.05, it(1, 3) + pred_axis(1, 1) * 0.05, it(2, 3) + pred_axis(2, 1) * 0.05); - glColor3f(0, 0, 1); - glVertex3f(it(0, 3), it(1, 3), it(2, 3)); - glVertex3f(it(0, 3) + pred_axis(0, 2) * 0.05, it(1, 3) + pred_axis(1, 2) * 0.05, it(2, 3) + pred_axis(2, 2) * 0.05); + rlColor3f(0, 0, 1); + rlVertex3f(it(0, 3), it(1, 3), it(2, 3)); + rlVertex3f(it(0, 3) + pred_axis(0, 2) * 0.05, it(1, 3) + pred_axis(1, 2) * 0.05, it(2, 3) + pred_axis(2, 2) * 0.05); } } - glEnd(); - glLineWidth(1.0f); + rlEnd(); } if (show_trajectory) { - glPointSize(3); - glColor3f(0, 1, 1); - glBegin(GL_POINTS); + color3f(0, 1, 1); + beginPoints(3); for (const auto& wd : worker_data) { for (const auto& it : wd.intermediate_trajectory) - glVertex3f(it(0, 3), it(1, 3), it(2, 3)); + pointVertex3f(it(0, 3), it(1, 3), it(2, 3)); } - glEnd(); - glPointSize(1); + endPoints(); - glColor3f(1, 0, 0); - glBegin(GL_LINES); + rlBegin(RL_LINES); + rlColor3f(1, 0, 0); if (worker_data.size() > 0) { const auto& wd = worker_data[worker_data.size() - 1]; @@ -2066,114 +2077,112 @@ void display() { const auto& it = wd.intermediate_trajectory[wd.intermediate_trajectory.size() - 1]; - glVertex3f(it(0, 3) - 1, it(1, 3), it(2, 3)); - glVertex3f(it(0, 3) + 1, it(1, 3), it(2, 3)); + rlVertex3f(it(0, 3) - 1, it(1, 3), it(2, 3)); + rlVertex3f(it(0, 3) + 1, it(1, 3), it(2, 3)); - glVertex3f(it(0, 3), it(1, 3) - 1, it(2, 3)); - glVertex3f(it(0, 3), it(1, 3) + 1, it(2, 3)); + rlVertex3f(it(0, 3), it(1, 3) - 1, it(2, 3)); + rlVertex3f(it(0, 3), it(1, 3) + 1, it(2, 3)); - glVertex3f(it(0, 3), it(1, 3), it(2, 3) - 1); - glVertex3f(it(0, 3), it(1, 3), it(2, 3) + 1); + rlVertex3f(it(0, 3), it(1, 3), it(2, 3) - 1); + rlVertex3f(it(0, 3), it(1, 3), it(2, 3) + 1); } } - glEnd(); + rlEnd(); } if (show_reference_buckets_indoor) { std::scoped_lock lock(params.mutex_buckets_indoor); - glColor3f(1, 0, 0); - glBegin(GL_POINTS); + color3f(1, 0, 0); + beginPoints(1); for (const auto& b : params.buckets_indoor) { if (b.second.number_of_points == -1) { - glColor3f(0, 1, 1); + color3f(0, 1, 1); } else { - glColor3f(1, 0, 0); + color3f(1, 0, 0); } if (!show_without_filtered_buckets) { - glVertex3f(b.second.mean.x(), b.second.mean.y(), b.second.mean.z()); + pointVertex3f(b.second.mean.x(), b.second.mean.y(), b.second.mean.z()); } else { if (b.second.number_of_hits < 20) { - glVertex3f(b.second.mean.x(), b.second.mean.y(), b.second.mean.z()); + pointVertex3f(b.second.mean.x(), b.second.mean.y(), b.second.mean.z()); } } } - glEnd(); + endPoints(); } if (show_reference_buckets_outdoor) { std::scoped_lock lock2(params.mutex_buckets_outdoor); - glColor3f(0, 0, 1); - glBegin(GL_POINTS); + color3f(0, 0, 1); + beginPoints(1); for (const auto& b : params.buckets_outdoor) { if (b.second.number_of_points == -1) { - glColor3f(1, 0, 1); + color3f(1, 0, 1); } else { - glColor3f(0, 0, 1); + color3f(0, 0, 1); } - // glVertex3f(b.second.mean.x(), b.second.mean.y(), b.second.mean.z()); + // rlVertex3f(b.second.mean.x(), b.second.mean.y(), b.second.mean.z()); if (!show_without_filtered_buckets) { - glVertex3f(b.second.mean.x(), b.second.mean.y(), b.second.mean.z()); + pointVertex3f(b.second.mean.x(), b.second.mean.y(), b.second.mean.z()); } else { if (b.second.number_of_hits < 20) { - glVertex3f(b.second.mean.x(), b.second.mean.y(), b.second.mean.z()); + pointVertex3f(b.second.mean.x(), b.second.mean.y(), b.second.mean.z()); } } } - glEnd(); + endPoints(); } if (show_normal_vectors_indoor) { std::scoped_lock lock2(params.mutex_buckets_indoor); - glColor3f(0, 0, 1); - glBegin(GL_LINES); + rlBegin(RL_LINES); for (const auto& b : params.buckets_indoor) { - glColor3f(fabs(b.second.normal_vector.x()), fabs(b.second.normal_vector.y()), fabs(b.second.normal_vector.z())); + rlColor3f(fabs(b.second.normal_vector.x()), fabs(b.second.normal_vector.y()), fabs(b.second.normal_vector.z())); - glVertex3f(b.second.mean.x(), b.second.mean.y(), b.second.mean.z()); - glVertex3f( + rlVertex3f(b.second.mean.x(), b.second.mean.y(), b.second.mean.z()); + rlVertex3f( b.second.mean.x() + b.second.normal_vector.x(), b.second.mean.y() + b.second.normal_vector.y(), b.second.mean.z() + b.second.normal_vector.z()); } - glEnd(); + rlEnd(); } if (show_normal_vectors_outdoor) { std::scoped_lock lock2(params.mutex_buckets_outdoor); - glColor3f(0, 0, 1); - glBegin(GL_LINES); + rlBegin(RL_LINES); for (const auto& b : params.buckets_outdoor) { - glColor3f(fabs(b.second.normal_vector.x()), fabs(b.second.normal_vector.y()), fabs(b.second.normal_vector.z())); + rlColor3f(fabs(b.second.normal_vector.x()), fabs(b.second.normal_vector.y()), fabs(b.second.normal_vector.z())); - glVertex3f(b.second.mean.x(), b.second.mean.y(), b.second.mean.z()); - glVertex3f( + rlVertex3f(b.second.mean.x(), b.second.mean.y(), b.second.mean.z()); + rlVertex3f( b.second.mean.x() + b.second.normal_vector.x(), b.second.mean.y() + b.second.normal_vector.y(), b.second.mean.z() + b.second.normal_vector.z()); } - glEnd(); + rlEnd(); } if (show_covs_indoor) @@ -2195,51 +2204,59 @@ void display() { if (index_begin < worker_data.size()) { - glColor3f(1, 1, 0); - glBegin(GL_LINES); + rlBegin(RL_LINES); + rlColor3f(1, 1, 0); if (worker_data[index_begin].intermediate_trajectory.size() > 0) { auto p = worker_data[index_begin].intermediate_trajectory[0].translation(); - glVertex3f(p.x() - 1, p.y(), p.z()); - glVertex3f(p.x() + 1, p.y(), p.z()); + rlVertex3f(p.x() - 1, p.y(), p.z()); + rlVertex3f(p.x() + 1, p.y(), p.z()); - glVertex3f(p.x(), p.y() - 1, p.z()); - glVertex3f(p.x(), p.y() + 1, p.z()); + rlVertex3f(p.x(), p.y() - 1, p.z()); + rlVertex3f(p.x(), p.y() + 1, p.z()); - glVertex3f(p.x(), p.y(), p.z() - 1); - glVertex3f(p.x(), p.y(), p.z() + 1); + rlVertex3f(p.x(), p.y(), p.z() - 1); + rlVertex3f(p.x(), p.y(), p.z() + 1); } - glEnd(); + rlEnd(); } if (index_end < worker_data.size()) { - glColor3f(0, 1, 1); - glBegin(GL_LINES); + rlBegin(RL_LINES); + rlColor3f(0, 1, 1); if (worker_data[index_end].intermediate_trajectory.size() > 0) { auto p = worker_data[index_end].intermediate_trajectory[0].translation(); - glVertex3f(p.x() - 1, p.y(), p.z()); - glVertex3f(p.x() + 1, p.y(), p.z()); + rlVertex3f(p.x() - 1, p.y(), p.z()); + rlVertex3f(p.x() + 1, p.y(), p.z()); - glVertex3f(p.x(), p.y() - 1, p.z()); - glVertex3f(p.x(), p.y() + 1, p.z()); + rlVertex3f(p.x(), p.y() - 1, p.z()); + rlVertex3f(p.x(), p.y() + 1, p.z()); - glVertex3f(p.x(), p.y(), p.z() - 1); - glVertex3f(p.x(), p.y(), p.z() + 1); + rlVertex3f(p.x(), p.y(), p.z() - 1); + rlVertex3f(p.x(), p.y(), p.z() + 1); } - glEnd(); + rlEnd(); } } - ImGui_ImplOpenGL2_NewFrame(); - ImGui_ImplGLUT_NewFrame(); - ImGui::NewFrame(); + rlImGuiBegin(); ShowMainDockSpace(); view_kbd_shortcuts(); + // Drag and drop: a Mandeye data folder, or any file in one, is processed like "Load & process scanning". + const auto dropped = takeDroppedFiles(); + if (!dropped.empty() && !loRunning) + { + fs::path folder(dropped[0]); + if (!fs::is_directory(folder)) + folder = folder.parent_path(); + openData(folder.string()); + } + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_O, false)) { openData(); @@ -2447,7 +2464,7 @@ void display() if (ImGui::Button("Load & process scanning")) openData(); if (ImGui::IsItemHovered()) - ImGui::SetTooltip("Select folder from where to load data and start processing (Ctrl+O)"); + ImGui::SetTooltip("Select folder from where to load data and start processing (Ctrl+O), or drop the folder on the window"); ImGui::SameLine(); ImGui::Dummy(ImVec2(20, 0)); @@ -2476,6 +2493,9 @@ void display() ImGui::EndDisabled(); ImGui::MenuItem("Show initial points", nullptr, &show_initial_points); + ImGui::MenuItem("Intensity shading", nullptr, &intensity_shading); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Shade initial and reference points by LiDAR intensity"); ImGui::MenuItem("Show trajectory", nullptr, &show_trajectory); ImGui::MenuItem("Show trajectory as axes", nullptr, &show_trajectory_as_axes); ImGui::MenuItem("Show prediction vectors", nullptr, &show_prediction_vectors); @@ -2535,11 +2555,11 @@ void display() ImGuizmo::Enable(true); ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); - GLfloat projection[16]; - glGetFloatv(GL_PROJECTION_MATRIX, projection); + float projection[16]; + getProjectionMatrix(projection); - GLfloat modelview[16]; - glGetFloatv(GL_MODELVIEW_MATRIX, modelview); + float modelview[16]; + getModelviewMatrix(modelview); ImGuizmo::Manipulate( &modelview[0], @@ -2575,11 +2595,11 @@ void display() ImGuizmo::Enable(true); ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); - GLfloat projection[16]; - glGetFloatv(GL_PROJECTION_MATRIX, projection); + float projection[16]; + getProjectionMatrix(projection); - GLfloat modelview[16]; - glGetFloatv(GL_MODELVIEW_MATRIX, modelview); + float modelview[16]; + getModelviewMatrix(modelview); ImGuizmo::Manipulate( &modelview[0], @@ -2625,14 +2645,12 @@ void display() info_window(infoLines, appShortcuts); } + end3DAndDrawLabels(); + if (compass_ruler) drawMiniCompassWithRuler(); - ImGui::Render(); - ImGui_ImplOpenGL2_RenderDrawData(ImGui::GetDrawData()); - - glutSwapBuffers(); - glutPostRedisplay(); + rlImGuiEnd(); } void on_exit() @@ -2643,23 +2661,6 @@ void on_exit() 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) { @@ -2805,17 +2806,15 @@ int main(int argc, char* argv[]) std::cout << argv[0] << " input_folder parameters(*.toml) output_folder" << std::endl; initGL(&argc, argv, winTitle, display, mouse); - glutCloseFunc(on_exit); #ifdef _WIN32 InitTaskbarProgress(); #endif - glutMainLoop(); + mainLoop(); - ImGui_ImplOpenGL2_Shutdown(); - ImGui_ImplGLUT_Shutdown(); - ImGui::DestroyContext(); + shutdownGL(); + on_exit(); } } catch (const std::bad_alloc& e) { diff --git a/apps/lidar_odometry_step_1_legacy/CMakeLists.txt b/apps/lidar_odometry_step_1_legacy/CMakeLists.txt new file mode 100644 index 00000000..7d2210d4 --- /dev/null +++ b/apps/lidar_odometry_step_1_legacy/CMakeLists.txt @@ -0,0 +1,85 @@ +cmake_minimum_required(VERSION 4.0.0) + +# Legacy GLUT + legacy immediate-mode OpenGL build of step1, kept side by +# side with apps/lidar_odometry_step_1 (raylib-based since it was ported off +# GLUT), the same way step2/step3 keep their *_legacy builds. Only the GUI +# translation unit is copied: the odometry sources it builds on carry no +# GL/GLUT code, so they are compiled from apps/lidar_odometry_step_1 as-is. +project(lidar_odometry_step_1_legacy) + +set(STEP1_DIR ${REPOSITORY_DIRECTORY}/apps/lidar_odometry_step_1) + +set(SOURCES + lidar_odometry_gui.cpp + ${STEP1_DIR}/lidar_odometry_utils.cpp + ${STEP1_DIR}/lidar_odometry_utils_optimizers.cpp + ${STEP1_DIR}/lidar_odometry.cpp + ${STEP1_DIR}/points_global_spill.cpp + ${STEP1_DIR}/raw_cloud_loader.cpp + ${STEP1_DIR}/toml_io.cpp + "../../core/src/utils.cpp" +) + +# Windows: add resource file +if(WIN32) + list(APPEND SOURCES "resource.rc") +endif() + +add_executable(lidar_odometry_step_1_legacy ${SOURCES}) + +target_compile_definitions(lidar_odometry_step_1_legacy PRIVATE WITH_GUI=1) +# target_apply_hdmapping_profiler() is defined by apps/lidar_odometry_step_1. +target_apply_hdmapping_profiler(lidar_odometry_step_1_legacy) + +target_include_directories( + lidar_odometry_step_1_legacy + PRIVATE ${STEP1_DIR} + ${REPOSITORY_DIRECTORY}/core/include + ${REPOSITORY_DIRECTORY}/core_hd_mapping/include + ${THIRDPARTY_DIRECTORY} + ${THIRDPARTY_DIRECTORY}/glm + ${EIGEN3_INCLUDE_DIR} + ${THIRDPARTY_DIRECTORY}/tomlplusplus/include + ${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 + ${THIRDPARTY_DIRECTORY}/Fusion/Fusion) + +target_link_libraries( + lidar_odometry_step_1_legacy + PRIVATE + vqf + Fusion + unordered_dense::unordered_dense + spdlog::spdlog + UTL::include + OpenGL::GLU + ${FREEGLUT_LIBRARY} + ${OPENGL_gl_LIBRARY} + ${PLATFORM_LASZIP_LIB} + ${PLATFORM_MISCELLANEOUS_LIBS} + ${CORE_LIBRARIES} + ${GUI_LIBRARIES}) + +if(WIN32) + add_custom_command( + TARGET lidar_odometry_step_1_legacy + POST_BUILD + COMMAND + ${CMAKE_COMMAND} -E copy $ + $ + COMMAND_EXPAND_LISTS) +endif() + +if (MSVC) + target_compile_options(lidar_odometry_step_1_legacy PRIVATE /bigobj) +endif() + +hdmapping_install_app(lidar_odometry_step_1_legacy) diff --git a/apps/lidar_odometry_step_1_legacy/icon.ico b/apps/lidar_odometry_step_1_legacy/icon.ico new file mode 100644 index 0000000000000000000000000000000000000000..14e651907f3c13531c15763be35ff3465b595b43 GIT binary patch literal 20645 zcmXV11ymbdunz9-P^>NP4#kU=;_mJ(?(R-;cemh9vEY=V#ogWA4gx8v zON)!Dx-Xq(yJg`@JcZ?+<<#=g^zKnvD2j{VemcnJ!x}9>%*i?ISoa8RS5Xk0u>VMZ zkPiA}j)d%K2ul7y#gyr`_`$-}qOIN6Z}gFSfANp;(7^9Spl_eC=|I^3_ou@mP5BuDlF$Gn!nIsnTsT!ViD^y` zjn4_=pFt&7%IfNp=H}FyJa*>iYn?kgJBsS+F__em1$nf{N=izfsi@*pQU<#NV5H^b zsJ?vBjtmr^`Ufk5_-A-{@A({Ju~=_bX#mMNIr#tv)AOmG#23&8hlM33CG}6{i}5|5 zG!XIMaVje(DB()1aYYnGD=vM(R-AY7ecUC$XVmPTDwY`@9kphk5&ps{_lb%sWc~f^ zUf1ii36=DVDsq7M)Ecbe_1@TM#_dNG0^e_1T8nIao3^WMY*tgBOB8bDYxFyJNRz(F zH-c2v)Gv>I)?D?Xu*79&`%K=4YH}K%sJFpsA~k5%S|O`TNDxrVt0*2==XD@{dU)>4=;1_~>p!1jg)Hc}5<$v;9m*QkasAa3EPw)1K&p~4)ak88k*99 zpD?|FtyEPcINP5h>Mhesx-A)FfeCTDmU-N-`JME8#kU57m^0jM-6!?!^?9V7A-=H~giAoc z0o>rO$2}6rc6krfies&CJaF8RR8~dU$(2Pv@|R zS+Dz+86t&H!b<%SwsSb{eM8J0E()AShfKYJ4V{Xa7P((;tCZ9yMAwI#ML;qR+Mst|P98EKh=ChT{cAwUM|me_$tv?FJpan{xol0>9S_tEJdwd+3U|S$+0V|9LBl(~v85S4I3i!{7D%Y7 zt3Ub1)N8eZrBI3O^FABESz=Nvoc!Hx>XcyO{K(CnqEsl!=KWxgjg76S_=jcGVs*)- zQonO)VtxZZP~#3a&>wPh79ppkeS3yN8D48CEB(vNMJsxU4ha_*x7PZR9UlDy?YS#K zaZ@RTk==<9T2V`Dm}eyQnUn-?SNebrgZ-C=DiBKFJwE-+&Ami`hj-jf)?jzqlZD41 zTj}N7cmA^2Y6QbFo->9(?kyq~WHCeGKYxcGkN=_jHlmIY;o*lE;-Ey>G!185)$DUA z<4BkO;Ehfx4kjkWxjR8#*P67gEg9skEy%!cbsRif@lq0imy9QHJEMY1&Xy(A`J50R zFK=yaotj1cK^5eBwlW+;%6~_~ZSU&+87gv%=&dCXOnXvRWifXKy&|`UgO5LG5-T{^ zXf=&~kc@$fYC2P1%I>_tD*21BM+`%7N2kRFr6dkuwG#YYO*{w2mNB9cuS zP$st>H497ZuV1KEq}fUEsZpoP&2`?7j{hYIZo9S7E}v)n-?~g|Iyx#Nm0AsoxFM61 zipt8$go2)^IMi7~X>2Cp5fSmwpjwA%YRINvO-JH59(rl!RWxnn$6XgedhFd`AebK=CQ{z_WY!I@3$xz*zrDOYhH#}4` zvcUehs>SiJIQ5CtKvz{%w3Fl(L~e#GdL9RAgIn+((q$%CZm~<8n~K2vTI;y`y+k&> zx2U3KuMb05;N!MkbZ992T(w@G0hzl$5)SpnL1C=2rs@nAT=%YPoy7zh#Kh+-G}7b@ zjO0^!WbYDK+c87n)x_8sSh_(axNTT?`2Y46B<)RC3WlVD)RMNb*_Q%VT$}MT+>@Vy zt1CAV%2_4N+gq$z&hi7-MYI1c-S1?2Jj_knFoV-C8!ygYi#iYE$&@D(Bz z6%~_yOF6RG$H3oAog_|DH(S#x&)4>l&yCcD6NK%_ziDpw`jlSXeKo&mP!A>$&OE{V zm<6lJW`9ZE&+&?RdU}$d&q26>nwgt<;;Jp7y+LgZ!0al6?EUj=;|VXN>kEse==Qx? zyU)v`0IqXz-oWpazqz!UtYz`!aD7LdQ-YFma_^q+77dLsG+_2evigkZVuJ<7bg9CB z9IA@=&5CVPRyt;T=8xh>ri_|4Re8Nm0|O zWnacssF#PxLR*H_TReUsZ4zel-M;iwlcc5Za zq%AVQjyC;$#1R$9A`VXnq@mPp;P6P9;Mh45b8TV_!AiSpAXCnv7}sL%9k9ZV}L9l2eV z=;JoD5CtJ3#aL>}Eov1dE7+DmFQ0zI)bq*5RORH`pKi_!?=H*72}q z{U1zQxuWNGL)mCgb27?>j)Y>Wwqm@&s)4)`5gCk-k*`pEucf)~B^Fc%j-dtOPEn3B zZ6u8w7Hc(kE+BNxn=Uh%*NX!_(^I_@iio0O-1_7)?t{m@4jvkuat5CY zx3%W_57xoWEqh+g7TfIgx+0NhQ5ZsJO6?soAesumjzY%l%$;oH+bp(b(uz2AM(NC*m_7~4uF=sJi}Or!)?jj$%UB?bhJ!Cea&xjs z&dstJ>DMIJ5Kf5@j#igOT7Ut?Q_yqw(gEEI*+6{0EIar2v$*5{gd_EDF&Q> zbbOpH=+(TAo2QePjD?k-hR{YKkv~(MI4=wGygQ*|0ta1=^PgX=x~)HwW+vc{xCjcm z&<11rUVF(Y{rPEs*+TkT_jcNq?42u>{CfqR)rG+uS1?6Gb{eE0O-(DU8lx}lE_Aj7iVn1;FfQHN6DnP%S0YVg2_K&`F zcniT_^F3w)^ozYX4R!EFvWC7D+dV%Aci@f)$sW|p7y7tU>%7sXbpxY@<%8vxn#MA8 zaeZGQr~R&@mgSl53qxCl^O&&~?56dRN-({!>WpJwhF(W~X`Ih(7{Rp>@`AG3P$$W~ z zt9?+s)>owk<qB4oR^NCe(P@8;;Qud%w&H&yGkC( zSY5Rnt~s01l{>?d_Gewd97DSWz|`?Iz( zUrGZ=C~fci$OL+_l@zgJH*h;-vWh}nzfNgzw6QHPNS#cm3?fbqRO%1LQlax&4v88`lB2Z7yqQl*=$#MLt_a=fBc{? zF{-t=C!4F(>T_H7lk)ZTjhYZ(p-j+G-ohXZStv6Pxn*uveUx&H{=k>8v0-SXMH+m% z-15xli?@+*_D@`s(;%PVVkVPu8tv`(r_>Jf3ZX;FfR}PNh4E+<8gZ4&Rulm!XrCY$ zUJm-KpE%p@94%>l=^?7^_rh`bcqtsTjA`vt?RD$ZWrm0V)B0N*j;Wrk_PhqMUYuX? zxnCu9B##V2_MbK09KE!-+C(6qYMmX*S@Lw9;pO9EpE)<@9Kgl2`sZHh$Bq`dv7GQ7 zGT%^N{|A7Ns+P5@equz2N!m^h<35iLKlb>Hy%BNi(@ZrrBRw@zxZCGkSl9M!HWNvi ziZ5T0kC7kh+_?(3B>#PCeRg;~+f&PLgIgjb!P(0VIjZ7(l@=uuPEvn!6NuU*bkX_h&pvr@rjRM`$~90tZ`N-k^@$fctqsqFW3~JRo(~ z@VnMJ?aRya?IxkSmN8JAD=&hUr(O>z}`E7J2`E;`$$=?n~HtTWBnuY z%$0od!3Nx0)UTOCp7gz7)wLUYGrBc+BQt>>Vp_DbV+tTVHPaK~Bj>Y9&dLq0-|~gq z-P{Mrt?e`?Ahe>_>jI2=ZfcibpnY)jMysy%Z6gd)zaiFMYQE!!2E*s0Ot&lhzmpq~ z-&TIson^{+e){}`m&62iyVm#BikhW{%EBi4T5IOltz#xZMZ#@;5O@C=2OamXg5Z-R z>OKFRoHP}l?RK0}SBbxSMY7a0)n)Ibv!rCmlZO&Uu!IsJ7Ey15*p!E{2r`oEu_yyl z5RC@q{NdIRyC0+{t}v+9qR$*vfYj9foGI)blDQ-i2kxZgWF- z)&TJP-^3DH>oV8c&Ou?khtMQj2--MA4l!5y!;C%pLw(;j9mDwV;+EM^bo(2jHbG#cDfT z4zP>X+|0c1-kfSSxmD|DO0|YYZntD)TJJpS1TC)_Ek+p!Z5Fry*8qSKP*a?2&IhE# z{+6ghN`hX)ND`a-k=l^ioPG*^0=KBU2`Q|`xAT%PxL_?+0NDqY`j?zY(X4A+3d-pt zkLZ|~al>o1rIGFw;Qx+}_8&SIkifU)7an_tl?gj&k_2V>jPre_rjE}%UPSL$V|^^B z{l!rrrsFb5SJl{Vi`>hVjI#AH4UhPgC;la~ayZw&-Yu{^& zKh%iT^>BI^DL{=xF?6h-K4MBJ{rQn{_cp=;VKw`UtEzUpOV&Ry;BY&u;k0B`>S7Ib zkR0EmWsyBhV%M?Be+NmRzxzV`2NCt+1aYOvXCS}}H`Hx^!(NT*qsVoLBO+{cfZkYk z)Y!S+buJg&oa$VCe*N6}z3=_=IuiIKB4q(c&Wwup(P)DtWM2F zQrK*k8u4X1$Jl~5j?Jm|(8Y7(L{1{NG=4Q(t@D>HAcfa!e?wkk0fB^5)Gtk}W+@?F z>s>zHFPB41F)Uj3!3si;!J z%c`G)FLrE5z8WH*af(F0qiKb*M z*{MeKZsCh^^$C%jLgsB8{EGAGn_*#HA6`NUsSA)0M|W_}-+ zIcg~B8^|YDL~3`Y&}-nKcarI~Vx`fRom8&32^OMqFEwF83 zg|?iCgnb3*=;%z%%$gQ&f1@@?M6|-3=}KH9LCqwIQE=@Z9#XB+LoC#ne^#POZ+FPtF!j=dO*PIGN)R+3*pk!Gq}Ir&ifR?$SJi7 zu%-OU15K=S)dkTa8{j6@{gysBuWs|y{pudt6t+l^dDo=4;ujRmmQJL8e9FQM^zRm-&T`Xl9~a~Mh?34~1Wl9c%`W`*P8cbZQo3PLB>s<9I>GkqJuq}CHDt4 zyoX|NNdxk)1)FXGOM~y^vE31|P=j~pU@T0gn`ChyiO1t9EHu<{gsH{(w@r6s_JDW%H5^8z zhd;mi>hSgQTVL$6IF?8 z=gqHMT>MNVHY3-)N%QmbeLYB^7xcHK&!*%ypSQVXLnVT-7mA?OstW^RQT|#o8R%J^ zh7AUaAgdBlpYNtT@a5saoEE_^Td|_9ltkEO5SBS$QW{$N9Lo2y^Q~X9J2$9>8W5h>+;V*+6hQm>?vnQl;m_G;_p=2=w zp3$b{Bq4Wkqj7(9;qaX8x9j_epm2X^)gx+_Q>GiIC28gT!Ek7_~xW7L_H_KZoQ+K&cMe6Z% zt-WfghfFHS0IzJ&JFx=C(NGRCmMDsRt)Z|^e?{lW_gMs48lGo9N8udd$jZ36) zskxw>U+v`m6J}a(KPGjkl_4U`DkrNf4CN?w-Tm0!^k3gT4ZP%v>6*Q+o_lOk%vI`n z_rHHsUP_>LnX}CX)R;o=LNRq0m?Akc@bVVbj!pMM1v`>8z9(B1lq2Iyh^pXm;xa}? zg26l7tsd4=WPr(A(x%n3TrTZ^ij2~0nc$sAo8#Y5lUc=!M#}vwDJcnkF*oL~92qWd zznRN6FNgpcUC3*D3%`;(l4qN)IMcYkUh)DUNR{d77P3*I#+FN zVh)SZlbPo(JKE*$I06D{t#mWB1(meaOek4-YT>nCS8WKTI&Uphj zL7*nkc)kFxCte$6+N^H0W!g(ceoj?V>}XohVSCu4>>o>o)BAd1Y)xMz!vsV~$@-ffsK%KCjQ zyIh)Ta20MpcGw4?5=RCHgD)qbhWj5J(+*hWkua>3I*Xggym&uOhL_NL|E>v)y^?wLwODC& z(Di%0&7OKdAr0wheWg;zeZ_zS3PTQg4v8X$?ka|THbzBZ#T)|WykZ{_5FtZ#Wj(zTx0KZJgF@b1px3ertCl-!|zz#KGO*FdSfj z-Y)EWn3!}XTTD4^;amt`#w8=4g_^_-S$B!u+U%0PX?(QErC*yTz)z_y(0c1p)6kRl z^z@v^0bQWZ0=O$wBG;b&7Rk1KM^*bYfp<(Zhv56jti^H_-yjAadKI31fr5I6~fVl2npXA|w zRh?ydn&E^il?c5U3}5R=>5TnvP3LYff}wAaP__nl+?mhG|ErGwhHMCFK zrLzK9e_Y+(C3au8{(;_Y(JZwNC|~W3W$yDg*B3>m-s3y?)sU6BMZkpYg0Z<5=YSxR!FfL&;3f@xMnN1qJ?+K_xP)7^sMLd!d0wAdllnNH z_Aa5p1sAqPM1D&h^LB4sjyvCg_*ghZ_I*>m!g*hLfGxf?Lm;Y0an7Iq)QX@{tUtgf zI8HfAykx$A(f>sAz<7@m3xVqK5FFpWgW^~g*Nq95%j9+d3JvuZ6K{kt?O;a>NjF;s zx$bKEvG4s}p0Rsqu0JpXC7$V9efn$-QVd%}NX~zq$mHgMIN=P^?5;PH0ZsAK-sya= zlhH2cr{kOW-bCg)pVL^@obi>!J8uW&MIQ#U(*b2y!D?>ltc)jU`YUKMCMG{EJL zdd+!}3z=qh`C3Iedf-RmM|9PVMYH*Vxz9K>5i4VS)R&|6_k~>`mgZt zjR=|?!~Srr?1G6Vt+uRH+x-9pGQ-!l+e*C_NYOP7oEa)A##VDuuoyKex8Il|uDJz% zowY&n`X$FWZ&3Sr*D}Ar=?7fCyUUwBzeK&v)68ogJc$4i9+w0$mDYxL(kyJAtF)Yn z{}z8|ArYx1B>RHUEoz__@QtJ-B;Xp%$F5HCKl|6*sR6D*4^ugk?wnwTW+vYB&1bH2 zgD{~0TUL2S^i>_&FMOf|by@iI~VC9rDWs~nP zEL^4yEk_l}K0JYj^M|wdmSQDg2ct=(T5MMr0B?PEc2;ibUjyCf)z*e&C~qP?_X^<*KZ+&MmX zM9?SMG_&{Z1?P`LPg3PPS+tSguOFmVXNU;7;t;~#l5=|MJ3KxKzrS$!y+5yOw%t^~ zoH68s^3~BY!OC+RcgH58_;{tZWcE%Z7qWuB_w6X|51UeiUz{yVt%poLf1q|~kE9TGwosa#@&a#UEJr_wTH{?bYVZC)i#%7&gfi? z)QMcbM-DBn#i}O`R27;}0zp!t!m{7LMK(T}tE#E}S3_X+y0Zof$SWnef`%KQH~VMu zGyxBGK+%vkhRElEK;G}K_p<2%=~*i)uLfn^RpqM>GUqv6HaU3bDDW5OtB(RL?cH`6 zaR5;R8lonKX>wO$GOzyFXWHg0Jiy9abzM>2J`h1z!!dsC+x3x~8=4NuatZ41?Co7` zhLP9XZ+_*rSrV6%8&mR;7ZCx$z@u(lF;p8)&1xiOtvr_M45@-ZYrfAm=A&t%Hm7e1 ztN0R|6F#2o98H<~hb*vwA8szshG8>tr*n>hgtQ6NUCsX{(Pe`-33i$H3^H@>Di5C@ z7X{+o0#Yd6*mowfawc6OGhfu{85kBDt#BCBt4(jgY5=q(ZfzMK9UZ;67OLaI$xS1o zq5`H?C{S?!PirpJnGo?gey*#l(<>Q^@t7S2I-C;I(}#uM-rP{nucDb@Crj#UYE$9z^wkrxJNHG=JFtvgV(-`JE}E&}6*S6f zJo-fO@nfLF_TYamIZ*onjEwxeJb$Vvg6jj5*hg7%CH1bKvbpg5`-c{Zv4;nD{j^(X6NPIQTJ5Ec zJBBR~E6^}vK9(U7OP2j}aRgw~wH8n=o}}OcPLx|LU}bl1hE0PloWaLJ|Nc0r@?(Ph zZyKH7u3u0PR_Uk!HUt!Ha)f-IoZAN9nm2UjlB@2opLks5S2=m<_JI2-AJ=(BX#dM+ zRT+eyYSb6X4($G^sVVJ_l#GnOKts|Lo!d>7WtrjTS}g0Rd7qNqK59dVqs<@}tX5kI z$B|}2TGF?#UjxtgpdIj%gf7s2Zi0vbjQCOSIRF=MmZ8R zRCsC}Lkah8WRdN&^8Tnji5~*$RZrI|*Aq^c0|XA+(L7f>i3-?r5kX!t!6sw~_N*x^ z2IFHq>%YL@(QN)yjtPMV6>dj;Ju#n-Pqq*WZi-L~$=A%CxN4GpKY>XX zQZ+SxJ%M+-&ET^fk2x(1HorG-MO;a@sm>SJzz{Ej1OttY)V*UIknFfZn+Wmy<-v)b zcf*1!4M?PP{f~*tB6Z3h^FV=1sANZsf~VFGe+S0 zj}Inv6f?3s;LFuDhKq59P<){XEk9JLE$3d;1iCx3BMcb)c5kz5Ob|(nwZ<9-)7}}B zSxVj9ht`xNKf;Mj{fZ1l)?eYyG`2tjw^;dbz2YhOK#czNwemiG{r@w-7Y6)>jGKuq z!cU-gjd6bkmUW73c&yXz;|zVDB9$`e3)%X#SZw^@S`M$yLPi*8j z^_)?vXY?bQuzAK%LsMHb%zbTm8~jMlJvRQ!mxGW@P?tMR99hHy;3xoK25Y!=go?Q} z5+Bk8izwmQp7;pv-wJ4OX8`~56A_l5-VM_%_6U>6A-Ved+QKD#?yheDvomCitoIxY z3ozpzwQ`N-%vBev1Yl<%Bad9CUdt@9tSsHDOAf^R*!=dB#hQ7&H_KqZHsAYGMnOTd z@dDX2&+pH>*tSPLDivD(*wS8G!6~pXw05-n@mv@U<&F#lGzNdvtTg^HmAzVnv^T*U z4V4pGX-#ogp~y8#QRboNM0Y3yJH16H>5F#MqA4Pa?Vo|@4C`1DAk)5Q1=Lmoe#*F6 zd8(u9eM5MyL{_a6*kQibaovq^^eH?$ng3sKWC6H}5OPud&Y+v(NH_f?`$>@)B97c0 zU)S!*<7aS>;i`5%SfF))m(g>c;5%f6WDzm1?&Z&B}-9`Q=aPQQtX4{8LondUn zwqeJ3Zk9%^KGw_CS#9+F$uxbFCLY?p@uX~}2Ztor)Z$hiR}~k|6ZU?mF}UtmG`X0h z<{;Z5JCLmYfXB>dyJjm_S^_JNtWk~f%ITy3y?jSG)|iybF7y9Za+^H}CpclJbgAKJ zTvp*^2=rlS7h{x&A%R5HZSdch9ym$^kNvp38#C>q1j}TYOTrIIAyCh*c%5ux6&W090&aB_ibWCwjFr39sB= zXSTHe{osCyt^RWH3F*>dh0}vwu$}5p=;1eLkm44j(cRr5Bf|qcY}r~e6by_?+5#?r zJXHJ*;*tD!OXLX3i2g+rfazlWK zQ#CK29PNa;v`a|G&6@Izu&z*3yEeMs;18T1UrkI*Oob6q_V1AlodbSDm$M)(az+7Q zm1uI2WEAAn=JaOTsthsd5|(UU&cFbbpn?M0$QpG&6+CMj$|+oGXwmiXF5UTKV#%Rc zD=gU0d~B^NwrjrcVnX_juyBf8czuejA#^1Bn2jS?A}ZX;c6-r!cC{tWOeI$CBi2Zm z&tr{N2mY1=Ny*g0q$kFi?B6uyGq))YnajRDlcen z-+H5hF$sG`lyE&^Z#PcJ_U(PQVB1E5b6h3Ff}(iVymA%rS?*R$Bzjm;w@gfa?>8T! z5=|O#D_datOM-RPDjA<*YOWy0Uc0VYwg{001@sv#>aWHMvufIH1A z?0C3(B!5tCn+;t8=vo%^;~~FBAr^BDv72y@0%QF^`^5J-9gD4xh=?QM*M&25qe&{n zju3vfsVOO0Ls3=ttC0g+$a5rJO<7)1<%h67(d<4^Js-g~9ROltF~m-CuV^C!3&5I8 z_+JB|k&w_o^6~};rx6wud_4Te6mWLw@VS2gi-=6#&`_iutw~6^OSd_TK>wKq895TV z%_wfAfyPa~Jw3qZ`P+z)?3bOdR(be;;tnWLBgg`$?Gp@AC(e!pPs)csmr`e5g*ws` z)0goav9bA#Q;hLz*Zzb8Y`X}ga4M?L>Az}Gr@pF4BOXb1V=*(3HCxWkH`KHS3ob6c zwEe_rXTML-(EO$)LFIx@)0S_Nk@HVxo7*_Dz%zPvdic`K$Gwy{$FWc1t2^9R_g4z! z46t#_A|eJFCHI$%elo#>#2m@Gy+fP)Msya-wioz*UrZ z+%?_H|CBW{&Y*6y-Qadc?YntGBu@Qu`6Sf=U3iiTJK=_eIt{`FZoIVoNsqKxDC>`Y z`={IQGy;s3t1Y)Nt6sEpETwjmUmV!j>S4Y^@v^C^X*9S_<99utA$pRNqVpZ@X zgC|`2vWkJ5I0wY3M#W0uUmJ&MTv!+$6+unOoe~~yzg19(goH#59NFj=3515J(_}0I z2V&;N+nVHWG#E)z_PdR zD^l&U59C?rp9`s!k05YYPZ*df8F*s#yc!mgf2Xblexv?EWp?o~2TQou&m#P^-zy^MI-_x42UJ93( z<~125G}rYbH5#&dqYpM5I+GQ8KKj}9xG8m)oa7mH9G%~f+{?*^3ZteH5ffgd;7QyE zk7j7N-c>|>`evy}$kpC_8e1u+T~Nmdyh?~D744+kb6YUL!^{&&Gr3O*>IpJVQa>{!tRCNv4P;8HNM3K;XqGMBB3D} zeS>uUBInPD?+cw0qP8Mq4MjuPp&M5}|)30?_SpY|*W#aQY^|V2OL4mEe z8RY`6Q!=LeyB=Rk3+k+Q-4Lfu_t1DSiKp&9<{f|GdxW#hC}oc`a!-f?UP>tZq&iOG z_r=G{%&^5}C6FQj3JUdTJy!JfneA~Z>e1uX6QAjw{3nmippwD)rJW_ci#&qbEZ5NJ z!}{xv3cmME-B3K37ASkVYZA8T`H@NsdVZ_05o?qp^Vo@xah+mQQbIjg4$Jmpr=w#K zn^jd+Gg5HnrZU?_C7pzfp(#ysqUu?k?lA%DJku{uZQzLIw&wV0Ki7vE$)PTr9+rod z)&JLw>bQUnsu;qZ6x{btnT*m{;$bt0FnN2YO;Cw*-$ti`keRQ_HV`O-8RxOKVL+WWT^`p143vK~pMc1# zRzd0Jlw)sjDg#%SxOpzIArU{BUSjWc2x6!ps!E9RptcPRT@LA#uvu@2SXC*5v=ej` zyZ<};U3Pn|CL^K>CMi*mQ|w!(43VW&X@1YZYpl>hgi03 zYHZe;+HjUsroLTAw#!P6;cwFMZY@Ddn!2O=GLT*^g9#6dEXp9?|K{crOr%R|GOaVH zJwhZ9jS?0*y}~pz&tDLcBkzEBJ6x<K9`)V3Po^4;Fi|YO(?~A>^JLo!jfU&owkA|gi3p6yU)i3Q-4;suCyZr0jh-L zzwY416B2s*3PrADH{mx}3m-uq`L;H_2><2N5^zaq!W27lluSpucd@D}P@mTdnbwLm zI!FtRz$__6t}n^|k)v$5X_PN65sX8IFCS}r**`RhNshC81_q^<42R|V^(>$Jh1(t= zLdkSJlH*JWBtB@Sk2ApKRk4g4Q?U!623%0JdbhdLAtM9ti@c149RLs=Z@<2`9|L5;_v_C;DSv{$`;SRlenWI%`}djV zt_Z-MSI^szRCMuwE^2W;B7Q{_(&`BIAZP4nMN?H1<8d~?FWTv!Jk0jbY_Z#qAkZ%= zYDAS&=?pEfe>`W|SYluva?MI`{_*4IVjX{C?!290Cr@V+xQ)2Q);-eIUByw&?lJ~O zjpJ^mdXUlAF1yxzz@ONS!M5^K87(ppiNHQ6G&o0P_Xd~H z%G)mSzGBS>jfDIzI&gyyxd0o7XgKc8hwtZ#raT#Ua9Mb`$rOC3+rs&*KW~;g3q2lL-A5D-Ngq z&c3l4jFPgh1|kP7GA(KYYw2{(gLKBc&nBsLD7#8+$7_;@Vw&=4#vvY1ud zA>)7Ep*DnHm{T*C+P(mQU0z*oZXiuJP;8czlB%Ml_8+=Wb9M$6x|fQQNnDt+5d%3p z!Ru}#uwl??c7OnzaMCq7jzqle|`{1c=M2)|&3hJmirjPbU#&7ekOCg5sn0zfk%J$s}%yM3YSCjpD^jaucF}f|Z)m!%*&-QE$3MqZU7q&K=g*-$Y&Hn84sX5^5l;uJD_D=|dP^c)Jrx z)%HJ4j2{=Aw%g5p_Xx;(@#d84yHP*4l5rLr+O#hK6X(EEP3Q`IWK8NJI?Q7CrtL$( z_{djBV?;@5vzMgO$psGs0}b#cBLLZT-PibL__B8h@axT<(S7wH0ZfoHQv+$FwnHo! zIW>phy1;cedt_#=(M6YMic5xG59!z@46erjqFRHew@5L)EZlnSZ7WMj6ybTY`DeMv~dIt$xR+)apVy zOHwqW1)Ph9x}0s>&+;F_K%orr>0gX<%a z*F4|F9It>aT7(5Gng{#0UDp2wmTrIGv%JH%uZ|bVFqha#4vp_HFf*HP_J+Ki`@Mgn zp$R)WqCYw&0Yr4yPur&xYg|y~7%(P_sidT2YHDh~=CRKPw9;|cvzjlGoBd>?C?HLB91FlvbOy*bghqLtf*bDcRD%V zSJJ@a^7;uy3{X&u=2_b)h+_VcI?uqtnz3R3MHFgmYw_RybxqgS>x9s~Z9bJ%bPX`+ zHopfE3mQ4CnyTD#p8GT!bFVu*F|nT?MrI7y@r*jnm2&D~fak6S@%3r%H0Oh}?6oS* z>TMBz5pl30!dL6V4TK~2`DJ?`+xCEM&CQ2xDUR5aVAo}K*6}#QGgAnrFTq?mbsZP< z`Jn!D=ujKz-3k=CR20s|J-s1!0EUI z2FE66&HI%>8En>*B4_EoD3^6?grR3>*?8WUx9odeg0WAbq0Fm7|K<1kU_hs1w4B`u zJ<36!xZ36jM947nLd0h=P3Ur_w919q|1c55s;~ok&OqV=bbM5k=9n@mU)0_Oa`|V z2LxcO&IA}{(R=a*bWR@P&HmYP2@U~Hv@KztKcEpabN#-K1CrUfPh{T3S#OAS49Agd zfU0`R{=TugItJ*9%m&(EfgzNAWfhg_vx!H|PuczmM)nZj z6~0q*^Uga10E)kbN15zg7!BRo_476$W>b^ zAam4CvThj{{HMxuR@<-$aOCV2C)xNRYll{*MJ2TW#OCKy>U!WGAxRTc`-pjXj^4)a1C3zLi|WY(C9@pGE(rNfQ9f{Eg~(9QXDWq)1YfRhH*>fHTJUU9W}{Y7o)T&HFb*Uk>jM z`v)5J#ySDZ1R!inuRc1Ro?Hrox48e!ursI1xSG;|Y{KYZE9NSVR!hciClN|o=HLGoCXEqs=kwERc-CRKq=DTzdDPpZS_f|GH5Ah zw30E$$AcM9Eg8sKu0Rl6Ck;!fDoFuWntBDz?|xP`*;os~Zd;m*U}#lA&Ot{;MROkKGr8%-Io|P@H0FI3Wn+5Xzs(^=j#X$g zu-#O{wRN7E-5=F<4mK z*q*2PFRv*Vl@!l$fH3kbt0wJpdjHhQ^+iRqf7gY!zAeaknFc?IHMV}kKI*XD_3Zic z4k;Tz{LV?p_2N7u>;qEA4!ZN}mq4 zj3aA>Bj5HB^_e!r)Xvlb90osC4_9PKuohB#!4bPfvv!RLZ+NKLb!F zoPG8pDSX(amFj!Dnp}fk5&6guAx48ztJ8+~|zk`=R9o4)v9J9Vjy zNdboVD2uh`g-^RtRxNr+~8a}L;IkE~Ah+*_ONzlspSnCjm-ll}%H zdf`H>_TF}8S-1B3R4`_;xXy(ma0mv!N=_{;}sh=DFaY4i>k_pGcD95Kg86C z{#eeD=Cls_HQhpIYi9D(r{^#cb#-+zR-AvZy!Fg($VnZ9Q>V7W(nYMFjT?W% z@IiJ?dFh>Nskxo`qxKwgTT9Y*w~cBBLd=XaCn5U%dm}oa3@#)e>3jY{op@u0=}|p{ z$mADEf7h9%f08=R#dZ8aZEW;GZe_F7zI+?6YSJr3*Ug?G+Eu$7k6yXFc-a&drZmj7 zBg0EDL4s!cZ=n&%wbVW^DvEftP-Lxm*bl18B{?zBP*?wY>hAhLn1* zx<^Dqg;#fg0cmmV#`>@y^o5vs49I6HcVD~lW2dCEM^2F4SswP^9gjA)!cwD}7t2og zOT_5_(ZaU#5e|w;L1r|cDsZ2jkkd3Hi!93SCaAENqR>dtGIyBO4sCDhSCDG0d z9Y8))MX;^NTAn@Qa;q6TcyV*|H#`3^WHh_Lh&XZ8U(kx-5{E7z9I~Me5l^_Kb6TjANvon9 zpQ5pS@Z@tBRK9>B{*Up6flrAOCgB4eRA4KVTPB@)OL5RFyimi-ve{_OPXFI_g`G zc6QDt64~%wRQr`q8%A|8xmY0DfkrYBq-v2#s{^gFuk-^71My zDtbvEJg%!tJKhy(nT{)bZ}d+1G&4IZ-IfRLN8sA5zROL%;d(z&DTV7wWzf0^%B`qK zvJV|@ne8v$gv4&JDMW2+YYU7qPMQDymqy9SO7gs(pjmliH>1-(-Nw@fukCOj5q|_G)QsGr)R5r*8^hnds;+w#KmE1Ku(t zWPK2_eeW$j9@r!w*FmGj0S|N74@VO0s%BGo_FCl3zKAm02QMRyjXz}gjnvv|pgD65 zpSZ1d>%x|{h*m&=iok=KL>Ry4YjXqD>5PA=s0`9kgN>2UCx@oYZf*aXdCae7NzC=r zp5R&Hfid0MUG1s;_1P3C=Yzm8Ve1KOwkV5kI`8umZ`X;b9wq^EO6^W&6x@;qvd3!x z<#!IGR1lxX$dcI_a8(~%OB+f0+^9BF+3tK|#C>P*YU(9gmph)Zvh_S8+q8LFQBje9 zVHZ-bu1vrEr)0N!+|Qr)19V>^jM$ij4!AdKJOPkp>C0vQ`WymzesUg6CZMv|-R$pn-DGP`xIG)QlQmJxY=mpix5Yk@VmRgb}5R*D1&u=*SPq2x@%8isb6P;ATrx^ zWuHl)3_VVg3EBe{u>-Yp(aA88v3E^OTAG@PWUU)Im}FK|QsMxz?diZBcF#WnD!BFl z05ZU;`lw`wtraRv@@u}#a3Ch3Q6Q0W0PgCl)`tlu*_`}^egd&8D-~=4Qxr>PA?sSl zsEmWaE~tW(syt}JZIHu`4rm1g)ZUL}OCT_Nr_>C)0ectYz$3;Y$fZuydKk{g9GT-k z$%6A|i`c07p&_>9cUmHDsxqx3`_KWLJ7!mmvJ>UZS#>roxa3_+_+T(adMV`g_|A#U z^c_tK)@LMsDKm>Be0sW2rj|QtCUAf~sr;Jg0cGF!;pVqJWDok*VBIveXJs0=>;G;QQfqa^9L$>pdHgG9SF<%~zFqi~Z zbAvI8&upg=v<-L+RYez8=KI?lreG#;(&?6XsSqE%lK!Y`c`p&(r07oyiLKvTNp97* z%RBYYz~4kRg=~MWw~`&?i1{!L-YxOCW(ii|23OYNd(_A+xcL z)#NfVrb0*r0x20T1Mx@?P@B`hu_PJ+vmufs6GLDYd1^F{|BnwFDwc@v6@l-})UC_H z%d$|&e~j7w+p8_FQfQ9BNL4|HIZ`~0%kijTRYrjr3uIi$Kl0%{j+Fd>w~H627stgZ zUQ%&HIHI@F;nrK9a`dI_8hBz_#|Bx;A1$AnrU+h0#DPE*s)_HrwwgOX&wF99>BpT{ zB#V46=zvl*f{C3TH`?0TUJura>wLxj>8*7fp4B7DLt37mp7`3kpyp7)o~74j3=9mI z)zk_}V^u!T)r09kfhn(M7&=YLx-5(yI|uJU;PJiytA};vT{C>cdPRkN6Ky|89Fasc zwY0Rvk3909`&xo|<66aqFpkLuBPwtk3D|?7LOt#5?EFop$Oq}->grh!JemmVSizUU zm!-{3O{Y);5O3shqI*FC#%_Suz>A>oNZ`=nlUJg19 zemEeaqLK=lQJ)X4?5wQg&#qlUSN1?4pGh>3^O^0FnK`V!F@#mwnn))9Ty@fbBTcMy z$wCh!9Du)XR_xi8rz3$v1h1y|(Bm+5c6Lq=c|%|loaycB>&u=CEZ|1yn=`Yqxt(8Z o{^x|DPk{fKIseC7{Eul&sWtUlf1y2rD@GuU^)KlWb#8_K16F{UA^-pY literal 0 HcmV?d00001 diff --git a/apps/lidar_odometry_step_1_legacy/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1_legacy/lidar_odometry_gui.cpp new file mode 100644 index 00000000..03bb2aff --- /dev/null +++ b/apps/lidar_odometry_step_1_legacy/lidar_odometry_gui.cpp @@ -0,0 +1,2833 @@ +#include +#include +#include +#include + +#include + +#include +#include + +#include "lidar_odometry.h" +#include "lidar_odometry_utils.h" + +#include +#include +#include +#include +#include +#include + +#include "toml_io.h" +#include +#include +#include +#include +#include +#include + +#ifdef _WIN32 +#include "resource.h" +#include +#endif + +/////////////////////////////////////////////////////////////////////////////////// + +// This is LiDAR odometry (step 1) +// This program calculates trajectory based on IMU and LiDAR data provided by MANDEYE mobile mapping system +// https://github.com/JanuszBedkowski/mandeye_controller The output is a session proving trajekctory and point clouds that can be further +// processed by "multi_view_tls_registration" program. + +std::string winTitle = std::string("Step 1 (Lidar odometry) ") + HDMAPPING_CONFIGURE_PROJECT_VERSION; + +std::vector infoLines = { + "This program is first step in MANDEYE process.", + "", + "It results trajectory and point clouds as single session for 'multi_view_tls_registration_step_2' program.", + "", + "Next step will be to load session.json file with 'multi_view_tls_registration_step_2' program." +}; + +// App specific shortcuts (Type and Shortcut are just for easy reference) +static const std::vector appShortcuts = { { "Normal keys", "A", "" }, + { "", "Ctrl+A", "" }, + { "", "B", "" }, + { "", "Ctrl+B", "" }, + { "", "C", "" }, + { "", "Ctrl+C", "" }, + { "", "D", "" }, + { "", "Ctrl+D", "" }, + { "", "E", "" }, + { "", "Ctrl+E", "" }, + { "", "F", "" }, + { "", "Ctrl+F", "" }, + { "", "G", "" }, + { "", "Ctrl+G", "" }, + { "", "H", "" }, + { "", "Ctrl+H", "" }, + { "", "I", "" }, + { "", "Ctrl+I", "" }, + { "", "J", "" }, + { "", "Ctrl+K", "" }, + { "", "K", "" }, + { "", "Ctrl+K", "" }, + { "", "L", "" }, + { "", "Ctrl+L", "" }, + { "", "M", "" }, + { "", "Ctrl+M", "" }, + { "", "N", "" }, + { "", "Ctrl+N", "" }, + { "", "O", "" }, + { "", "Ctrl+O", "Open data" }, + { "", "P", "" }, + { "", "Ctrl+P", "" }, + { "", "Q", "" }, + { "", "Ctrl+Q", "" }, + { "", "R", "" }, + { "", "Ctrl+R", "" }, + { "", "Shift+R", "" }, + { "", "S", "" }, + { "", "Ctrl+S", "" }, + { "", "Ctrl+Shift+S", "" }, + { "", "T", "" }, + { "", "Ctrl+T", "" }, + { "", "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; + +bool is_settings_gui = false; +bool full_debug_messages = false; +NDT ndt; +bool show_reference_buckets_indoor = false; +bool show_reference_buckets_outdoor = false; +bool show_reference_points = false; +int dec_reference_points = 100; +bool show_initial_points = true; +bool show_trajectory = true; +bool show_trajectory_as_axes = false; +bool show_prediction_vectors = false; +bool intermediate_trajectory_prediction_axes = false; +bool show_covs_indoor = false; +bool show_covs_outdoor = false; +int dec_covs = 10; +bool simple_gui = true; +bool step_1_done = false; +bool step_2_done = false; +bool step_3_done = false; +bool calculations_failed = false; +bool show_without_filtered_buckets = false; +bool show_normal_vectors_indoor = false; +bool show_normal_vectors_outdoor = false; + +int lastPar = 1; + +std::vector worker_data; + +std::string working_directory = ""; +bool initial_transformation_gizmo = false; + +float m_gizmo[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; + +float x_displacement = 0.01; + +int index_begin = -1; +int index_end = -1; +bool gizmo_stretch_interval = false; +Eigen::Affine3d stretch_gizmo_m = Eigen::Affine3d::Identity(); + +LidarOdometryParams params; +std::vector csv_files; +std::vector sn_files; +std::string imuSnToUse; +Session session; + +std::vector> pointsPerFile; +Imu imu_data; +Trajectory trajectory; +std::atomic loRunning{ false }; +std::atomic loProgress{ 0.0 }; +std::atomic loPause{ false }; +std::chrono::time_point loStartTime; +std::atomic loElapsedSeconds{ 0.0 }; +std::atomic loEstimatedTimeRemaining{ 0.0 }; + +fs::path outwd; + +#if _WIN32 +#define DEFAULT_PATH "C:\\" +#else +#define DEFAULT_PATH "~" +#endif + +/////////////////////////////////////////////////////////////////////////////////// + +void set_lidar_odometry_default_params(LidarOdometryParams& params) +{ + params.decimation = 0.01; + params.in_out_params_indoor.resolution_X = 0.1; + params.in_out_params_indoor.resolution_Y = 0.1; + params.in_out_params_indoor.resolution_Z = 0.1; + + params.in_out_params_outdoor.resolution_X = 0.3; + params.in_out_params_outdoor.resolution_Y = 0.3; + params.in_out_params_outdoor.resolution_Z = 0.3; + + params.filter_threshold_xy_inner = 0.3; + params.filter_threshold_xy_outer = 70.0; + params.threshould_output_filter = 0.3; + + params.use_robust_and_accurate_lidar_odometry = false; + params.distance_bucket = 0.2; + params.polar_angle_deg = 10.0; + params.azimutal_angle_deg = 10.0; + params.robust_and_accurate_lidar_odometry_iterations = 20; + + params.max_distance_lidar = 70.0; + params.nr_iter = 1000; + params.sliding_window_trajectory_length_threshold = 200; + params.real_time_threshold_seconds = 10; +} + +// Helper function to format time in human-readable format +std::string formatTime(double seconds) +{ + if (seconds < 0) + return "Calculating..."; + + int hours = static_cast(seconds / 3600); + int minutes = static_cast((seconds - hours * 3600) / 60); + int secs = static_cast(seconds - hours * 3600 - minutes * 60); + + std::ostringstream oss; + + if (hours > 0) + { + oss << std::setw(2) << std::setfill('0') << hours << "h "; + } + if (minutes > 0 || hours > 0) + { + oss << std::setw(2) << std::setfill('0') << minutes << "m "; + } + + oss << std::setw(2) << std::setfill('0') << secs << "s"; + + return oss.str(); +} + +// Helper function to format estimated completion time +std::string formatCompletionTime(double remainingSeconds) +{ + if (remainingSeconds < 0) + return "Calculating..."; + + auto now = std::chrono::system_clock::now(); + auto estimatedCompletion = now + std::chrono::seconds(static_cast(remainingSeconds)); + auto completion_time_t = std::chrono::system_clock::to_time_t(estimatedCompletion); + + // Format as HH:MM:SS - Cross-platform time formatting + struct tm completion_tm; +#ifdef _WIN32 + localtime_s(&completion_tm, &completion_time_t); +#else + localtime_r(&completion_time_t, &completion_tm); +#endif + + char timeStr[32]; + strftime(timeStr, sizeof(timeStr), "%H:%M", &completion_tm); + + return std::string(timeStr); +} + +#if 0 +std::vector> get_batches_of_points(std::string laz_file, int point_count_threshold, conststd::vector& prev_points) +{ + std::vector> res_points; + std::vector points = load_point_cloud(laz_file, false, 0, 10000, {}); + + std::vector tmp_points = prev_points; + int counter = tmp_points.size(); + for (size_t i = 0; i < points.size(); i++) + { + counter++; + tmp_points.push_back(points[i]); + if (counter > point_count_threshold) + { + res_points.push_back(tmp_points); + tmp_points.clear(); + counter = 0; + } + } + + if (tmp_points.size() > 0) + { + res_points.push_back(tmp_points); + } + return res_points; +} +#endif + +void find_best_stretch( + std::vector points, std::vector timestamps, std::vector poses, std::string fn1, std::string fn2) +{ + for (size_t i = 0; i < points.size(); i++) + { + auto lower = std::lower_bound(timestamps.begin(), timestamps.end(), points[i].timestamp); + points[i].index_pose = std::distance(timestamps.begin(), lower); + } + + std::set indexes; + + for (size_t i = 0; i < points.size(); i++) + { + indexes.insert(points[i].index_pose); + } + // build trajectory + std::vector trajectory; + std::vector ts; + + for (auto& s : indexes) + { + trajectory.push_back(poses[s]); + ts.push_back(timestamps[s]); + } + + std::cout << "trajectory.size() " << trajectory.size() << std::endl; + // Sleep(2000); + + std::vector points_reindexed = points; + for (size_t i = 0; i < points_reindexed.size(); i++) + { + points_reindexed[i].index_pose = get_index_in_set(indexes, points[i].index_pose); + } + /// + std::vector best_trajectory = trajectory; + uint64_t min_buckets = ULLONG_MAX; + + for (double x = 0.0; x < 0.2; x += 0.0005) + { + std::vector trajectory_stretched; + + Eigen::Affine3d m_x_offset = Eigen::Affine3d::Identity(); + m_x_offset(0, 3) = x; + + Eigen::Affine3d m = trajectory[0]; + trajectory_stretched.push_back(m); + for (size_t i = 1; i < trajectory.size(); i++) + { + Eigen::Affine3d m_update = trajectory[i - 1].inverse() * trajectory[i] * (m_x_offset); + m = m * m_update; + trajectory_stretched.push_back(m); + } + + NDT::GridParameters rgd_params; + rgd_params.resolution_X = 0.3; + rgd_params.resolution_Y = 0.3; + rgd_params.resolution_Z = 0.3; + NDTBucketMapType my_buckets; + + std::vector points_global = points_reindexed; + + std::vector points_global2; + + for (auto& p : points_global) + { + if (p.point.z() > 0) + { + p.point = trajectory_stretched[p.index_pose] * p.point; + points_global2.push_back(p); + // if (p.point.norm() > 6 && p.point.norm() < 15) + //{ + // points_global.push_back(p); + // } + } + } + std::scoped_lock lock(params.mutex_buckets_indoor, params.mutex_buckets_outdoor); + + update_rgd(rgd_params, my_buckets, points_global2, trajectory_stretched[0].translation()); + + std::cout << "number of buckets [" << x << "]: " << my_buckets.size() << std::endl; + if (my_buckets.size() < min_buckets) + { + min_buckets = my_buckets.size(); + best_trajectory = trajectory_stretched; + } + } + + std::map trajectory_for_interpolation; + for (size_t i = 0; i < best_trajectory.size(); i++) + { + trajectory_for_interpolation[ts[i]] = best_trajectory[i].matrix(); + } + + std::vector pointcloud_global; + std::vector intensity; + std::vector timestamps_; + for (const auto& p : points_reindexed) + { + Eigen::Matrix4d pose = getInterpolatedPose(trajectory_for_interpolation, p.timestamp); + Eigen::Affine3d b; + b.matrix() = pose; + Eigen::Vector3d vec = b * p.point; + pointcloud_global.push_back(vec); + intensity.push_back(p.intensity); + timestamps_.push_back(p.timestamp); + } + + std::cout << "saving file: " << fn1 << std::endl; + exportLaz(fn1, pointcloud_global, intensity, timestamps_); + + pointcloud_global.clear(); + for (const auto& p : points_reindexed) + { + Eigen::Vector3d vec = trajectory[p.index_pose] * p.point; + pointcloud_global.push_back(vec); + } + exportLaz(fn2, pointcloud_global, intensity, timestamps_); +} + +#if 0 +void alternative_approach() +{ + int point_count_threshold = 10000; + + std::cout << "aternative_approach" << std::endl; + + std::vector input_file_names; + input_file_names = mandeye::fd::OpenFileDialog("Load las files", {}, true); + std::sort(std::begin(input_file_names), std::end(input_file_names)); + + std::vector csv_files; + std::vector laz_files; + std::for_each(std::begin(input_file_names), std::end(input_file_names), [&](const std::string& fileName) + { + if (fileName.ends_with(".laz") || fileName.ends_with(".las")) + { + laz_files.push_back(fileName); + } + if (fileName.ends_with(".csv")) + { + csv_files.push_back(fileName); + } }); + + std::cout << "imu files: " << std::endl; + for (const auto& fn : csv_files) + { + std::cout << fn << std::endl; + } + + std::cout << "loading imu" << std::endl; + Imu imu_data; + + std::for_each(std::begin(csv_files), std::end(csv_files), [&imu_data](const std::string& fn) + { + auto imu = load_imu(fn.c_str(), 0); + std::cout << fn << std::endl; + imu_data.insert(std::end(imu_data), std::begin(imu), std::end(imu)); }); + + std::map trajectory; + int counter = 1; + const float RAD_TO_DEG = 180.0f / static_cast(M_PI); + + if (params.use_vqf) + { + double avg_dt = 1.0 / 200.0; + if (imu_data.size() >= 2) + { + double t0 = std::get<0>(imu_data.front()).first; + double t1 = std::get<0>(imu_data.back()).first; + if (t1 > t0) + avg_dt = (t1 - t0) / static_cast(imu_data.size() - 1); + } + + VQFParams vqf_params = buildVQFParams(params); + VQF vqf(vqf_params, avg_dt); + + for (const auto& [timestamp_pair, gyr, acc] : imu_data) + { + const double g = 9.80665; + vqf_real_t gyr_vqf[3] = { static_cast(gyr.x()), static_cast(gyr.y()), static_cast(gyr.z()) }; + vqf_real_t acc_vqf[3] = { static_cast(acc.x()) * g, static_cast(acc.y()) * g, static_cast(acc.z()) * g }; + + vqf.update(gyr_vqf, acc_vqf); + + vqf_real_t quat[4]; + if (params.vqf_useMagnetometer) + vqf.getQuat9D(quat); + else + vqf.getQuat6D(quat); + Eigen::Quaterniond d(quat[0], quat[1], quat[2], quat[3]); + Eigen::Affine3d t{ Eigen::Matrix4d::Identity() }; + t.rotate(d); + trajectory[timestamp_pair.first] = t.matrix(); + counter++; + if (counter % 100 == 0) + { + Eigen::Vector3d euler = d.toRotationMatrix().eulerAngles(0, 1, 2) * (180.0 / M_PI); + std::cout << "Roll " << euler.x() << ", Pitch " << euler.y() << ", Yaw " << euler.z() << " [" << counter << " of " << imu_data.size() << "]" << std::endl; + } + } + } + else + { + FusionAhrs ahrs; + FusionAhrsInitialise(&ahrs); + if (params.fusionConventionNwu) ahrs.settings.convention = FusionConventionNwu; + else if (params.fusionConventionEnu) ahrs.settings.convention = FusionConventionEnu; + else if (params.fusionConventionNed) ahrs.settings.convention = FusionConventionNed; + ahrs.settings.gain = params.fusion_gain; + + bool first = true; + double last_ts = 0.0; + + for (const auto& [timestamp_pair, gyr, acc] : imu_data) + { + const FusionVector gyroscope = { static_cast(gyr.x() * RAD_TO_DEG), + static_cast(gyr.y() * RAD_TO_DEG), + static_cast(gyr.z() * RAD_TO_DEG) }; + const FusionVector accelerometer = { acc.x(), acc.y(), acc.z() }; + + if (first) + { + FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, 1.0f / 200.0f); + first = false; + } + else + { + float ts_diff = static_cast(timestamp_pair.first - last_ts); + if (ts_diff < 0.01f) + FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, ts_diff); + else + FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, 1.0f / 200.0f); + } + last_ts = timestamp_pair.first; + + FusionQuaternion quat = FusionAhrsGetQuaternion(&ahrs); + Eigen::Quaterniond d{ quat.element.w, quat.element.x, quat.element.y, quat.element.z }; + Eigen::Affine3d t{ Eigen::Matrix4d::Identity() }; + t.rotate(d); + trajectory[timestamp_pair.first] = t.matrix(); + counter++; + if (counter % 100 == 0) + { + const FusionEuler euler = FusionQuaternionToEuler(quat); + std::cout << "Roll " << euler.angle.roll << ", Pitch " << euler.angle.pitch << ", Yaw " << euler.angle.yaw + << " [" << counter << " of " << imu_data.size() << "]" << std::endl; + } + } + } + + /////////////////////////////////////////////////////////////////////////////// + std::cout << "point cloud file names" << std::endl; + for (const auto& fn : laz_files) + { + std::cout << fn << std::endl; + } + + std::vector prev_points; + std::vector> all_points; + std::vector> tmp_points = get_batches_of_points(laz_files[0], point_count_threshold, prev_points); + + for (size_t i = 0; i < tmp_points.size() - 1; i++) + { + all_points.push_back(tmp_points[i]); + } + + for (size_t i = 1; i < laz_files.size(); i++) + { + prev_points = tmp_points[tmp_points.size() - 1]; + tmp_points = get_batches_of_points(laz_files[i], point_count_threshold, prev_points); + for (size_t j = 0; j < tmp_points.size() - 1; j++) + { + all_points.push_back(tmp_points[j]); + } + } + + ////////// + std::vector timestamps; + std::vector poses; + for (const auto& t : trajectory) + { + timestamps.push_back(t.first); + Eigen::Affine3d m; + m.matrix() = t.second; + poses.push_back(m); + } + + for (size_t i = 0; i < all_points.size(); i++) + { + std::cout << all_points[i].size() << std::endl; + std::string fn1 = "C:/data/tmp/" + std::to_string(i) + "_best.laz"; + std::string fn2 = "C:/data/tmp/" + std::to_string(i) + "_original.laz"; + + find_best_stretch(all_points[i], timestamps, poses, fn1, fn2); + } +} +#endif + +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); + + std::cout << "intersection: " << pos.x() << " " << pos.y() << " " << pos.z() << std::endl; + + return pos; +} + +void step1(const std::atomic& loPause) +{ + std::string input_folder_name; + std::vector input_file_names; + input_folder_name = mandeye::fd::SelectFolder("Select Mandeye data folder"); + + std::cout << "Selected folder: '" << input_folder_name << std::endl; + + if (fs::exists(input_folder_name)) + { + std::string newTitle = winTitle + " - ..\\" + std::filesystem::path(input_folder_name).filename().string(); + glutSetWindowTitle(newTitle.c_str()); + + for (const auto& entry : fs::directory_iterator(input_folder_name)) + if (entry.is_regular_file()) + input_file_names.push_back(entry.path().string()); + + if (load_data(input_file_names, params, pointsPerFile, imu_data, full_debug_messages)) + { + working_directory = fs::path(input_file_names[0]).parent_path().string(); + calculate_trajectory(trajectory, imu_data, params, full_debug_messages); + if (compute_step_1(pointsPerFile, params, trajectory, worker_data, loPause)) + { + release_raw_clouds(pointsPerFile, params); // nothing after step 1 reads them + step_1_done = true; + } + else + std::cerr << "Lidar odometry step 1 failed (see the messages above)" << std::endl; + } + else + { + std::string message_info = "Problem with loading data from folder '" + input_folder_name + + "' (Pease check if folder exists). Program will close once You click OK!!!"; + std::cout << message_info << std::endl; + [[maybe_unused]] pfd::message message("Information", message_info.c_str(), pfd::choice::ok, pfd::icon::info); + message.result(); + + exit(1); + } + } +} + +void step2(const std::atomic& loPause) +{ + double ts_failure = 0.0; + if (compute_step_2(worker_data, params, ts_failure, loProgress, loPause, full_debug_messages)) + step_2_done = true; + else + { + for (size_t fileNo = 0; fileNo < csv_files.size(); fileNo++) + { + const std::string& imufn = csv_files.at(fileNo); + const std::string snFn = (fileNo >= sn_files.size()) ? ("") : (sn_files.at(fileNo)); + const auto idToSn = MLvxCalib::GetIdToSnMapping(snFn); + // GetId of Imu to use + int imuNumberToUse = MLvxCalib::GetImuIdToUse(idToSn, imuSnToUse); + auto imu = load_imu(imufn.c_str(), imuNumberToUse); + + if (imu.size() > 0) + { + if (std::get<0>(imu[imu.size() - 1]).first > ts_failure) + break; + } + std::cout << "file: '" << imufn << "' [OK]" << std::endl; + } + calculations_failed = true; + } +} + +void save_results(bool info, double elapsed_seconds) +{ + outwd = get_next_result_path(working_directory); + save_result(worker_data, params, outwd, elapsed_seconds); + if (info) + { + std::string message_info = "Results saved to folder: '" + outwd.string() + "'"; + std::cout << message_info << std::endl; + [[maybe_unused]] pfd::message message("Information", message_info.c_str(), pfd::choice::ok, pfd::icon::info); + message.result(); + } +} + +void settings_gui() +{ + if (ImGui::Begin("Settings", &is_settings_gui)) + { + ImGui::Checkbox("simple_gui", &simple_gui); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Show even more parameters"); + + ImGui::NewLine(); + + if (!loRunning) + { + // TaitBryanPose motion_model_correction; + ImGui::Text("motion_model_correction [deg]:"); + ImGui::PushItemWidth(ImGuiNumberWidth); + ImGui::InputDouble("om, rotation via X", ¶ms.motion_model_correction.om); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(omText); + ImGui::InputDouble("fi, rotation via Y", ¶ms.motion_model_correction.fi); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(fiText); + ImGui::InputDouble("ka, rotation via Z", ¶ms.motion_model_correction.ka); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(kaText); + ImGui::PopItemWidth(); + + if (ImGui::Button("Set example")) + { + params.motion_model_correction.om = 0.0; + params.motion_model_correction.fi = 0.05; + params.motion_model_correction.ka = 0.0; + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("motion_model_corrections for LiDAR X-axis (forward direction)"); + + ImGui::NewLine(); + + ImGui::Text("lidar_odometry_motion_model sigmas [m] / [deg]:"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("position/rotational uncertainties"); + + ImGui::PushItemWidth(ImGuiNumberWidth); + ImGui::InputDouble("x_1", ¶ms.lidar_odometry_motion_model_x_1_sigma_m, 0.0, 0.0, "%.4f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(xText); + ImGui::SameLine(); + ImGui::InputDouble("om_1", ¶ms.lidar_odometry_motion_model_om_1_sigma_deg, 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(omText); + + ImGui::InputDouble("y_1", ¶ms.lidar_odometry_motion_model_y_1_sigma_m, 0.0, 0.0, "%.4f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(yText); + ImGui::SameLine(); + ImGui::InputDouble("fi_1", ¶ms.lidar_odometry_motion_model_fi_1_sigma_deg, 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(fiText); + + ImGui::InputDouble("z_1", ¶ms.lidar_odometry_motion_model_z_1_sigma_m, 0.0, 0.0, "%.4f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(zText); + ImGui::SameLine(); + ImGui::InputDouble("ka_1", ¶ms.lidar_odometry_motion_model_ka_1_sigma_deg, 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(kaText); + + ImGui::PopItemWidth(); + + ImGui::Text("lidar_odometry_motion_model_fix_origin sigmas [m] / [deg]:"); + + ImGui::PushItemWidth(ImGuiNumberWidth); + ImGui::InputDouble("x_1##o", ¶ms.lidar_odometry_motion_model_fix_origin_x_1_sigma_m); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(xText); + ImGui::SameLine(); + ImGui::InputDouble("om_1##o", ¶ms.lidar_odometry_motion_model_fix_origin_om_1_sigma_deg); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(omText); + + ImGui::InputDouble("y_1##o", ¶ms.lidar_odometry_motion_model_fix_origin_y_1_sigma_m); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(yText); + ImGui::SameLine(); + ImGui::InputDouble("fi_1##o", ¶ms.lidar_odometry_motion_model_fix_origin_fi_1_sigma_deg); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(fiText); + + ImGui::InputDouble("z_1##o", ¶ms.lidar_odometry_motion_model_fix_origin_z_1_sigma_m); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(zText); + ImGui::SameLine(); + ImGui::InputDouble("ka_1##o", ¶ms.lidar_odometry_motion_model_fix_origin_ka_1_sigma_deg); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(kaText); + + ImGui::PopItemWidth(); + } + + ImGui::NewLine(); + + if (!simple_gui) + { + ImGui::SetNextItemWidth(ImGuiNumberWidth); + ImGui::InputInt("Threshold nr poses", ¶ms.threshold_nr_poses); + if (params.threshold_nr_poses < 1) + { + params.threshold_nr_poses = 1; + } + + ImGui::NewLine(); + } + + // ImGui::Checkbox("show_all_points", &show_all_points); + + if (calculations_failed) + { + ImGui::Text("CALCULATIONS FAILED... please read information in console"); + ImGui::End(); + return; + } + + ImGui::PushItemWidth(ImGuiNumberWidth); + ImGui::InputDouble("Filter threshold XY inner [m]", ¶ms.filter_threshold_xy_inner, 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + { + ImGui::BeginTooltip(); + ImGui::Text("all local points inside lidar xy_circle radius will be removed during load"); + ImGui::Text("Minimum value is given by Lidar's specs (Close Proximity Blind Zone)"); + ImGui::Text("Value can be higher to filter out close range permanent obstacles"); + ImGui::Text("e.g.: 0.1[m] for Livox Mid-360"); + ImGui::EndTooltip(); + } + ImGui::InputDouble("Filter threshold XY outer [m]", ¶ms.filter_threshold_xy_outer, 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + { + ImGui::BeginTooltip(); + ImGui::Text("all local points outside lidar xy_circle radius will be removed during load"); + ImGui::Text("Maximum value is given by Lidar's specs (Detection Range)"); + ImGui::Text("Value can be lower to adapt for different reflectivity or real world contrains"); + ImGui::Text("e.g.: 70[m] @ 80%% reflectivity for Livox Mid-360"); + ImGui::EndTooltip(); + } + ImGui::InputDouble("Threshold output filter [m]", ¶ms.threshould_output_filter, 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("all local points inside lidar xy_circle radius will be removed during save"); + ImGui::PopItemWidth(); + + if (!simple_gui) + { + ImGui::NewLine(); + ImGui::Text("NDT bucket size (inner/outer)"); + ImGui::PushItemWidth(ImGuiNumberWidth); + ImGui::InputDouble("##x", ¶ms.in_out_params_indoor.resolution_X, 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(xText); + if (params.in_out_params_indoor.resolution_X < 0.01) + { + params.in_out_params_indoor.resolution_X = 0.01; + } + ImGui::SameLine(); + ImGui::InputDouble("X##ndt", ¶ms.in_out_params_outdoor.resolution_X, 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(xText); + if (params.in_out_params_outdoor.resolution_X < 0.01) + { + params.in_out_params_outdoor.resolution_X = 0.01; + } + + ImGui::InputDouble("##y", ¶ms.in_out_params_indoor.resolution_Y, 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(yText); + if (params.in_out_params_indoor.resolution_Y < 0.01) + { + params.in_out_params_indoor.resolution_Y = 0.01; + } + ImGui::SameLine(); + ImGui::InputDouble("Y##ndt", ¶ms.in_out_params_outdoor.resolution_Y, 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(yText); + if (params.in_out_params_outdoor.resolution_Y < 0.01) + { + params.in_out_params_outdoor.resolution_Y = 0.01; + } + + ImGui::InputDouble("##z", ¶ms.in_out_params_indoor.resolution_Z, 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(zText); + if (params.in_out_params_indoor.resolution_Z < 0.01) + { + params.in_out_params_indoor.resolution_Z = 0.01; + } + ImGui::SameLine(); + ImGui::InputDouble("Z##ndt", ¶ms.in_out_params_outdoor.resolution_Z, 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(zText); + if (params.in_out_params_outdoor.resolution_Z < 0.01) + { + params.in_out_params_outdoor.resolution_Z = 0.01; + } + + ImGui::NewLine(); + + ImGui::InputDouble("Downsampling", ¶ms.decimation, 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Larger value of downsampling better performance, but worse accuracy"); + + ImGui::InputDouble("Max distance of processed points [m]", ¶ms.max_distance_lidar, 0.0, 0.0, "%.2f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Local LiDAR coordinates"); + + ImGui::InputInt("Number of iterations", ¶ms.nr_iter); + ImGui::InputDouble( + "Sliding window trajectory length threshold [m]", ¶ms.sliding_window_trajectory_length_threshold, 0.0, 0.0, "%.2f"); + ImGui::InputInt("Threshold initial points", ¶ms.threshold_initial_points); + + ImGui::NewLine(); + + // AHRS type selection + ImGui::Checkbox("Use VQF (instead of Fusion/Madgwick)", ¶ms.use_vqf); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Unchecked = Fusion (Madgwick complementary filter, default)\nChecked = VQF (Versatile Quaternion-based Filter)"); + + if (!params.use_vqf) + { + // Fusion-specific parameters + static int fusionConvention; // 0=NWU, 1=ENU, 2=NED + if (fusionConvention < 0 || fusionConvention > 2) + fusionConvention = 0; + + ImGui::Text("Fusion convention: "); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Coordinate system conventions for sensor fusion defining how the axes are oriented relative to world frame"); + + ImGui::SameLine(); + ImGui::RadioButton("NWU", &fusionConvention, 0); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("North West Up"); + ImGui::SameLine(); + ImGui::RadioButton("ENU", &fusionConvention, 1); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("East North Up"); + ImGui::SameLine(); + ImGui::RadioButton("NED", &fusionConvention, 2); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("North East Down"); + + params.fusionConventionNwu = (fusionConvention == 0); + params.fusionConventionEnu = (fusionConvention == 1); + params.fusionConventionNed = (fusionConvention == 2); + + ImGui::InputDouble("Fusion gain", ¶ms.fusion_gain, 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Complementary filter gain (0-1). Higher = more accelerometer trust."); + } + + ImGui::NewLine(); + + ImGui::Checkbox("Use motion from previous step", ¶ms.use_motion_from_previous_step); + ImGui::Checkbox("Lidar-only odometry (no IMU)", ¶ms.lidar_only_odometry); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Predict each window's rotation and translation from the previous one (constant velocity) instead of from " + "the IMU. For recordings without a usable IMU, e.g. a synthetic stationary one."); + ImGui::Checkbox("Use IMU preintegration", ¶ms.use_imu_preintegration); + if (params.use_imu_preintegration) + { + const char* methods[] = { "Euler, no gravity comp., SM velocity", "Trapezoidal, no gravity comp., SM velocity", + "Euler, gravity comp., SM velocity", "Trapezoidal, gravity comp., SM velocity", + "Kalman, gravity comp., SM velocity", "Euler, gravity comp., AHRS velocity", + "Trapezoidal, gravity comp., AHRS velocity", "Kalman, gravity comp., AHRS velocity" }; + ImGui::Combo("IMU preintegration method", ¶ms.imu_preintegration_method, methods, IM_ARRAYSIZE(methods)); + } + if (params.use_vqf) + { + ImGui::InputDouble("VQF tauAcc [s]", ¶ms.vqf_tauAcc, 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + { + ImGui::BeginTooltip(); + ImGui::Text("VQF accelerometer time constant (tauAcc) in seconds."); + ImGui::Text("Controls how strongly accelerometer corrects the gyroscope-based orientation."); + ImGui::Text("Higher = more gyro trust (stable but may drift). Lower = more acc trust (noisy but no drift)."); + ImGui::EndTooltip(); + } + + if (ImGui::TreeNode("VQF Gyro Bias Estimation")) + { + ImGui::Checkbox("Motion bias estimation", ¶ms.vqf_motionBiasEstEnabled); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Enables gyroscope bias estimation during motion phases,\nbased on the inclination correction only (without " + "magnetometer)."); + + if (params.vqf_motionBiasEstEnabled) + { + ImGui::InputDouble("Bias sigma motion [deg/s]", ¶ms.vqf_biasSigmaMotion, 0.0, 0.0, "%.4f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Std dev of converged bias estimation uncertainty during motion.\nDetermines trust on motion bias " + "estimation " + "updates.\nSmall value leads to fast convergence. Default: 0.1"); + ImGui::InputDouble("Bias vertical forgetting", ¶ms.vqf_biasVerticalForgettingFactor, 0.0, 0.0, "%.6f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Forgetting factor for unobservable bias in vertical direction during motion.\nGyro bias is not observable " + "vertically without magnetometer.\nRelative weight of artificial zero measurement ensuring\nbias estimate " + "decays to zero. Default: 0.0001"); + } + + ImGui::InputDouble("Bias sigma init [deg/s]", ¶ms.vqf_biasSigmaInit, 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Std dev of the initial bias estimation uncertainty. Default: 0.5 deg/s"); + ImGui::InputDouble("Bias forgetting time [s]", ¶ms.vqf_biasForgettingTime, 0.0, 0.0, "%.1f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Time in which bias estimation uncertainty increases from 0 to 0.1 deg/s.\nDetermines the system noise assumed " + "by " + "the Kalman filter. Default: 100.0"); + ImGui::InputDouble("Bias clip [deg/s]", ¶ms.vqf_biasClip, 0.0, 0.0, "%.2f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Maximum expected gyroscope bias.\nUsed to clip bias estimate and measurement error in update step.\nAlso used " + "by " + "rest detection to not regard large constant angular rate as rest.\nDefault: 2.0"); + + ImGui::Checkbox("Rest bias estimation", ¶ms.vqf_restBiasEstEnabled); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Enables rest detection and gyroscope bias estimation during rest phases.\nDuring rest, gyro bias is estimated " + "from low-pass filtered gyro readings."); + + if (params.vqf_restBiasEstEnabled) + { + ImGui::InputDouble("Bias sigma rest [deg/s]", ¶ms.vqf_biasSigmaRest, 0.0, 0.0, "%.4f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Std dev of converged bias estimation uncertainty during rest.\nDetermines trust on rest bias estimation " + "updates.\nSmall value leads to fast convergence. Default: 0.03"); + ImGui::InputDouble("Rest min time [s]", ¶ms.vqf_restMinT, 0.0, 0.0, "%.2f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Time threshold for rest detection.\nRest is detected when measurements have been close to\nthe low-pass " + "filtered reference for the given time. Default: 1.5"); + ImGui::InputDouble("Rest filter tau [s]", ¶ms.vqf_restFilterTau, 0.0, 0.0, "%.2f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Time constant for the second-order Butterworth low-pass filter\nused to obtain the reference for rest " + "detection. Default: 0.5"); + ImGui::InputDouble("Rest threshold gyro [deg/s]", ¶ms.vqf_restThGyr, 0.0, 0.0, "%.2f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Angular velocity threshold for rest detection.\nDeviation norm between measurement and reference must be " + "below threshold.\nEach component must also be below biasClip. Default: 2.0"); + ImGui::InputDouble("Rest threshold acc [m/s2]", ¶ms.vqf_restThAcc, 0.0, 0.0, "%.2f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Acceleration threshold for rest detection.\nDeviation norm between measurement and reference must be " + "below threshold.\nDefault: 0.5"); + } + + ImGui::TreePop(); + } + + ImGui::Checkbox("Use magnetometer", ¶ms.vqf_useMagnetometer); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Enable 9D mode (gyro+acc+mag) for absolute heading correction.\nDefault: off (6D mode, gyro+acc only, heading " + "from " + "gyro integration)."); + + if (params.vqf_useMagnetometer && ImGui::TreeNode("VQF Magnetometer")) + { + ImGui::InputDouble("tauMag [s]", ¶ms.vqf_tauMag, 0.0, 0.0, "%.2f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Time constant for magnetometer update.\nSmall values imply trust on magnetometer, large values trust on " + "gyroscope.\nCorresponds to cutoff frequency of first-order LP filter\nfor heading correction. Default: 9.0"); + ImGui::Checkbox("Mag disturbance rejection", ¶ms.vqf_magDistRejectionEnabled); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Enables magnetic disturbance detection and rejection.\nFor short disturbances, mag correction is fully " + "disabled.\nFor long disturbances (>magMaxRejectionTime), correction uses\nincreased time constant " + "(magRejectionFactor)."); + ImGui::InputDouble("Mag current tau [s]", ¶ms.vqf_magCurrentTau, 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Time constant for current norm/dip value in disturbance detection.\nFast LP filter for robustness with noisy " + "or " + "async mag measurements.\nSet to -1 to disable. Default: 0.05"); + ImGui::InputDouble("Mag ref tau [s]", ¶ms.vqf_magRefTau, 0.0, 0.0, "%.1f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Time constant for adjustment of the magnetic field reference.\nAllows reference to converge to observed " + "undisturbed field. Default: 20.0"); + ImGui::InputDouble("Mag norm threshold", ¶ms.vqf_magNormTh, 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Relative threshold for magnetic field strength for disturbance detection.\nRelative to the reference norm. " + "Default: 0.1 (10%%)"); + ImGui::InputDouble("Mag dip threshold [deg]", ¶ms.vqf_magDipTh, 0.0, 0.0, "%.1f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Threshold for the magnetic field dip angle for disturbance detection. Default: 10.0"); + ImGui::InputDouble("Mag new time [s]", ¶ms.vqf_magNewTime, 0.0, 0.0, "%.1f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Duration after which to accept a different homogeneous magnetic field.\nNew reference accepted when within " + "magNormTh and magDipTh for this time.\nOnly phases with sufficient movement (magNewMinGyr) count. Default: " + "20.0"); + ImGui::InputDouble("Mag new first time [s]", ¶ms.vqf_magNewFirstTime, 0.0, 0.0, "%.1f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Duration after which to accept a homogeneous magnetic field for the first time.\nUsed instead of magNewTime " + "when " + "no current estimate exists,\nto allow faster initial reference acquisition. Default: 5.0"); + ImGui::InputDouble("Mag new min gyro [deg/s]", ¶ms.vqf_magNewMinGyr, 0.0, 0.0, "%.1f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Minimum angular velocity needed to count time for new mag field acceptance.\nPeriods with angular velocity " + "norm " + "below this threshold\ndo not count towards magNewTime. Default: 20.0"); + ImGui::InputDouble("Mag min undisturbed [s]", ¶ms.vqf_magMinUndisturbedTime, 0.0, 0.0, "%.2f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Minimum duration within thresholds after which to regard\nthe field as undisturbed again. Default: 0.5"); + ImGui::InputDouble("Mag max rejection [s]", ¶ms.vqf_magMaxRejectionTime, 0.0, 0.0, "%.1f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Maximum duration of full magnetic disturbance rejection.\nUp to this duration, heading correction is fully " + "disabled\nand tracked by gyroscope only. After this, correction uses\nincreased time constant " + "(magRejectionFactor). Default: 60.0"); + ImGui::InputDouble("Mag rejection factor", ¶ms.vqf_magRejectionFactor, 0.0, 0.0, "%.2f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Factor by which to slow heading correction during long disturbed phases.\nAfter magMaxRejectionTime of full " + "rejection, correction uses\nthis factor to increase the time constant. Default: 2.0"); + ImGui::TreePop(); + } + } // end if (params.use_vqf) + + ImGui::PopItemWidth(); + } + ImGui::NewLine(); + if (!step_1_done) + { + if (ImGui::Button("Load data")) + { + loStartTime = std::chrono::system_clock::now(); + + step1(loPause); + std::cout << "Load data done please click 'Compute all' to continue calculations" << std::endl; + + std::chrono::duration elapsed = std::chrono::system_clock::now() - loStartTime; + double elapsedSeconds = elapsed.count(); + + std::cout << "Elapsed time: " << formatTime(elapsedSeconds).c_str() << std::endl; + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Select folder containing IMU *.csv and LiDAR *.laz files produced by MANDEYE (e.g.: 'continousScanning_*')"); + } + if (step_1_done && !step_2_done) + { + if (ImGui::Button("Compute all")) + step2(loPause); + + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Press this button for automatic LiDAR odometry calculation -> it will produce trajectory"); + } + + if (step_1_done && step_2_done) + { + if (ImGui::Button("Point cloud consistency and trajectory smoothness")) + run_consistency(worker_data, params); + if (ImGui::IsItemHovered()) + { + ImGui::BeginTooltip(); + ImGui::Text("This process makes trajectory smooth, point cloud will be more consistent"); + ImGui::Text("Press optionally before pressing 'Save result'"); + ImGui::EndTooltip(); + } + + ImGui::SameLine(); + ImGui::Checkbox("Use multiple Gaussians for each bucket", ¶ms.use_mutliple_gaussian); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Multiple Gaussians suppose to work better in floor plan indoor scenarios (multiple neighbouring rooms)"); + + if (ImGui::Button("Save result")) + save_results(true, 0.0); + if (ImGui::IsItemHovered()) + { + ImGui::BeginTooltip(); + ImGui::Text("Press this button for saving resulting trajectory and point clouds"); + ImGui::Text("as single session for 'multi_view_tls_registration_step_2' program"); + ImGui::EndTooltip(); + } + } + if (step_1_done && step_2_done) + { + if (ImGui::Button("Save all point clouds to single las/laz file")) + { + const auto output_file_name = mandeye::fd::SaveFileDialog("Save las/laz file", mandeye::fd::LAS_LAZ_filter, ".laz"); + + if (output_file_name.size() > 0) + { + session.fill_session_from_worker_data(worker_data, false, true, true, params.threshould_output_filter); + // save_all_to_las(session, output_file_name, false); + save_all_to_las(session, output_file_name, true, true); + } + } + } + if (!simple_gui) + { + ImGui::NewLine(); + ImGui::Checkbox("Use robust and accurate lidar odometry", ¶ms.use_robust_and_accurate_lidar_odometry); + + if (params.use_robust_and_accurate_lidar_odometry) + { + ImGui::PushItemWidth(ImGuiNumberWidth); + ImGui::InputDouble("Distance bucket [m]", ¶ms.distance_bucket, 0.0, 0.0, "%.3f"); + ImGui::InputDouble("Polar angle [deg]", ¶ms.polar_angle_deg, 0.0, 0.0, "%.3f"); + ImGui::InputDouble("Azimutal angle [deg]", ¶ms.azimutal_angle_deg, 0.0, 0.0, "%.3f"); + ImGui::InputInt("Number of iterations", ¶ms.robust_and_accurate_lidar_odometry_iterations); + // ImGui::InputDouble("Max distance lidar", ¶ms.max_distance_lidar); + ImGui::PopItemWidth(); + } + + ImGui::NewLine(); + + ImGui::Text("Rigid ICP using spherical coordinates"); + ImGui::PushItemWidth(ImGuiNumberWidth); + ImGui::InputDouble("Distance bucket [m]##ricp", ¶ms.distance_bucket_rigid_sf, 0.0, 0.0, "%.3f"); + ImGui::InputDouble("Polar angle [deg]##ricp", ¶ms.polar_angle_deg_rigid_sf, 0.0, 0.0, "%.3f"); + ImGui::InputDouble("azimutal angle [deg]##ricp", ¶ms.azimutal_angle_deg_rigid_sf, 0.0, 0.0, "%.3f"); + ImGui::InputInt("Number of iterations##ricp", ¶ms.robust_and_accurate_lidar_odometry_rigid_sf_iterations); + ImGui::InputDouble("Max distance [m]##ricp", ¶ms.max_distance_lidar_rigid_sf, 0.0, 0.0, "%.2f"); + ImGui::PopItemWidth(); + + ImGui::NewLine(); + + ImGui::Text("Rigid spherical feature sigmas [m] / [deg]:"); + ImGui::PushItemWidth(ImGuiNumberWidth); + ImGui::InputDouble("x##rgdsfs", ¶ms.rgd_sf_sigma_x_m, 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(xText); + ImGui::SameLine(); + ImGui::InputDouble("om##rgdsfs", ¶ms.rgd_sf_sigma_om_deg, 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(omText); + + ImGui::InputDouble("y##rgdsfs", ¶ms.rgd_sf_sigma_y_m, 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(yText); + ImGui::SameLine(); + ImGui::InputDouble("fi##rgdsfs", ¶ms.rgd_sf_sigma_fi_deg, 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(fiText); + + ImGui::InputDouble("z##rgdsfs", ¶ms.rgd_sf_sigma_z_m, 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(zText); + ImGui::SameLine(); + ImGui::InputDouble("ka##rgdsfs", ¶ms.rgd_sf_sigma_ka_deg, 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(kaText); + + ImGui::PopItemWidth(); + + ImGui::NewLine(); + + if (ImGui::Button("Save trajectory to ASCII (x y z)")) + { + std::string output_file_name = ""; + output_file_name = mandeye::fd::SaveFileDialog("Save trajectory", {}, ""); + std::cout << "File to save: '" << output_file_name << "'" << std::endl; + + if (output_file_name.size() > 0) + { + save_trajectory_to_ascii(worker_data, output_file_name); + } + } + + ImGui::NewLine(); + ImGui::Text("Reference point clouds for initial alignment:"); + + static std::vector input_file_names; + + if (ImGui::Button("Load laz/las files")) + { + input_file_names = mandeye::fd::OpenFileDialog("Load laz/las files", mandeye::fd::LAS_LAZ_filter, true); + if (input_file_names.size() > 0) + { + show_reference_points = true; + load_reference_point_clouds(input_file_names, params); + } + } + + if (ImGui::Button("Set parameters for drone with Ouster (https://ntu-aris.github.io/ntu_viral_dataset/)")) + { + params.decimation = 1.0; + params.in_out_params_indoor.resolution_X = 1.0; + params.in_out_params_indoor.resolution_Y = 1.0; + params.in_out_params_indoor.resolution_Z = 1.0; + + params.in_out_params_outdoor.resolution_X = 2.0; + params.in_out_params_outdoor.resolution_Y = 2.0; + params.in_out_params_outdoor.resolution_Z = 2.0; + } + + if (!input_file_names.empty()) + { + ImGui::Checkbox("Show points", &show_reference_points); + ImGui::SameLine(); + // ImGui::Checkbox("Show buckets ", &show_reference_buckets); + ImGui::SetNextItemWidth(ImGuiNumberWidth); + ImGui::InputInt("Downsamplingn###ref", &dec_reference_points); + + if (ImGui::Button("Filter reference buckets")) + { + filter_reference_buckets(params); + } + } + + if (params.initial_points.size() > 0) + { + ImGui::NewLine(); + ImGui::Text("Manipulate initial transformation:"); + ImGui::Checkbox("Show gizmo", &initial_transformation_gizmo); + // gizmo_stretch_interval + if (initial_transformation_gizmo) + { + m_gizmo[0] = (float)params.m_g(0, 0); + m_gizmo[1] = (float)params.m_g(1, 0); + m_gizmo[2] = (float)params.m_g(2, 0); + m_gizmo[3] = (float)params.m_g(3, 0); + m_gizmo[4] = (float)params.m_g(0, 1); + m_gizmo[5] = (float)params.m_g(1, 1); + m_gizmo[6] = (float)params.m_g(2, 1); + m_gizmo[7] = (float)params.m_g(3, 1); + m_gizmo[8] = (float)params.m_g(0, 2); + m_gizmo[9] = (float)params.m_g(1, 2); + m_gizmo[10] = (float)params.m_g(2, 2); + m_gizmo[11] = (float)params.m_g(3, 2); + m_gizmo[12] = (float)params.m_g(0, 3); + m_gizmo[13] = (float)params.m_g(1, 3); + m_gizmo[14] = (float)params.m_g(2, 3); + m_gizmo[15] = (float)params.m_g(3, 3); + } + else + { + if (ImGui::Button("Align to reference")) + { + for (int i = 0; i < 30; i++) + { + align_to_reference(params.in_out_params_indoor, params.initial_points, params.m_g, params.buckets_indoor); + } + } + } + } + + if (worker_data.size() > 0) + { + ImGui::NewLine(); + + ImGui::Text("Scans selection:"); + + 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 = std::min(index_end, static_cast(worker_data.size() - 1)); + + ImGui::SameLine(); + ImGui::Text(" to: "); + ImGui::SameLine(); + + int prev = index_end; + ImGui::SliderInt("##ts", &index_end, index_begin, static_cast(worker_data.size() - 1)); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("max %zu", worker_data.size() - 1); + ImGui::SameLine(); + ImGui::InputInt("##ti", &index_end, 1, 5); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("max %zu", worker_data.size() - 1); + if (index_end < index_begin) + index_end = index_begin; + if (index_end >= worker_data.size() - 1) + index_end = worker_data.size() - 1; + if (prev != index_end) + { + stretch_gizmo_m = worker_data[index_end].intermediate_trajectory[0]; + } + + ImGui::PopItemWidth(); + + ImGui::Text("Selection: "); + if (ImGui::Button("Select all")) + { + for (size_t k = 0; k < worker_data.size(); k++) + { + worker_data[k].show = true; + } + } + ImGui::SameLine(); + if (ImGui::Button("Unselect")) + { + for (size_t k = 0; k < worker_data.size(); k++) + { + worker_data[k].show = false; + } + } + + ImGui::Text("Select scans from range:"); + ImGui::SameLine(); + if (ImGui::Button("")) + { + for (size_t k = 0; k < worker_data.size(); k++) + worker_data[k].show = (k >= index_begin && k <= index_end); + } + + if (ImGui::Button("")) + { + if (index_begin > 1 && index_end > 1) + { + index_begin--; + index_end--; + + for (size_t k = 0; k < worker_data.size(); k++) + worker_data[k].show = (k >= index_begin && k <= index_end); + } + } + ImGui::SameLine(); + if (ImGui::Button("")) + { + if (index_begin + 1 < worker_data.size() && index_end + 1 < worker_data.size()) + { + index_begin++; + index_end++; + + for (size_t k = 0; k < worker_data.size(); k++) + worker_data[k].show = (k >= index_begin && k <= index_end); + } + } + + if (ImGui::Button("from - 10, to - 10>")) + { + if (index_begin > 10 && index_end > 10) + { + index_begin -= 10; + index_end -= 10; + + for (size_t k = 0; k < worker_data.size(); k++) + worker_data[k].show = (k >= index_begin && k <= index_end); + } + } + ImGui::SameLine(); + if (ImGui::Button("")) + { + if (index_begin + 10 < worker_data.size() && index_end + 10 < worker_data.size()) + { + index_begin += 10; + index_end += 10; + + for (size_t k = 0; k < worker_data.size(); k++) + worker_data[k].show = (k >= index_begin && k <= index_end); + } + } + + if (ImGui::Button("Export selected scans")) + { + auto output_file_name = mandeye::fd::SaveFileDialog("Save las/laz file", mandeye::fd::LAS_LAZ_filter, ".laz"); + Eigen::Affine3d pose; + if (output_file_name.size() > 0) + { + session.fill_session_from_worker_data(worker_data, true, false, false, params.threshould_output_filter); + save_all_to_las(session, output_file_name, false, true); + } + // TODO: give value to pose even if output_file_name is wrong + + std::cout << "----------------------------------------" << std::endl; + std::cout << "please add following lines to RESSO file:" << std::endl; + std::cout << fs::path(output_file_name).filename() << "(!!!please remove brackets!!!)" << std::endl; + std::cout << pose.matrix() << std::endl; + + std::cout << "example RESSO file" << std::endl; + std::cout << ".................................................." << std::endl; + std::cout << "3" << std::endl + << "scan_0.laz" << std::endl + << "0.999775 0.000552479 -0.0212158 -0.0251188" << std::endl + << "0.000834612 0.997864 0.0653156 -0.0381429" << std::endl + << "0.0212066 - 0.0653186 0.997639 -0.000757752" + << "0 0 0 1" << std::endl + << "scan_1.laz" << std::endl + << "0.999783 0.00178963 -0.0207603 -0.0309683" << std::endl + << "-0.000467341 0.99798 0.0635239 -0.0517512" << std::endl + << "0.0208321 -0.0635004 0.997764 0.00331449" << std::endl + << "0 0 0 1" << std::endl + << "scan_2.laz" << std::endl + << "0.999783 0.00163449 -0.0207736 -0.0309985" << std::endl + << "-0.000312224 0.997982 0.0634957 -0.0506113" << std::endl + << "0.0208355 -0.0634754 0.997766 0.0028499" << std::endl + << "0 0 0 1" << std::endl; + std::cout << "................................................." << std::endl; + } + + ImGui::SameLine(); + + if (ImGui::Button("Save RESSO file")) + { + auto output_file_name = mandeye::fd::SaveFileDialog("Save RESSO file", {}, ""); + std::cout << "RESSO file to save: '" << output_file_name << "'" << std::endl; + + if (output_file_name.size() > 0) + { + session.point_clouds_container.save_poses(fs::path(output_file_name).string(), false); + } + } + + if (index_end > index_begin) + { + ImGui::Separator(); + ImGui::Checkbox("Gizmo_stretch_interval", &gizmo_stretch_interval); + } + + // gizmo_stretch_interval + if (gizmo_stretch_interval) + { + if (index_end < worker_data.size()) + { + if (worker_data[index_end].intermediate_trajectory.size() > 0) + { + m_gizmo[0] = (float)stretch_gizmo_m(0, 0); + m_gizmo[1] = (float)stretch_gizmo_m(1, 0); + m_gizmo[2] = (float)stretch_gizmo_m(2, 0); + m_gizmo[3] = (float)stretch_gizmo_m(3, 0); + m_gizmo[4] = (float)stretch_gizmo_m(0, 1); + m_gizmo[5] = (float)stretch_gizmo_m(1, 1); + m_gizmo[6] = (float)stretch_gizmo_m(2, 1); + m_gizmo[7] = (float)stretch_gizmo_m(3, 1); + m_gizmo[8] = (float)stretch_gizmo_m(0, 2); + m_gizmo[9] = (float)stretch_gizmo_m(1, 2); + m_gizmo[10] = (float)stretch_gizmo_m(2, 2); + m_gizmo[11] = (float)stretch_gizmo_m(3, 2); + m_gizmo[12] = (float)stretch_gizmo_m(0, 3); + m_gizmo[13] = (float)stretch_gizmo_m(1, 3); + m_gizmo[14] = (float)stretch_gizmo_m(2, 3); + m_gizmo[15] = (float)stretch_gizmo_m(3, 3); + } + } + + if (ImGui::Button("Accept Gizmo (only translation)")) + { + if (index_end < worker_data.size()) + { + if (worker_data[index_end].intermediate_trajectory.size() > 0) + { + Eigen::Affine3d current_gizmo = Eigen::Affine3d::Identity(); + current_gizmo(0, 0) = m_gizmo[0]; + current_gizmo(1, 0) = m_gizmo[1]; + current_gizmo(2, 0) = m_gizmo[2]; + current_gizmo(3, 0) = m_gizmo[3]; + current_gizmo(0, 1) = m_gizmo[4]; + current_gizmo(1, 1) = m_gizmo[5]; + current_gizmo(2, 1) = m_gizmo[6]; + current_gizmo(3, 1) = m_gizmo[7]; + current_gizmo(0, 2) = m_gizmo[8]; + current_gizmo(1, 2) = m_gizmo[9]; + current_gizmo(2, 2) = m_gizmo[10]; + current_gizmo(3, 2) = m_gizmo[11]; + current_gizmo(0, 3) = m_gizmo[12]; + current_gizmo(1, 3) = m_gizmo[13]; + current_gizmo(2, 3) = m_gizmo[14]; + current_gizmo(3, 3) = m_gizmo[15]; + + auto first_pose = worker_data[index_begin].intermediate_trajectory[0]; + + Eigen::Vector3d translation = current_gizmo.translation() - first_pose.translation(); + + float number_all_nodes_inside_interval = 0; + for (int i = index_begin; i < index_end; i++) + { + number_all_nodes_inside_interval += (float)worker_data[i].intermediate_trajectory.size(); + } + + std::vector> all_poses; + for (size_t i = 0; i < worker_data.size(); i++) + { + std::vector poses; + for (size_t j = 0; j < worker_data[i].intermediate_trajectory.size(); j++) + { + poses.push_back(worker_data[i].intermediate_trajectory[j]); + } + all_poses.push_back(poses); + } + + float counter = 0; + + Eigen::Affine3d last_m = Eigen::Affine3d::Identity(); + + for (int i = index_begin; i < index_end; i++) + { + for (size_t j = 0; j < worker_data[i].intermediate_trajectory.size(); j++) + { + TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(worker_data[i].intermediate_trajectory[j]); + + pose.px = + first_pose.translation().x() + translation.x() * (counter / number_all_nodes_inside_interval); + pose.py = + first_pose.translation().y() + translation.y() * (counter / number_all_nodes_inside_interval); + pose.pz = + first_pose.translation().z() + translation.z() * (counter / number_all_nodes_inside_interval); + + counter += 1.0f; + + worker_data[i].intermediate_trajectory[j] = affine_matrix_from_pose_tait_bryan(pose); + + last_m = worker_data[i].intermediate_trajectory[j]; + } + } + + for (size_t i = index_end; i < worker_data.size(); i++) + { + for (size_t j = 0; j < worker_data[i].intermediate_trajectory.size(); j++) + { + Eigen::Affine3d m_update = Eigen::Affine3d::Identity(); + + if (j == 0) + { + m_update = all_poses[i - 1][all_poses[i - 1].size() - 1].inverse() * all_poses[i][0]; + } + else + { + m_update = all_poses[i][j - 1].inverse() * all_poses[i][j]; + } + last_m = last_m * m_update; + worker_data[i].intermediate_trajectory[j] = last_m; + } + } + } + } + } + ImGui::Separator(); + } + + ImGui::NewLine(); + ImGui::Text("Show/hide individual scans:"); + + int n_items = (int)worker_data.size(); + int digits = (n_items > 0) ? (int)std::log10(n_items - 1) + 1 : 1; + + for (int i = 0; i < n_items; i++) + { + // std::string text = "[" + std::to_string(i) + "]"; + // ImGui::Checkbox(text.c_str(), &worker_data[i].show); + std::stringstream ss; + ss << "[" << std::setw(digits) << std::setfill('0') << i << "]"; + + ImGui::Checkbox(ss.str().c_str(), &worker_data[i].show); + + // Arrange checkboxes in rows of 5 + if ((i + 1) % 5 != 0) + ImGui::SameLine(); + } + } + } + } + + ImGui::End(); +} + +void progress_window() +{ + ImGui::Begin("Progress", nullptr, ImGuiWindowFlags_AlwaysAutoResize); + + ImGui::Text(("Working directory:\n'" + working_directory + "'").c_str()); + + // Calculate elapsed time and ETA + auto currentTime = std::chrono::system_clock::now(); + std::chrono::duration elapsed = currentTime - loStartTime; + double elapsedSeconds = elapsed.count(); + loElapsedSeconds.store(elapsedSeconds); + + float progress = loProgress.load(); + double estimatedTimeRemaining = 0.0; + + if (progress > 0.01f && progress < 100.0f) + { // Only estimate when we have meaningful progress + double totalEstimatedTime = elapsedSeconds / progress; + estimatedTimeRemaining = totalEstimatedTime - elapsedSeconds; + loEstimatedTimeRemaining.store(estimatedTimeRemaining); + } + + // Format progress text with time information + char progressText[256]; + char timeInfo[512]; + + if (progress > 0.01f) + { + std::string completionTime = formatCompletionTime(estimatedTimeRemaining); + snprintf(progressText, sizeof(progressText), "Processing: %.1f%%", progress * 100.0f); + snprintf( + timeInfo, + sizeof(timeInfo), + "Elapsed: %s | Remaining: %s | Estimated finish: ~%s", + formatTime(elapsedSeconds).c_str(), + formatTime(estimatedTimeRemaining).c_str(), + completionTime.c_str()); + } + else + { + snprintf(progressText, sizeof(progressText), "Processing: %.1f%%", progress * 100.0f); + snprintf(timeInfo, sizeof(timeInfo), "Elapsed: %s | Calculating completion time...", formatTime(elapsedSeconds).c_str()); + } + + ImGui::ProgressBar(progress, ImVec2(-1.0f, 0.0f), progressText); + ImGui::Text("%s", timeInfo); + +#ifdef _WIN32 + // Update Windows taskbar progress + if (progress > 0.01f && progress < 1.0f) + { + SetTaskbarProgress(progress); + } + else if (progress >= 1.0f) + { + ClearTaskbarProgress(); + } +#endif + + ImGui::NewLine(); + + if (!loPause) + { + if (ImGui::Button("Pause")) + loPause.store(true); + + if (ImGui::IsItemHovered()) + { + ImGui::BeginTooltip(); + ImGui::Text("Pausing calculation will not save intermediate state!"); + ImGui::Text("You can't resume if you close the program!"); + ImGui::EndTooltip(); + } + } + else + { + if (ImGui::Button("Resume")) + loPause.store(false); + } + ImGui::SameLine(); + ImGui::Text("Also check console for progress.."); + + ImGui::Checkbox("Show reference buckets indoor", &show_reference_buckets_indoor); + ImGui::Checkbox("Show reference buckets outdoor", &show_reference_buckets_outdoor); + ImGui::Checkbox("Show initial points", &show_initial_points); + ImGui::Checkbox("Show trajectory", &show_trajectory); + ImGui::Checkbox("Show trajectory as axes", &show_trajectory_as_axes); + ImGui::Checkbox("Show prediction vectors", &show_prediction_vectors); + ImGui::Checkbox("Show intermediate trajectory prediction axes", &intermediate_trajectory_prediction_axes); + ImGui::Checkbox("Show without filtered buckets", &show_without_filtered_buckets); + ImGui::Checkbox("Show normal vectors indoor", &show_normal_vectors_indoor); + ImGui::Checkbox("Show normal vectors outdoor", &show_normal_vectors_outdoor); + // ImGui::Checkbox("Show covs indoor", &show_covs_indoor); + // ImGui::Checkbox("Show covs outdoor", &show_covs_outdoor); + + ImGui::End(); +} + +void openData() +{ + is_settings_gui = false; + info_gui = false; + + loRunning.store(true); + loProgress.store(0.0f); + loElapsedSeconds.store(0.0); + loEstimatedTimeRemaining.store(0.0); + loStartTime = std::chrono::system_clock::now(); + + step1(loPause); + + if (step_1_done) + { + std::thread loThread( + []() + { + std::chrono::time_point start, end; + start = std::chrono::system_clock::now(); + + step2(loPause); + if (!step_2_done) + { + loRunning = false; + std::cerr << "Lidar odometry step 2 failed: no results saved" << std::endl; + return; + } + + end = std::chrono::system_clock::now(); + std::chrono::duration elapsed_seconds = end - start; + + save_results(false, elapsed_seconds.count()); + + loRunning = false; + + std::ostringstream oss; + oss << "Data saved to folder:\n'" << outwd.string() << "'\n" + << "Calculated trajectory length: " << std::fixed << std::setprecision(1) + << params.total_length_of_calculated_trajectory << "[m]\n" + << "Elapsed time: " << formatTime(elapsed_seconds.count()).c_str(); + + [[maybe_unused]] pfd::message message("Information", oss.str(), pfd::choice::ok, pfd::icon::info); + message.result(); + }); + + loThread.detach(); + } + else // no data loaded + loRunning = false; +} + +bool step1( + const std::string& folder, + LidarOdometryParams& params, + std::vector>& pointsPerFile, + Imu& imu_data, + std::string& working_directory, + Trajectory& trajectory, + std::vector& worker_data, + const std::atomic& loPause) +{ + std::vector input_file_names; + + for (const auto& entry : fs::directory_iterator(folder)) + { + if (!entry.is_directory()) + { + std::cout << entry.path() << std::endl; + input_file_names.push_back(entry.path().string()); + } + } + + if (load_data(input_file_names, params, pointsPerFile, imu_data, full_debug_messages)) + { + working_directory = fs::path(input_file_names[0]).parent_path().string(); + calculate_trajectory(trajectory, imu_data, params, full_debug_messages); + if (!compute_step_1(pointsPerFile, params, trajectory, worker_data, loPause)) + return false; + release_raw_clouds(pointsPerFile, params); // nothing after step 1 reads them + std::cout << "step_1_done" << std::endl; + return true; + } + return false; +} + +bool step2(std::vector& worker_data, LidarOdometryParams& params, const std::atomic& loPause) +{ + double ts_failure = 0.0; + std::atomic loProgress; + return compute_step_2(worker_data, params, ts_failure, loProgress, loPause, full_debug_messages); +} + +void save_results( + bool info, + double elapsed_seconds, + std::string& working_directory, + std::vector& worker_data, + LidarOdometryParams& params, + fs::path outwd) +{ + save_result(worker_data, params, outwd, elapsed_seconds); +} + +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(); + + reshape((GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); + + viewLocal = Eigen::Affine3f::Identity(); + + 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); + + glMatrixMode(GL_MODELVIEW); + glLoadMatrixf(viewLocal.matrix().data()); + + showAxes(); + + if (show_initial_points) + { + glColor3d(0.0, 1.0, 0.0); + glPointSize(point_size); + glBegin(GL_POINTS); + for (const auto& p : params.initial_points) + { + auto pp = params.m_g * p.point; + glVertex3d(pp.x(), pp.y(), pp.z()); + } + glEnd(); + } + +#if 0 // ToDo + for (size_t i = 0; i < worker_data.size(); i++) + { + if (worker_data[i].show) + { + glPointSize(1); + glColor3d(0.0, 0.0, 1.0); + glBegin(GL_POINTS); + for (const auto &p : worker_data[i].intermediate_points) + { + Eigen::Vector3d pt = worker_data[i].intermediate_trajectory[p.index_pose] * p.point; + glVertex3d(pt.x(), pt.y(), pt.z()); + } + glEnd(); + glPointSize(1); + + glLineWidth(1); + glBegin(GL_LINES); + const auto &it = worker_data[i].intermediate_trajectory[0]; + glColor3f(1, 0, 0); + glVertex3f(it(0, 3), it(1, 3), it(2, 3)); + glVertex3f(it(0, 3) + it(0, 0), it(1, 3) + it(1, 0), it(2, 3) + it(2, 0)); + + glColor3f(0, 1, 0); + glVertex3f(it(0, 3), it(1, 3), it(2, 3)); + glVertex3f(it(0, 3) + it(0, 1), it(1, 3) + it(1, 1), it(2, 3) + it(2, 1)); + + glEnd(); + } + } +#endif + + if (show_reference_points) + { + glColor3f(1, 0, 0); + glBegin(GL_POINTS); + for (size_t i = 0; i < params.reference_points.size(); i += dec_reference_points) + glVertex3f(params.reference_points[i].point.x(), params.reference_points[i].point.y(), params.reference_points[i].point.z()); + glEnd(); + } + + if (show_trajectory_as_axes) + { + // glBegin(GL_LINE_STRIP); + glBegin(GL_LINES); + for (const auto& wd : worker_data) + { + for (const auto& it : wd.intermediate_trajectory) + { + glColor3f(1, 0, 0); + glVertex3f(it(0, 3), it(1, 3), it(2, 3)); + glVertex3f(it(0, 3) + it(0, 0) * 0.1, it(1, 3) + it(1, 0) * 0.1, it(2, 3) + it(2, 0) * 0.1); + + glColor3f(0, 1, 0); + glVertex3f(it(0, 3), it(1, 3), it(2, 3)); + glVertex3f(it(0, 3) + it(0, 1) * 0.1, it(1, 3) + it(1, 1) * 0.1, it(2, 3) + it(2, 1) * 0.1); + + glColor3f(0, 0, 1); + glVertex3f(it(0, 3), it(1, 3), it(2, 3)); + glVertex3f(it(0, 3) + it(0, 2) * 0.1, it(1, 3) + it(1, 2) * 0.1, it(2, 3) + it(2, 2) * 0.1); + } + } + glEnd(); + } + + if (show_prediction_vectors) + { + glDisable(GL_DEPTH_TEST); + glLineWidth(3.0f); + glBegin(GL_LINES); + for (const auto& wd : worker_data) + { + if (wd.intermediate_trajectory.empty() || wd.imu_prediction_vector.norm() < 1e-6) + continue; + + Eigen::Vector3d start = wd.intermediate_trajectory.front().translation(); + Eigen::Vector3d end = start + wd.imu_prediction_vector; + + glColor3f(1.0f, 0.0f, 1.0f); // magenta + glVertex3d(start.x(), start.y(), start.z()); + glVertex3d(end.x(), end.y(), end.z()); + } + glEnd(); + glLineWidth(1.0f); + glEnable(GL_DEPTH_TEST); + } + + if (intermediate_trajectory_prediction_axes) + { + glLineWidth(1.0f); + glBegin(GL_LINES); + for (const auto& wd : worker_data) + { + if (wd.intermediate_trajectory.empty() || wd.intermediate_trajectory_prediction.empty()) + continue; + + for (size_t i = 0; i < std::min(wd.intermediate_trajectory.size(), wd.intermediate_trajectory_prediction.size()); i++) + { + const auto& it = wd.intermediate_trajectory[i]; + const auto& pred_axis = wd.intermediate_trajectory_prediction[i]; + + glColor3f(1, 0, 0); + glVertex3f(it(0, 3), it(1, 3), it(2, 3)); + glVertex3f(it(0, 3) + pred_axis(0, 0) * 0.05, it(1, 3) + pred_axis(1, 0) * 0.05, it(2, 3) + pred_axis(2, 0) * 0.05); + + glColor3f(0, 1, 0); + glVertex3f(it(0, 3), it(1, 3), it(2, 3)); + glVertex3f(it(0, 3) + pred_axis(0, 1) * 0.05, it(1, 3) + pred_axis(1, 1) * 0.05, it(2, 3) + pred_axis(2, 1) * 0.05); + + glColor3f(0, 0, 1); + glVertex3f(it(0, 3), it(1, 3), it(2, 3)); + glVertex3f(it(0, 3) + pred_axis(0, 2) * 0.05, it(1, 3) + pred_axis(1, 2) * 0.05, it(2, 3) + pred_axis(2, 2) * 0.05); + } + } + glEnd(); + glLineWidth(1.0f); + } + + if (show_trajectory) + { + glPointSize(3); + glColor3f(0, 1, 1); + glBegin(GL_POINTS); + for (const auto& wd : worker_data) + { + for (const auto& it : wd.intermediate_trajectory) + glVertex3f(it(0, 3), it(1, 3), it(2, 3)); + } + glEnd(); + glPointSize(1); + + glColor3f(1, 0, 0); + glBegin(GL_LINES); + if (worker_data.size() > 0) + { + const auto& wd = worker_data[worker_data.size() - 1]; + if (wd.intermediate_trajectory.size() > 0) + { + const auto& it = wd.intermediate_trajectory[wd.intermediate_trajectory.size() - 1]; + + glVertex3f(it(0, 3) - 1, it(1, 3), it(2, 3)); + glVertex3f(it(0, 3) + 1, it(1, 3), it(2, 3)); + + glVertex3f(it(0, 3), it(1, 3) - 1, it(2, 3)); + glVertex3f(it(0, 3), it(1, 3) + 1, it(2, 3)); + + glVertex3f(it(0, 3), it(1, 3), it(2, 3) - 1); + glVertex3f(it(0, 3), it(1, 3), it(2, 3) + 1); + } + } + glEnd(); + } + + if (show_reference_buckets_indoor) + { + std::scoped_lock lock(params.mutex_buckets_indoor); + glColor3f(1, 0, 0); + glBegin(GL_POINTS); + for (const auto& b : params.buckets_indoor) + { + if (b.second.number_of_points == -1) + { + glColor3f(0, 1, 1); + } + else + { + glColor3f(1, 0, 0); + } + if (!show_without_filtered_buckets) + { + glVertex3f(b.second.mean.x(), b.second.mean.y(), b.second.mean.z()); + } + else + { + if (b.second.number_of_hits < 20) + { + glVertex3f(b.second.mean.x(), b.second.mean.y(), b.second.mean.z()); + } + } + } + glEnd(); + } + + if (show_reference_buckets_outdoor) + { + std::scoped_lock lock2(params.mutex_buckets_outdoor); + glColor3f(0, 0, 1); + glBegin(GL_POINTS); + for (const auto& b : params.buckets_outdoor) + { + if (b.second.number_of_points == -1) + { + glColor3f(1, 0, 1); + } + else + { + glColor3f(0, 0, 1); + } + // glVertex3f(b.second.mean.x(), b.second.mean.y(), b.second.mean.z()); + if (!show_without_filtered_buckets) + { + glVertex3f(b.second.mean.x(), b.second.mean.y(), b.second.mean.z()); + } + else + { + if (b.second.number_of_hits < 20) + { + glVertex3f(b.second.mean.x(), b.second.mean.y(), b.second.mean.z()); + } + } + } + glEnd(); + } + + if (show_normal_vectors_indoor) + { + std::scoped_lock lock2(params.mutex_buckets_indoor); + glColor3f(0, 0, 1); + glBegin(GL_LINES); + for (const auto& b : params.buckets_indoor) + { + glColor3f(fabs(b.second.normal_vector.x()), fabs(b.second.normal_vector.y()), fabs(b.second.normal_vector.z())); + + glVertex3f(b.second.mean.x(), b.second.mean.y(), b.second.mean.z()); + glVertex3f( + b.second.mean.x() + b.second.normal_vector.x(), + b.second.mean.y() + b.second.normal_vector.y(), + b.second.mean.z() + b.second.normal_vector.z()); + } + glEnd(); + } + + if (show_normal_vectors_outdoor) + { + std::scoped_lock lock2(params.mutex_buckets_outdoor); + glColor3f(0, 0, 1); + glBegin(GL_LINES); + for (const auto& b : params.buckets_outdoor) + { + glColor3f(fabs(b.second.normal_vector.x()), fabs(b.second.normal_vector.y()), fabs(b.second.normal_vector.z())); + + glVertex3f(b.second.mean.x(), b.second.mean.y(), b.second.mean.z()); + glVertex3f( + b.second.mean.x() + b.second.normal_vector.x(), + b.second.mean.y() + b.second.normal_vector.y(), + b.second.mean.z() + b.second.normal_vector.z()); + } + glEnd(); + } + + if (show_covs_indoor) + { + std::scoped_lock lock(params.mutex_buckets_indoor); + for (const auto& b : params.buckets_indoor) + draw_ellipse(b.second.cov, b.second.mean, Eigen::Vector3f(1.0f, 0.0f, 0.0f), 3); + } + + if (show_covs_outdoor) + { + std::scoped_lock lock2(params.mutex_buckets_outdoor); + for (const auto& b : params.buckets_outdoor) + draw_ellipse(b.second.cov, b.second.mean, Eigen::Vector3f(0.0f, 0.0f, 1.0f), 3); + } + + // + if (worker_data.size() > 0) + { + if (index_begin < worker_data.size()) + { + glColor3f(1, 1, 0); + glBegin(GL_LINES); + if (worker_data[index_begin].intermediate_trajectory.size() > 0) + { + auto p = worker_data[index_begin].intermediate_trajectory[0].translation(); + glVertex3f(p.x() - 1, p.y(), p.z()); + glVertex3f(p.x() + 1, p.y(), p.z()); + + glVertex3f(p.x(), p.y() - 1, p.z()); + glVertex3f(p.x(), p.y() + 1, p.z()); + + glVertex3f(p.x(), p.y(), p.z() - 1); + glVertex3f(p.x(), p.y(), p.z() + 1); + } + glEnd(); + } + + if (index_end < worker_data.size()) + { + glColor3f(0, 1, 1); + glBegin(GL_LINES); + if (worker_data[index_end].intermediate_trajectory.size() > 0) + { + auto p = worker_data[index_end].intermediate_trajectory[0].translation(); + glVertex3f(p.x() - 1, p.y(), p.z()); + glVertex3f(p.x() + 1, p.y(), p.z()); + + glVertex3f(p.x(), p.y() - 1, p.z()); + glVertex3f(p.x(), p.y() + 1, p.z()); + + glVertex3f(p.x(), p.y(), p.z() - 1); + glVertex3f(p.x(), p.y(), p.z() + 1); + } + glEnd(); + } + } + + ImGui_ImplOpenGL2_NewFrame(); + ImGui_ImplGLUT_NewFrame(); + ImGui::NewFrame(); + + ShowMainDockSpace(); + + view_kbd_shortcuts(); + + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_O, false)) + { + openData(); + + // workaround + io.AddKeyEvent(ImGuiKey_O, false); + io.AddKeyEvent(ImGuiMod_Ctrl, false); + } + + if (!loRunning) + { + if (ImGui::BeginMainMenuBar()) + { + if (ImGui::BeginMenu("Presets")) + { + if (ImGui::MenuItem("1 Velocity < 8km/h, tiny spaces (default)", nullptr, (lastPar == 1))) + { + lastPar = 1; + + set_lidar_odometry_default_params(params); + + std::cout << "clicked: Set parameters for velocity up to 8km/h, tiny spaces" << std::endl; + } + if (ImGui::MenuItem("2 Velocity < 8km/h, larger spaces, precise forestry (generic)", nullptr, (lastPar == 2))) + { + lastPar = 2; + + params.decimation = 0.01; + params.in_out_params_indoor.resolution_X = 0.1; + params.in_out_params_indoor.resolution_Y = 0.1; + params.in_out_params_indoor.resolution_Z = 0.1; + + params.in_out_params_outdoor.resolution_X = 0.3; + params.in_out_params_outdoor.resolution_Y = 0.3; + params.in_out_params_outdoor.resolution_Z = 0.3; + + params.filter_threshold_xy_inner = 1.0; + params.filter_threshold_xy_outer = 70.0; + params.threshould_output_filter = 1.5; + + params.use_robust_and_accurate_lidar_odometry = false; + params.distance_bucket = 0.2; + params.polar_angle_deg = 10.0; + params.azimutal_angle_deg = 10.0; + params.robust_and_accurate_lidar_odometry_iterations = 20; + + params.max_distance_lidar = 70.0; + params.nr_iter = 500; + params.sliding_window_trajectory_length_threshold = 10000; + params.real_time_threshold_seconds = 10; + + std::cout << "clicked: Set parameters for velocity < 8km/h, larger spaces, precise forestry (generic)" << std::endl; + } + if (ImGui::MenuItem("3 Velocity < 30 km/h, largest open spaces, fast motion", nullptr, (lastPar == 3))) + { + lastPar = 3; + + params.decimation = 0.03; + params.in_out_params_indoor.resolution_X = 0.3; + params.in_out_params_indoor.resolution_Y = 0.3; + params.in_out_params_indoor.resolution_Z = 0.3; + + params.in_out_params_outdoor.resolution_X = 0.5; + params.in_out_params_outdoor.resolution_Y = 0.5; + params.in_out_params_outdoor.resolution_Z = 0.5; + + params.filter_threshold_xy_inner = 1.0; + params.filter_threshold_xy_outer = 70.0; + params.threshould_output_filter = 3.0; + + params.use_robust_and_accurate_lidar_odometry = false; + params.distance_bucket = 0.2; + params.polar_angle_deg = 10.0; + params.azimutal_angle_deg = 10.0; + params.robust_and_accurate_lidar_odometry_iterations = 20; + + params.max_distance_lidar = 70.0; + params.nr_iter = 500; + params.sliding_window_trajectory_length_threshold = 200; + params.real_time_threshold_seconds = 10; + + std::cout << "clicked: Set parameters for velocity < 30 km/h, largest open spaces, fast motion" << std::endl; + } + ImGui::Separator(); + if (ImGui::MenuItem( + "4: Best accuracy, precision, and robustness (uncheck to calculate faster)", + nullptr, + params.ablation_study_use_anisotropic_weighting)) + { + params.ablation_study_use_anisotropic_weighting = !params.ablation_study_use_anisotropic_weighting; + } + ImGui::MenuItem("5: No IMU, lidar-only odometry (combine with 1-3)", nullptr, ¶ms.lidar_only_odometry); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "For recordings without IMU data, written with a synthetic stationary IMU (e.g. ouster_pcap_to_laz " + "--stationary-imu): each window's rotation and translation are predicted from the previous window instead of " + "from the IMU."); + + ImGui::EndMenu(); + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Set parameters for MANDEYE data processing"); + + if (ImGui::BeginMenu("Parameters")) + { + ImGui::MenuItem( + "Remove IMU bias from first stationary scan", nullptr, ¶ms.use_removie_imu_bias_from_first_stationary_scan); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("IMU bias will be removed given there's a stationary period at the start of the recording"); + + ImGui::MenuItem("Multithread", nullptr, ¶ms.useMultithread); + ImGui::SetNextItemWidth(ImGuiNumberWidth / 2); + ImGui::InputDouble("Time threshold [s]", ¶ms.real_time_threshold_seconds, 0.0, 0.0, "%.3f"); + if (ImGui::IsItemHovered()) + { + ImGui::BeginTooltip(); + ImGui::Text("Optimization timeout"); + ImGui::Text("smaller value gives faster calculations, less precise results"); + ImGui::Text("e.g.: 0.1 [s] will make processing time aprox equal to scan time"); + ImGui::Text("0.3 [s] will make processing time aprox equal to 3x scan time"); + ImGui::EndTooltip(); + } + + if (ImGui::MenuItem("Set real time performance")) + params.real_time_threshold_seconds = 0.1; + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("For IMU 200Hz, 20 nodes in optimization window"); + + ImGui::Separator(); + + ImGui::Text("Debug:"); + + ImGui::MenuItem("Full processing messages", nullptr, &full_debug_messages); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Show more messages during processing in console window"); + ImGui::MenuItem("Save calibration validation file", nullptr, ¶ms.save_calibration_validation); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Save calibrated points from first laz file in 'calibrationValidation.asc' file"); + ImGui::SetNextItemWidth(ImGuiNumberWidth); + ImGui::InputInt("Number of calibration validation points", ¶ms.calibration_validation_points); + + ImGui::Separator(); + if (ImGui::BeginMenu("Ablation study")) + { + ImGui::MenuItem("Use planarity", nullptr, ¶ms.ablation_study_use_planarity); + ImGui::MenuItem("Use norm", nullptr, ¶ms.ablation_study_use_norm); + ImGui::MenuItem("Use hierarchical RGD (outer RGD turned on)", nullptr, ¶ms.ablation_study_use_hierarchical_rgd); + ImGui::MenuItem("Use view point and normal vectors", nullptr, ¶ms.ablation_study_use_view_point_and_normal_vectors); + ImGui::MenuItem("Use threshold '1e-6' outer RGD", nullptr, ¶ms.ablation_study_use_threshold_outer_rgd); + ImGui::MenuItem("Use anisotropic weighting", nullptr, ¶ms.ablation_study_use_anisotropic_weighting); + ImGui::EndMenu(); + } + + ImGui::Separator(); + + ImGui::MenuItem("Saving results with index pose", nullptr, ¶ms.save_index_pose); + + ImGui::Separator(); + + if (ImGui::MenuItem("Load parameters")) + { + auto input_file_names = mandeye::fd::OpenFileDialog("Load parameters file", mandeye::fd::Toml_filter, ".toml"); + + if (input_file_names.size() > 0) + { + try + { + // Use the original TomlIO class for loading parameters in GUI + TomlIO toml_io; + toml_io.LoadParametersFromTomlFile(input_file_names[0], params); + std::cout << "Parameters loaded from: " << input_file_names[0] << std::endl; + } catch (const std::exception& e) + { + std::cerr << "Error loading TOML file: " << e.what() << std::endl; + } + } + } + + if (ImGui::MenuItem("Save parameters")) + { + auto output_file_name = mandeye::fd::SaveFileDialog("Save parameters file", mandeye::fd::Toml_filter, ".toml"); + std::cout << "Parameters file to save: '" << output_file_name << "'" << std::endl; + + if (!output_file_name.empty()) + { + // Use the original TomlIO class for saving parameters in GUI + TomlIO toml_io; + bool success = toml_io.SaveParametersToTomlFile(output_file_name, params); + if (success) + std::cout << "Parameters file generated: " << output_file_name << std::endl; + else + std::cerr << "Failed to save parameters." << std::endl; + } + } + + ImGui::EndMenu(); + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Processing parameter list"); + + ImGui::SameLine(); + ImGui::Dummy(ImVec2(20, 0)); + ImGui::SameLine(); + + if (ImGui::Button("Load & process scanning")) + openData(); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Select folder from where to load data and start processing (Ctrl+O)"); + + ImGui::SameLine(); + ImGui::Dummy(ImVec2(20, 0)); + ImGui::SameLine(); + + if (ImGui::BeginMenu("View")) + { + ImGui::BeginDisabled(!(session.point_clouds_container.point_clouds.size() > 0)); + { + 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::EndDisabled(); + + ImGui::MenuItem("Show initial points", nullptr, &show_initial_points); + ImGui::MenuItem("Show trajectory", nullptr, &show_trajectory); + ImGui::MenuItem("Show trajectory as axes", nullptr, &show_trajectory_as_axes); + ImGui::MenuItem("Show prediction vectors", nullptr, &show_prediction_vectors); + ImGui::MenuItem("Show intermediate trajectory prediction axes", nullptr, &intermediate_trajectory_prediction_axes); + + ImGui::MenuItem("Show compass/ruler", "key C", &compass_ruler); + + ImGui::MenuItem("Lock Z", "Shift + Z", &lock_z, !is_ortho); + + // ImGui::MenuItem("show_covs", nullptr, &show_covs); + + ImGui::Separator(); + + ImGui::Text("Colors:"); + + ImGui::ColorEdit3("Background", (float*)¶ms.clear_color, ImGuiColorEditFlags_NoInputs); + + bg_color = params.clear_color; + + ImGui::Separator(); + + ImGui::MenuItem("Settings", nullptr, &is_settings_gui); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Show power user settings window with more parameters"); + + ImGui::EndMenu(); + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Scene view relevant parameters"); + + camMenu(); + + 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 (initial_transformation_gizmo) + { + ImGuiIO& io = ImGui::GetIO(); + // ImGuizmo ----------------------------------------------- + ImGuizmo::BeginFrame(); + ImGuizmo::Enable(true); + ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); + + 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); + + params.m_g(0, 0) = m_gizmo[0]; + params.m_g(1, 0) = m_gizmo[1]; + params.m_g(2, 0) = m_gizmo[2]; + params.m_g(3, 0) = m_gizmo[3]; + params.m_g(0, 1) = m_gizmo[4]; + params.m_g(1, 1) = m_gizmo[5]; + params.m_g(2, 1) = m_gizmo[6]; + params.m_g(3, 1) = m_gizmo[7]; + params.m_g(0, 2) = m_gizmo[8]; + params.m_g(1, 2) = m_gizmo[9]; + params.m_g(2, 2) = m_gizmo[10]; + params.m_g(3, 2) = m_gizmo[11]; + params.m_g(0, 3) = m_gizmo[12]; + params.m_g(1, 3) = m_gizmo[13]; + params.m_g(2, 3) = m_gizmo[14]; + params.m_g(3, 3) = m_gizmo[15]; + } + + if (gizmo_stretch_interval) + { + ImGuiIO& io = ImGui::GetIO(); + // ImGuizmo ----------------------------------------------- + ImGuizmo::BeginFrame(); + ImGuizmo::Enable(true); + ImGuizmo::SetRect(0, 0, io.DisplaySize.x, io.DisplaySize.y); + + 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); + + stretch_gizmo_m(0, 0) = m_gizmo[0]; + stretch_gizmo_m(1, 0) = m_gizmo[1]; + stretch_gizmo_m(2, 0) = m_gizmo[2]; + stretch_gizmo_m(3, 0) = m_gizmo[3]; + stretch_gizmo_m(0, 1) = m_gizmo[4]; + stretch_gizmo_m(1, 1) = m_gizmo[5]; + stretch_gizmo_m(2, 1) = m_gizmo[6]; + stretch_gizmo_m(3, 1) = m_gizmo[7]; + stretch_gizmo_m(0, 2) = m_gizmo[8]; + stretch_gizmo_m(1, 2) = m_gizmo[9]; + stretch_gizmo_m(2, 2) = m_gizmo[10]; + stretch_gizmo_m(3, 2) = m_gizmo[11]; + stretch_gizmo_m(0, 3) = m_gizmo[12]; + stretch_gizmo_m(1, 3) = m_gizmo[13]; + stretch_gizmo_m(2, 3) = m_gizmo[14]; + stretch_gizmo_m(3, 3) = m_gizmo[15]; + } + + if (is_settings_gui) + { + lastPar = 0; + + settings_gui(); + } + + if (loRunning) + progress_window(); + + cor_window(); + + if (info_gui) + { + infoLines[infoLines.size() - 2] = "It saves session file in " + working_directory + "\\lio_result_*"; + info_window(infoLines, appShortcuts); + } + + if (compass_ruler) + drawMiniCompassWithRuler(); + + ImGui::Render(); + ImGui_ImplOpenGL2_RenderDrawData(ImGui::GetDrawData()); + + glutSwapBuffers(); + glutPostRedisplay(); +} + +void on_exit() +{ + remove_cache(params); +} + +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_MIDDLE_BUTTON || glut_button == GLUT_RIGHT_BUTTON) && state == GLUT_DOWN && io.KeyCtrl) + setNewRotationCenter(x, y); + + if (state == GLUT_DOWN) + mouse_buttons |= 1 << glut_button; + else if (state == GLUT_UP) + mouse_buttons = 0; + + mouse_old_x = x; + mouse_old_y = y; + } +} + +int main(int argc, char* argv[]) +{ + spdlog::cfg::load_env_levels(); + spdlog::flush_on(spdlog::level::warn); + set_lidar_odometry_default_params(params); + + try + { + if (checkClHelp(argc, argv)) + { + std::cout << winTitle << "\n\n" + << "USAGE:\n" + << std::filesystem::path(argv[0]).stem().string() << " /?\n" + << std::filesystem::path(argv[0]).stem().string() << " \n" + << std::filesystem::path(argv[0]).stem().string() << " --dump-default-params \n\n" + << "where\n" + << " Path where scan files are located (*.csv, *.laz, *.sn)\n" + << " Path to TOML parameter file (*.toml)\n" + << " Path where processed session should be stored\n" + << " (with alone, default parameters are used and the\n" + << " session is stored in the next free /lio_result_N,\n" + << " as the GUI does)\n" + << " --dump-default-params \n" + << " Write the default parameters to and exit\n" + << " -h, /h, --help, /? Show this help and exit\n\n"; + + return 0; + } + + for (int i = 1; i < argc; ++i) + { + if (std::string(argv[i]) == "--dump-default-params") + { + if (i + 1 >= argc) + { + std::cerr << "--dump-default-params requires an output file (*.toml)" << std::endl; + return 1; + } + const fs::path out_file(argv[i + 1]); + // Value-initialised, as the static `params` is: the NDT grid members carry no + // initialisers, and a default-initialised local would dump stack garbage. + LidarOdometryParams default_params{}; + set_lidar_odometry_default_params(default_params); + TomlIO toml_io; + toml_io.SaveParametersToTomlFile(out_file.string(), default_params); + std::error_code ec; + if (!fs::exists(out_file, ec) || fs::file_size(out_file, ec) == 0 || ec) + { + std::cerr << "Could not write default parameters to: " << out_file.string() << std::endl; + return 1; + } + std::cout << "Default parameters saved to: " << out_file.string() << std::endl; + return 0; + } + } + + if (argc == 2) // running from command line + { + auto path = fs::path(argv[1]); + if (is_directory(path)) + { + std::string working_directory; + std::vector worker_data; + + std::chrono::time_point start, end; + start = std::chrono::system_clock::now(); + + std::atomic loPause{ false }; + if (!step1(path.string(), params, pointsPerFile, imu_data, working_directory, trajectory, worker_data, loPause) || + !step2(worker_data, params, loPause)) + { + std::cerr << "Lidar odometry failed: no results saved" << std::endl; + return 1; + } + + end = std::chrono::system_clock::now(); + std::chrono::duration elapsed_seconds = end - start; + std::time_t end_time = std::chrono::system_clock::to_time_t(end); + std::cout << "calculations finished computation at " << std::ctime(&end_time) + << "Elapsed time: " << formatTime(elapsed_seconds.count()).c_str() << "s\n"; + + // Only the input folder was given: store the session where the GUI would, + // in the next free lio_result_N under the working directory. + if (working_directory.empty()) + { + std::cerr << "No Mandeye data could be loaded from: '" << path.string() << "'" << std::endl; + return 1; + } + const fs::path result_dir = get_next_result_path(working_directory); + save_results(false, elapsed_seconds.count(), working_directory, worker_data, params, result_dir); + std::cout << "Results saved to folder: '" << result_dir.string() << "'" << std::endl; + remove_cache(params); + } + } + else if (argc == 4) // runnning from command line with custom params + { + // Load parameters from file using original TomlIO class + TomlIO toml_io; + toml_io.LoadParametersFromTomlFile(argv[2], params); + std::cout << "Parameters loaded OK from: " << argv[2] << std::endl; + + std::string working_directory; + std::vector worker_data; + + std::chrono::time_point start, end; + start = std::chrono::system_clock::now(); + + std::atomic loPause{ false }; + if (!step1(argv[1], params, pointsPerFile, imu_data, working_directory, trajectory, worker_data, loPause) || + !step2(worker_data, params, loPause)) + { + std::cerr << "Lidar odometry failed: no results saved" << std::endl; + return 1; + } + + end = std::chrono::system_clock::now(); + std::chrono::duration elapsed_seconds = end - start; + std::time_t end_time = std::chrono::system_clock::to_time_t(end); + std::cout << "calculations finished computation at " << std::ctime(&end_time) + << "Elapsed time: " << formatTime(elapsed_seconds.count()).c_str() << "s\n"; + + save_results(false, elapsed_seconds.count(), working_directory, worker_data, params, argv[3]); + remove_cache(params); + } + else // full GUI mode + { + std::cout << argv[0] << " input_folder parameters(*.toml) output_folder" << std::endl; + + initGL(&argc, argv, winTitle, display, mouse); + glutCloseFunc(on_exit); + +#ifdef _WIN32 + InitTaskbarProgress(); +#endif + + glutMainLoop(); + + ImGui_ImplOpenGL2_Shutdown(); + ImGui_ImplGLUT_Shutdown(); + ImGui::DestroyContext(); + } + } catch (const std::bad_alloc& e) + { + std::cerr << "System is out of memory : " << e.what() << std::endl; + mandeye::fd::OutOfMemMessage(); + } catch (const std::exception& e) + { + std::cout << e.what(); + } catch (...) + { + std::cerr << "Unknown fatal error occurred." << std::endl; + } + + return 0; +} \ No newline at end of file diff --git a/apps/lidar_odometry_step_1_legacy/resource.h b/apps/lidar_odometry_step_1_legacy/resource.h new file mode 100644 index 00000000..4f3204ca --- /dev/null +++ b/apps/lidar_odometry_step_1_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/lidar_odometry_step_1_legacy/resource.rc b/apps/lidar_odometry_step_1_legacy/resource.rc new file mode 100644 index 00000000..ac2be718 --- /dev/null +++ b/apps/lidar_odometry_step_1_legacy/resource.rc @@ -0,0 +1,54 @@ +// 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" + +// GLFW (used by raylib) only picks up a window/taskbar icon from a +// resource named exactly GLFW_ICON on Windows; without it GLFW falls +// back to the generic IDI_WINLOGO icon at runtime. +GLFW_ICON 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 1\0" +VALUE "FileVersion", "0.100.1\0" +VALUE "InternalName", "Odometry\0" +VALUE "LegalCopyright", "(c) 2026 github.com/MapsHD/HDMapping\0" +VALUE "OriginalFilename", "lidar_odometry_step_1.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/apps/mandeye_raw_data_viewer/CMakeLists.txt b/apps/mandeye_raw_data_viewer/CMakeLists.txt index 7477ecdf..4363923d 100644 --- a/apps/mandeye_raw_data_viewer/CMakeLists.txt +++ b/apps/mandeye_raw_data_viewer/CMakeLists.txt @@ -11,7 +11,6 @@ set(SOURCES ../lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp ../lidar_odometry_step_1/points_global_spill.cpp ../lidar_odometry_step_1/raw_cloud_loader.cpp - "../../core/src/utils.cpp" ) # Windows: add resource file @@ -23,6 +22,8 @@ add_executable( mandeye_raw_data_viewer ${SOURCES} ) +# raylib-based (ported off GLUT; the GLUT build is kept as apps/mandeye_raw_data_viewer_legacy): +# (core_raylib_utils) replaces with the same names. target_include_directories( mandeye_raw_data_viewer PRIVATE include @@ -32,11 +33,6 @@ target_include_directories( ${THIRDPARTY_DIRECTORY}/glm ${EIGEN3_INCLUDE_DIR} ${THIRDPARTY_DIRECTORY}/tomlplusplus/include - ${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 @@ -46,16 +42,16 @@ target_include_directories( target_link_libraries( mandeye_raw_data_viewer - PRIVATE vqf Fusion + PRIVATE + # core_raylib_utils brings in core_raylib (core, raylib), raylib_widgets and rlImGui. + core_raylib_utils + imgui_raylib + implot_raylib + vqf Fusion unordered_dense::unordered_dense spdlog::spdlog UTL::include - ${FREEGLUT_LIBRARY} - ${OPENGL_gl_LIBRARY} - OpenGL::GLU ${PLATFORM_LASZIP_LIB} - ${CORE_LIBRARIES} - ${GUI_LIBRARIES} ${PLATFORM_MISCELLANEOUS_LIBS} ${OpenCV_LIBS}) diff --git a/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp b/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp index a938e25f..6e4725c8 100644 --- a/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp +++ b/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp @@ -1,13 +1,7 @@ -// clang-format off -#include -#include -// clang-format on - #include -#include -#include #include #include +#include #include #include @@ -17,8 +11,8 @@ #include #include #include +#include #include -#include #include @@ -37,8 +31,6 @@ #ifdef _WIN32 #include "resource.h" -#include // <-- Required for ShellExecuteA -#include #endif /////////////////////////////////////////////////////////////////////////////////// @@ -131,13 +123,14 @@ bool show_imu_data = false; bool show_cameras_data = true; bool is_settings_gui = false; bool color_by_intensity = false; +bool intensity_shading = true; float intensity_gamma = 1.0f; namespace photos { cv::Mat imgToShow; std::string imgToShowFn; - GLuint photo_texture_cam0 = 0; + unsigned int photo_texture_cam0 = 0; double nearestTs = 0; int photo_width_cam0 = 0; int photo_height_cam0 = 0; @@ -197,19 +190,12 @@ void render_nearest_photo(double ts) imgToShow = img_rgb; imgToShowFn = photo_file; photos::nearestTs = double(it->first) / 1e9; - // upload to OpenGL texture + // upload to OpenGL texture (rlLoadTexture() sets linear filtering and 1-byte unpack alignment for RGB) photo_width_cam0 = imgToShow.cols; photo_height_cam0 = imgToShow.rows; - if (photo_texture_cam0 == 0) - { - glGenTextures(1, &photo_texture_cam0); - } - glBindTexture(GL_TEXTURE_2D, photo_texture_cam0); - glPixelStorei(GL_UNPACK_ALIGNMENT, 1); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); - glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, photo_width_cam0, photo_height_cam0, 0, GL_RGB, GL_UNSIGNED_BYTE, imgToShow.data); - glBindTexture(GL_TEXTURE_2D, 0); + if (photo_texture_cam0 != 0) + rlUnloadTexture(photo_texture_cam0); + photo_texture_cam0 = rlLoadTexture(imgToShow.data, photo_width_cam0, photo_height_cam0, PIXELFORMAT_UNCOMPRESSED_R8G8B8, 1); } void optimize() @@ -1202,14 +1188,9 @@ void loadFiles(std::vector input_file_names) } } -void openFolder() +//! Appends the files of a Mandeye data folder (and its CAMERA_0 photos) to input_file_names. +void collectFolderFiles(const std::string& input_folder_name, std::vector& input_file_names) { - std::string input_folder_name; - std::vector input_file_names; - input_folder_name = mandeye::fd::SelectFolder("Select Mandeye data folder"); - - spdlog::info("Selected folder: '{}'", input_folder_name); - const std::vector subDirs{ "", "CAMERA_0" }; for (const auto& subdir : subDirs) { @@ -1221,10 +1202,37 @@ void openFolder() input_file_names.push_back(entry.path().string()); } } +} + +void openFolder() +{ + std::string input_folder_name; + std::vector input_file_names; + input_folder_name = mandeye::fd::SelectFolder("Select Mandeye data folder"); + + spdlog::info("Selected folder: '{}'", input_folder_name); + + collectFolderFiles(input_folder_name, input_file_names); loadFiles(input_file_names); } +//! Drag and drop: dropped folders load like "Open folder", dropped files like "Open files". +void openDropped(const std::vector& paths) +{ + std::vector input_file_names; + for (const auto& path : paths) + { + if (fs::is_directory(path)) + collectFolderFiles(path, input_file_names); + else + input_file_names.push_back(path); + } + + if (!input_file_names.empty()) + loadFiles(input_file_names); +} + void openFiles() { std::vector input_file_names = mandeye::fd::OpenFileDialog("Load all files", mandeye::fd::All_Filter, true); @@ -1524,14 +1532,13 @@ void settings_gui() void display() { ImGuiIO& io = ImGui::GetIO(); - glViewport(0, 0, (GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); + rlViewport(0, 0, GetRenderWidth(), GetRenderHeight()); - 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); + 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(); - glMatrixMode(GL_PROJECTION); - glLoadIdentity(); + rlMatrixMode(RL_PROJECTION); + rlLoadIdentity(); updateCameraTransition(); @@ -1539,7 +1546,7 @@ void display() if (!is_ortho) { - reshape((GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); + reshape((int)io.DisplaySize.x, (int)io.DisplaySize.y); viewLocal.translate(rotation_center); @@ -1552,10 +1559,11 @@ void display() viewLocal.translate(-rotation_center); - glLoadMatrixf(viewLocal.matrix().data()); + rlMultMatrixf(viewLocal.matrix().data()); } else updateOrthoView(); + captureFrameMatrices(); // /* glPointSize(point_size); @@ -1579,12 +1587,12 @@ void display() glEnd(); glPointSize(1);*/ - glPointSize(point_size); - glBegin(GL_POINTS); + beginPoints(point_size); if (index_rendered_points_local >= 0 && index_rendered_points_local < all_data.size()) { double max_diff = 0.0; float max_intensity = 0.0f; + const IntensityRange range = intensityRange(all_data[index_rendered_points_local].points_local); for (size_t i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) { if (color_by_intensity && all_data[index_rendered_points_local].points_local[i].intensity > max_intensity) @@ -1642,21 +1650,24 @@ void display() ii = std::clamp(ii, 0.0f, 1.0f); if (intensity_gamma != 1.0f) ii = std::pow(ii, intensity_gamma); - glColor3f(ii, ii, ii); + color3f(ii, ii, ii); } - else if (all_data[index_rendered_points_local].lidar_ids[i] == 0) - glColor3f(pc_color.x, pc_color.y, pc_color.z); else - glColor3f(pc_color2.x, pc_color2.y, pc_color2.z); + { + const ImVec4& c = all_data[index_rendered_points_local].lidar_ids[i] == 0 ? pc_color : pc_color2; + if (intensity_shading) + color3fIntensityShaded(c.x, c.y, c.z, all_data[index_rendered_points_local].points_local[i].intensity, range); + else + color3f(c.x, c.y, c.z); + } - glVertex3f(p.x(), p.y(), p.z()); + pointVertex3f(p.x(), p.y(), p.z()); //} } } } } - glEnd(); - glPointSize(1); + endPoints(); if (index_rendered_points_local >= 0 && index_rendered_points_local < all_data.size()) { @@ -1688,22 +1699,21 @@ void display() } glVertex3f(p.x(), p.y(), p.z());*/ + // Drawn 1 px wide: the core-profile context has no wide lines (was glLineWidth(2)). Eigen::Affine3d m = all_data[index_rendered_points_local].poses[index_pose]; - glLineWidth(2.0); - glBegin(GL_LINES); - glColor3f(1.0f, 0.0f, 0.0f); - glVertex3f(m(0, 3), m(1, 3), m(2, 3)); - glVertex3f(m(0, 3) + m(0, 0), m(1, 3) + m(1, 0), m(2, 3) + m(2, 0)); - - glColor3f(0.0f, 1.0f, 0.0f); - glVertex3f(m(0, 3), m(1, 3), m(2, 3)); - glVertex3f(m(0, 3) + m(0, 1), m(1, 3) + m(1, 1), m(2, 3) + m(2, 1)); - - glColor3f(0.0f, 0.0f, 1.0f); - glVertex3f(m(0, 3), m(1, 3), m(2, 3)); - glVertex3f(m(0, 3) + m(0, 2), m(1, 3) + m(1, 2), m(2, 3) + m(2, 2)); - glEnd(); - glLineWidth(1.0); + rlBegin(RL_LINES); + rlColor3f(1.0f, 0.0f, 0.0f); + rlVertex3f(m(0, 3), m(1, 3), m(2, 3)); + rlVertex3f(m(0, 3) + m(0, 0), m(1, 3) + m(1, 0), m(2, 3) + m(2, 0)); + + rlColor3f(0.0f, 1.0f, 0.0f); + rlVertex3f(m(0, 3), m(1, 3), m(2, 3)); + rlVertex3f(m(0, 3) + m(0, 1), m(1, 3) + m(1, 1), m(2, 3) + m(2, 1)); + + rlColor3f(0.0f, 0.0f, 1.0f); + rlVertex3f(m(0, 3), m(1, 3), m(2, 3)); + rlVertex3f(m(0, 3) + m(0, 2), m(1, 3) + m(1, 2), m(2, 3) + m(2, 2)); + rlEnd(); break; } } @@ -1711,14 +1721,14 @@ void display() if (show_rgd_nn) { - glColor3f(0, 0, 0); - glBegin(GL_LINES); + rlBegin(RL_LINES); + rlColor3f(0, 0, 0); for (const auto& nn : rgd_nn) { - glVertex3f(nn.first.x(), nn.first.y(), nn.first.z()); - glVertex3f(nn.second.x(), nn.second.y(), nn.second.z()); + rlVertex3f(nn.first.x(), nn.first.y(), nn.first.z()); + rlVertex3f(nn.second.x(), nn.second.y(), nn.second.z()); } - glEnd(); + rlEnd(); } if (show_mean_cov) @@ -1729,14 +1739,16 @@ void display() showAxes(); - ImGui_ImplOpenGL2_NewFrame(); - ImGui_ImplGLUT_NewFrame(); - ImGui::NewFrame(); + rlImGuiBegin(); ShowMainDockSpace(); view_kbd_shortcuts(); + const auto dropped = takeDroppedFiles(); + if (!dropped.empty()) + openDropped(dropped); + if (all_data.size() > 0) { if ((!io.KeyCtrl && !io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_RightArrow, true)) || ImGui::IsKeyPressed(ImGuiKey_PageUp, true) || @@ -1765,7 +1777,7 @@ void display() if (ImGui::Button("Open folder")) openFolder(); if (ImGui::IsItemHovered()) - ImGui::SetTooltip("Select folder containing files for analyze (Ctrl+O)"); + ImGui::SetTooltip("Select folder containing files for analyze (Ctrl+O), or drop folders/files on the window"); ImGui::SameLine(); @@ -1844,6 +1856,9 @@ void display() ImGui::MenuItem("Show IMU data", nullptr, &show_imu_data); ImGui::MenuItem("Show cameras data", nullptr, &show_cameras_data); + ImGui::MenuItem("Intensity shading", nullptr, &intensity_shading); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Shade the point cloud colors by LiDAR intensity"); ImGui::MenuItem("Color by intensity", nullptr, &color_by_intensity); if (ImGui::IsItemHovered()) ImGui::SetTooltip("Render points as grayscale from LiDAR intensity (normalized to current block max)"); @@ -1954,6 +1969,8 @@ void display() info_window(infoLines, appShortcuts); + end3DAndDrawLabels(); + if (compass_ruler) drawMiniCompassWithRuler(); @@ -1977,34 +1994,12 @@ void display() if (is_settings_gui) settings_gui(); - ImGui::Render(); - ImGui_ImplOpenGL2_RenderDrawData(ImGui::GetDrawData()); - - glutSwapBuffers(); - glutPostRedisplay(); + rlImGuiEnd(); } 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) { @@ -2058,12 +2053,12 @@ int main(int argc, char* argv[]) } } - glutMainLoop(); + mainLoop(); - ImGui_ImplOpenGL2_Shutdown(); - ImGui_ImplGLUT_Shutdown(); - ImGui::DestroyContext(); ImPlot::DestroyContext(); + if (photos::photo_texture_cam0 != 0) + rlUnloadTexture(photos::photo_texture_cam0); + shutdownGL(); } catch (const std::bad_alloc& e) { spdlog::error("System is out of memory : {}", e.what()); diff --git a/apps/mandeye_raw_data_viewer_legacy/CMakeLists.txt b/apps/mandeye_raw_data_viewer_legacy/CMakeLists.txt new file mode 100644 index 00000000..106335e4 --- /dev/null +++ b/apps/mandeye_raw_data_viewer_legacy/CMakeLists.txt @@ -0,0 +1,79 @@ +cmake_minimum_required(VERSION 4.0.0) + +# Legacy GLUT + legacy immediate-mode OpenGL build of the raw data viewer, +# kept side by side with apps/mandeye_raw_data_viewer (raylib-based since it +# was ported off GLUT), the same way step2/step3 keep their *_legacy builds. +project(mandeye_raw_data_viewer_legacy) + +# Source files +set(SOURCES + mandeye_raw_data_viewer.cpp + ../lidar_odometry_step_1/toml_io.cpp + ../lidar_odometry_step_1/lidar_odometry.cpp + ../lidar_odometry_step_1/lidar_odometry_utils.cpp + ../lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp + ../lidar_odometry_step_1/points_global_spill.cpp + ../lidar_odometry_step_1/raw_cloud_loader.cpp + "../../core/src/utils.cpp" +) + +# Windows: add resource file +if(WIN32) + list(APPEND SOURCES resource.rc) +endif() + +add_executable( + mandeye_raw_data_viewer_legacy ${SOURCES} +) + +target_include_directories( + mandeye_raw_data_viewer_legacy + PRIVATE include + ${REPOSITORY_DIRECTORY}/core/include + ${REPOSITORY_DIRECTORY}/core_hd_mapping/include + ${THIRDPARTY_DIRECTORY} + ${THIRDPARTY_DIRECTORY}/glm + ${EIGEN3_INCLUDE_DIR} + ${THIRDPARTY_DIRECTORY}/tomlplusplus/include + ${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 + ${THIRDPARTY_DIRECTORY}/Fusion/Fusion) + +target_link_libraries( + mandeye_raw_data_viewer_legacy + PRIVATE vqf Fusion + unordered_dense::unordered_dense + spdlog::spdlog + UTL::include + ${FREEGLUT_LIBRARY} + ${OPENGL_gl_LIBRARY} + OpenGL::GLU + ${PLATFORM_LASZIP_LIB} + ${CORE_LIBRARIES} + ${GUI_LIBRARIES} + ${PLATFORM_MISCELLANEOUS_LIBS} + ${OpenCV_LIBS}) + +if(WIN32) + add_custom_command( + TARGET mandeye_raw_data_viewer_legacy + POST_BUILD + COMMAND + ${CMAKE_COMMAND} -E copy $ + $ + COMMAND_EXPAND_LISTS) +endif() + +if (MSVC) + target_compile_options(mandeye_raw_data_viewer_legacy PRIVATE /bigobj) +endif() + +hdmapping_install_app(mandeye_raw_data_viewer_legacy) diff --git a/apps/mandeye_raw_data_viewer_legacy/icon.ico b/apps/mandeye_raw_data_viewer_legacy/icon.ico new file mode 100644 index 0000000000000000000000000000000000000000..710913e34e4b182cb7412859d885face7f382cd6 GIT binary patch literal 25645 zcmXV11yoy2n57ho6!+rp?hb|G1Sn8kix+nf4#kT*6qn*2w77e5cXzk2@88{Ua>z+C zlX>$#yZ5^T1M?2}`|t4%h8zap6b1$lcpa{yB!h}fhzz_$m6QFV`roJjJrO?u|MZ+n z%wb?m*5tl?R`*ys&9d+zo`r_}K?)Kd9VBJRcS5Ou z1ydKiL!$YYF^Od4RaZliYOrd%VmfrxZkHpl4^I82tz*q6#- zk}BxRC?zFjYGx*ox3RL6{(9#ffzvdm=7l~hIv&cC-+&I0o)UDY^ibNr+pkVT^ z%r z{o?BCU*|siLV6yXA;9oeOjv}lb`Xq4g-)No`|4OA{KWLaLbEO!znEAK#ChMT4_A}j zZiRmzS&;w4>Yb&n)oxc9>CU*&c|m!(P1rez*QHj3^{Sm!74>}+s{<0Mnw*5{SCR-7 z+=yvnp2evEP%3-Cab;`F(l;L^3=XgEF z$K`cnx&K3uAEf|>+0z6PbeKQA5w@TZ)?3_ zcr5--A3HH5e??z)EMfOHxT2R~0I&~_gWNFwqmukgrek0dxQ)6AF^jboh|Nkfhw-yR zPMDz*^G3aK8Ud_#36!TC`lq)$n-DR(%w{9jFnNX`nx;+Ec8NRN>xh9?Ir~IUA0Z5i zMr!(51QDFB`406+X##qcjeGN`iN^qWsq#f_kC z0}nG4UmWDe#>d5N>KP-1kyOtNiL1(=HE+k*M)}pR_nbb*ZvKoiWv21=xN|dV7Xg2H zL3qaq?a}Ogpcj|#lY<<-SpC2hAl2+Xu4=y{ykBufS8Cu2tMr!sENMb@dU`rO^dJa( zvF3g5`1X2FXEWkJw8Gi&t7j&G!yDz%T{sw@AYB|E7U@e8Wf_j@g0idR&n!`T1xBSo zx$XXN_y?DYfj*5D*Li)Pm&<{1T(`g6ptIItglBvR0vf8e5+;{N%S)3kZ8v8F1g9Ang)Z`QeJ$M4<#bLT~Ya(i;E>K z&VUd0*YYKei(R~`Y}s= zm4b;|A6+fD{1;NC;R(~VL7RR$&p1_Eck|pW!>urrtHn`o*}eo; z2ZlAyal9qmt{AFM9$q&Yn~3QKaYA zwzj*QVQI(bDWtH17VcIt7e{54yAziUkFX^$e#Ps~rCcBL zm?ef$F8i+EwSjemFu{s8KjQTRMnLx2qDhP_ZQg}iJQY!P5C&xgHg7@&HR41WdhQ1dm zWgY3A`X+W{LzUvvvM~IUlkwJcYN?HvDbm~|VpqD@4J-qT83`ZUt!x7?pM6Bf1@?(6 zTaFm-mhC%jGo5o)e&UX#INyuhUyO21V1zH^e}EURFAU|7Ghk&M>{=SHWGPHdNa_#9 zA+ug;DQ1pb*Bix6F$|bLcCCC673U|;YikQ+>(rY_F|6F~i4|2${lMGAeZ4RCmX=m$ z&T0Jer9Xy+mX6LMCzJL>1%}sV;qTM!aVshjKP%h&DvY~}PIPVxt#!=3O3=4nuQoa! zo}~3R=)IoTVd3)aFtA=Ufg6>giPWxg?Yf8CD!h{K_z655C#mr&T)^18xF)Itt#1{$CmDQZ#-Q`Hd1n9 zL{FS^*+u?0_pjRRR8R&clsrJ(zYkc`LVj$m%i(loVMo?y1}R-#A|}mB)7{~umt(`X zfq$hl{MS)40@vkr1Y+QqUt)ejF0hpb4UEPeBD{SA^i|%iJ$@i@GydW3rM=)&iyU9P z6n(cvR@9nc3_bAUZl+`ZC!}S=Dyt$!V#(!k8M#7&oD-V$R*(2k#9D1D%h7B5@_&1Iz z!H0aG@Mk6_rXC~|Qo{PiMyLNaaZcOfax0Pz3pwP;Cx-&g(|`{iY~TI9C*L|s?k;YV zv$3_d-fRR9eEY}Haqp8wwZmpN1(8OF{yRi?Nu(}pm=G}A6B^z&PIU`UYW29^AVnCv zZ~sk|uRoXh=!XA$w1cNr^@wk_?}Y6AN$X&yd{OK`DyfO|Fu1$BsWqu4B0-rzYIIuM zO>vwV<5M#<>>N9`16BKZVTm!1?+0jadt6Qqt#Di6c*RaxG!FE=M~K>Al%&19|J0YM z_DeFL^Bmz{7#Vo*aX)8*#C_#6KQXD(zH_Unf6QG>Fn0OJF4-KS#G{C(W0F^k{vZ!8 zPUk)5w(P2N-}~oR#T*YQ1)YOyPn{!9<77UbY2(~C4ak!B-9&5@-P2T6> zy|E0)Rs@q)n}=ggsNWYi!fTm*CrfGMPoZny^nX~BXF60_BG#g0VagfYDcm}1fbnoI zOOJu{TW+hs%s(?Z`S%U_;#XQ(x;NkX#4|KBBxwkvICJ9E5g^iePt=mtFfTJ5OVWhi zMJgp2jDdrLYxic+`4dej6(*+vv7X52t5dZMI*^oGShqDTj0(rrq=M zI=cx6`0ZH?B~7A!3C+fmsCYF-I5ey0G8i(gRB#7o`B&K~Gw(`A?DA`Rdb;~|-mo+J zvNhNUTB1iPU?gm@Z?QE4$FZm%A`e5E`(V2Dy6TyL2rnOK#&EUD3g$HDe-kwl4znnx zeF-;{JL`BRJ%j9%77!DP2AnR{R~{3HgpMSuau2w@z~TBA%LFIIN0beh+57xz_rHc>K>18l$f~-3a~~F*bnC0w(q)XEHJW8f}kB**nea zyVFE5tD$7AE0Xr%hIlrIROj?(DV09NhU&Xh(&M6Xa2aD5F?wf&bYqB+(uid0B&+T~ zP{3lIIM>Kd*fFJ$(s0AqBh5sh9(RE!XAtnr_*oJ!o=U>=IE|^OtSo=g41U;@Dc8Nf zO>j(R(e2;Q@mt&WhgX|AwC$t{PUZ!aqcz@u_-qjv$N{H z4qD*#Q%A#(u0K|xFT~orZ^2OilJ{cn3&5wg7%?C1@n=-qe==Apek_IbNWq?^WpQLx z^u}j&$AI+){8F`SFZzfz=ydqvv?UvnfUQ|a>i6O(`gD+=%K#(s)0Xp~@dK5*OkU&g z9Sw{;$MEsq!Ah^h$5_!TL}lU2URIJ+)rjY_R{IRdI)}hv)Ey*l`SRFOT$^6Pq<-0O z^R79K-!aeRYtYY#VR=%IjjmujHMN=e<`xhBv&y6rJGQzatG4x(u6xntLEnr&rKsn6 zqhabm!29dU)acNY&RItJ)rI8}$wIELfSOE)6 zD7bKtxhWEo?{rd_b^67efkJB)k9yifOkhz!iS#e|Hh_W5Z;!s|ceVysKZ870mZ<}+ z(v6ILKWeh9_YRZd=6c239PD_n?s({V)Z5y7t1)D=R_zZkHK~yL4Z2cQ7i|4A{xmwI z^eL7|~tnsl#{`-K0O~CQ}%lvtApmK)bN%6Ux#^(ZcMP<-d-c)!@LV7x3WycdE z*zNV!W}!NFf+4^vyvlUO5AssWXO5 zY%L8Ig%UOZhE4d*Cvf}x?EZeew=FPY^klDNi;>6>Td^rg?M=5{H~~~+AqX6 zB&^@}3S0E|>SQZ&yT87D?Nt54_LCE%SH%+!-SflZLmY8V zhIDPEaqGtun1FCOgP0-3?e0K5Qr4<%X#9_*@2ltq^JN*S$?3ffeUC2Qgz(9MJzQ$3 zaoM9`(YE^MhHVcZ@Kr(%%CIPo5DrX=vBT)pz{LsI#Ps5kiL*@V?I{J?n5lTe{nV8kxOWduuU`LErCkVQ!UvVtnsac%FBd(7W2dC!?x}d^yp4Wl!8z*e)7x9; z&-jjCjSaf1OXzFZ13s0OGrx-s)axpi((tWi% zg0Slfm48PdWJg?QKBl;Er{Vi?jN_Oo-={E8I3-e-HhOFPu#Y$~Eq+64l{6G%bXa!p z?lltPDo@cPTuGvV^+8B8R~TnEsQ*i6*Jhmsm(K&@K%RTtoFx&fadnX=Dj(Z3s&)E2 z${_fC7rCw%{DLA#p}Ux-Qut7!x{YP9<*rwK;~aO-QP#^Mg%OKP-FMaNvb1XjQ($yR z&gG*0+q?v*x1d-|g^~Wsd+?sNKe1e2i2oSmQwa0mBdR3$xD#w^y%gxPMp&Jq(4h*4 z!wO%#Yj$z)ZX4pa$InWTGy*-8i@Jaexe%E)qh7V!6&2EORFQ{0o5$>4zNd3=1JjZk z=iF2+CD6jz^VO?P^O+Buw$xo(n&Fi@?4Q0@-FjJQwXBN-E#x+gKPlK;Ag? zg#|!fEGsJ`4Mp2lYbzPwz#coJDGuI9*oz6C&4%p^o4KmPy8epe`{p+>hOCMj4J1>C z+v>Y3qhMDO3Kx*1rJI!_4f*PB9yt#U={jcEA7ZbBE;G@gYmarw2XPcHr8qT{U$xK=Iw43NXj)C8qJW>bW9mQBnBss%@aR;)oT zl06u(3Z-!NolBdx$Q4C>3bP=jWjauL@5y2vi4Zj1<;fStrR+V=3wZGy;&nBfsp8Ph z2ep~u_Jls}dtUb8Bf-HUQcQA3tI&V>=IYu#tFJOOC=-3!jx`3SaEvYE&BYo2>2P^m z{gP=(Nue|q7ybJ(p*y{F!Or;DH|A37nDb>Q1_fMU`}CUF>y6spL{4mxl|BRk0)b?@ zuXz$X_F@L$twRoI&;uyJ9u7B0K9PhG9mu}HK zN8VMT7kuC9C4}#7*t*tv3@d7VXymCN)4lHdEVNF`#x_y20~W^@WU0}Bw(Rlhm{eqH zyU?~gJQX(Jvl)yQrWmx0soL8!7kbobhC@y{C>FzHeg=Q97ddYyiO336OzEQcYl4CC znVcL(K&>!@oG&fuWcxuMZ5ul;`-$2vx<4vr@Y+_jRSNO&&bekej|r$~Sk;S2NiYc6FRLP#E*w&**>n74)KZz1Es$|MkdsE;#Iv$Cz**XyWBT+-{{0!mtuudWVH{a*g6KAK4kq$g2W!oJT8{3t zp{%u-JxPun?>En!yjpYkoFx;9IYPyhMxZ%?TAzw693jyXuM7USud8&1*s!I{+0331 zp|bBaK^+my%5$t_BU3VDAY)?+AOyeOz2+sa6n$B76VjY|{!IB(Uaobqb4Uc6PRaCY zXDG-JMpad{`C%*4b3eR`M3rbAZfNejCV8~2v` z{^zT*g2Hz9N4DMr3m!6?Uel;06_$);-fOdCuf}@&Tt+Q9h58U9<}d2fr(X%M7Dud2 z5Z~)tu4?9veE;%7*4G`uSBlPaF=Hi`%UH3vD>$ocV{4M0NZ90br~juXI3p*XICbxIHO}zG@K-wjR*vuzUzq zCK5H1T$WC!8cUrvXguZS4iHB~1^LhAJIwK-E141L8h$WC3}Pef*yx`C5Amo~ZT`-D z@BYLefgl-39m!ukVlja zzLdmTylS(Ac4KA^?T6QgxI@BqvaFFQ?Na>?CKxrjC8y`$)*cBSZ>m;q ztKs3{t!V|eJ_pz1{=FD>W%YmOH!u1=AyXeaoNnf%)Vn%Ijx3KZZ9u}it4tlH~o|pVCKA(nhhh$(P>ZJ!zpm1ZdMdPsr$6;uGa_=r12bHy! zYuvlawK0a8tIU1vx9@mBJy-9%j_#BJItUVLp|Eoe%R~A_>iS|F3Y=6T@P@I->bqTt z6Pi5JnjI|%$0_51;;=d_Lw>`@*u<0YqUcg`q?E~0it>sKH3bEtKkU?A+`5C2g2o@* zwP9d7o>$K~pD%g~8;XaIwaF0aa9|hX4(=RLpOF78Z60!ScOi_Owyhl>`T?x3!%m#c z8CR$wu&h_ZwFw?F{hk~RXxhECSb@kKa49HbKZUNI3CSy z5BD)y9!L~(tn?N&j;%jDKfHozLFtLrwr1A;zmo_&Xp_Eo;*hC%)7-rLsZYMoSYi)O zzJQX5Dsp$_tZ-kgR-Pe#F?$vt4*FW=-J3xW+V$`Ak^Pd{yTQT1jWVVx zMUQTckUU`(9Z3`^i-p94o}WT83BLNj*3Q+0Fy6hZPmTApD^+q>GBq~`pVCqXMGqU% z+?PMo&HK2ppxgTdN5sf;=emFL&EfM243b*tW5-r%lgHTUNH)a&iH=JhpMb2iRX??U znsV+om>Wjza<0FOXX?xzuL57jwZ799m>GY}L@mC!a0Lg?Cc1 zn%oZ0p~JzJGsLlxb)gpdw~mxzKYl1QpF24_r}4_RD*IKqCL|~egd})NA#|uL4hluv z5-c^o?_2A6I{oB*esACOK91HSwXJ$#T=!@NzM4|@{62%t z9YVyxjh|lhzHGwzk5`M@xBFNLilkmuECn5{;qFAdz5lrXiBLcxTQdN^9^=f^-YOe|6UYhj=uSuo5=k9lWe~i7x6)-@XI`0 zD8@m$xQr3tnltpVy$`QX$l&&(s73>L`w6n7k$QD8jo6*{6es?5eBKrOAb_Fi?(Y8V zDdx+<2U$Pg2SB{pVXJ~wiT%&@R6KXP%J0%TSZSeCxDB&Z;4I#x%e|6LlylZ+ z?IFjZdU0e>|4^+>QG)huioovZGOf%T`S)K{tj>`E^!_)0uE_CTj92%yj~_p#aao|n ziauG+H>Q*krD^^GWse-lPku$7c8Si3B^%m{agcB^@MP~sWm51c6HYU6o>^p@jWwMA zD(=kOPX{7t)1L3%i?KEN6f>f*Kiw_-MOh}V)*9Vsa;58vbX+W8V<$n0xZW4G<4$nV zIE7=c*_#eFP zE`~OsndYlurlvF@G~7V*U+}uw+e7iimXVEl)?V{^W%mLn=~YZ2h0Hj}Rc90fDHEwX zlo6_t1WDWxdWNiN*(fh9WI3BXg%GcDjK8OU?DqRAs*HY;Hp%+%{8gu2Tw{h>48J*? zT$nts-kK47*w6Gv`-sO(PEc`W(l=(8%(MU4 z$UbBUIPWlomC~3WCL#*R!h4%Gq&uScZxKX|L5aoO{7&(1Vj*^l zAz#kD_}u5Aa_R19A;+Fm(34v%>rTzwY+}WB{keoHcIziq%zw>M{jh64-5&yiLeWTI z`;>Q)&4|QVNyB1A_|Df$wZVm5O5ar%Maxcqf9z@svv8W}{Tk&$XPy)BaQw0C*t1v@ z+6CFqmUMKy?4W}&LZ7YdgnF^I-(Ync>v|qjaX@Qx1D}gxX?iAGl9QeoshX+6W_Sas z;BYqy+-=I_Eya@K;=10T=Wp>zNewAkOZMaGdps*0M`&{ml>?JEB>cN4#qG2%{e>~R zB`^8>-1jQdG_l=(?qem@k$%c0lzvKp7g_V(N2r#{oGoe|MpaCW}Or^_1Dq!VV@F30e0`V8-yH)TjwLH;7TH5U)5`OtW}u~!rNVx9jfH20-42fM@(Y=p z@cLVsO=oJLU!uBg{+ZZA7dk)C6b#M0^wYcN@H4o@{}I&C>@{}loy}EB4V#+I5j`WF zGfV*q`*o)Dw)bKEk9S+j0=nci=XqQnRV4z5~f_cni8IOT7AS9RZXvGxoRNL(-g2O)5R7Za;|OBJhZP6~C{MhiVbR!fVL zCG3Jo?O#=3u2aq?P1_GR9M|f4mcHOFulsKWW70X?CjNPn(sK(*V$yW#2dH>YXZNT8 znVDwu6NgGx!cZCPcKP0A_gGSNw+Q+^>xMDCb{{p(WTTFWu>=H=Y2bP!>jCohsYn}V z@NY`2?Urx4TGK-FJ39#;FEs-QW+Z>JZxjlnh7#zmM+~8y6uiGz>ziGD1!%g)VJK=q zyc?RI#1@d-%BXq``P8Wi{B(2(JE5~xz)fPa*XzpBgogB!_OE+CQEzu7nS_z`@G@00Mu@WP3*hW~e?|f}OEXu%&XaTU!U3AimqX|t#a&1bs zw`U}a*|Vgu#IM6?Yy;k4>Ch2zs_g7FPm_M-dW<{oIk(`49Ndg14k%Dag725tQN6a$ z`QE|s?`Jymzr9>zH&)nwyxNaWf-5*dhFMB76GOG+WwBdT=>YwyVybKo#8xJ4Tbu7# zSZE0st9tq9n%fdg=cl|`UmzF3l*UB&_RH|ZZ=02F^MskJ)`F9_F!c|u{tyi|*nfV( zId|51e|)#)xKcrDE1IFNon}_A{X5qmp>DTX(kZIHoyT%{2iCuso*kU>PVU^Wx|m$Y z^q%^PZ1djF%*;S;=XBF}tml3;??18Vk8fW(Y)A9eof6a@qhw|+Pz0qt1*F|-|H z7HAWc1E&)5LZuLI5-;yRv{|^|gNW=E48&^{ZITJ~e=}1|Jp2LzX}Wfh5Uz1ylff1D zr!k;u;_Te)qFCDuT&4-7D4ByH+Jv;}Kp!OQ2a7F3W$q=&{MWMPde^`$+4Aw3#FP-ta3Y=>XVJxC&HZwUK208`$_agu`RcK;K*fL3v8tPeDR`VxcrDbH} z<*PseMmbg}hGDvGddLe2B5~_sYCq6sk zx8`dyJqIYfbE$nYFd?Bu(hA0a9;aVEfvCz4Xm5zF+i>KA9a6q zeH}JBRE4O1b*=2DWaX`$14U6tj{*^mjB(lLu|pd5;FKXOK9-)*pNFDD`qOxEs2>^O4i@kCq0Q|mz52Jf@YLw$~gsx})_ph3&%7K?mk-~YR_APE6;sbbs{{sEh_;jJq&1M)tvn%ll}%jA01x967!_ zXzWbNHe-s^V+mX|X_U-rc~z};V@V+cMg zDJe+|J^O~Qvcj}fUeDw}6C_Yyr;!#X)m6Kzl@ELsED9wj9~}Uy%=z_p z9=(*z*l%A&6E?SE0ct$lRbN^==pY|+@$I4cml<5M zS=}KfY=7-fGR8|*<+@5>Ls5z5Zl4nw)CP$I;Y~k;z`ziD?!>LU{e6#vi%UcB_8wjM zFTfZIQ7hkA?^e~-*qW@r6C2*rrRwMdr#%a}@7Nh#6%4Ih-gi1LS*`CrufK`{8`3d){{BHq(po$wrqf|0q#&zWW_T0+&kdp(P5&fmvNXm#nC035|h!45szK>YjbGsBRdeD+>L#QfA2 ziSJWHRkoNL>9ZMVxk)J|hzdqBE2XKUyN8JEGegoOY{j3f6ixxgH(4bz*L3GvK#58q z-=_b{`=;kk#_=iozUWxmi0v^NL^86Qz`CCQdi$SP^}r%|j(N9nwrR%L%?0(<`#XbN z5nm7)=#fnNAxwyF)VQigF7_#%xl(VG#QN>Cbpa3Q$&ASHIrG)#oQR&lp}e{In2S>C ze~Wpimfogv2a8_m_w)Cs6Rv4xhLB0PbuvE=x+vi_0V}i^n41uZnhrCOwCvH(ahUU1 zwwvq6Z(|C(RYr_lxy@nn4;m3bc-mXZbG}PIYe7Tv&VoUNQ(#QFu-Y!x6TNgo8xk-j zl37~oE-RW{dE0$uw$}+@<*pY+S?2W}It(ek4!RUO;QBb(Wo-XFHW}`{Wdyh5wFO^I ztr==@v}ls^K&C>6-?c8*f%z{DNp=1gzLcXyiYTo5pg$?Jmzi$M1>utm}2_jmk>k{tmD^flpvyDj)s1rt_h$H5(;jOzyv#>c@Se@o%vi!hsO26H+6sx84j8dB_SK-_X`b%YR;jsC`h@t=-Sh1PCW0Tvlhc}P#49RGkS zCRHjmI+!_rGy2LP|4Fil?tSdpV&Q})y@dJCP>1)rOHI{fo;-ij=Tw%y%FC3KfPUHr zdNq1JF@=;;t8o=M?Cy}&cB)PIRgq&wU%edLUyMJKu`{!!kd0;x489nThksIpg;60Z z+Xu-0>gFRBmEV1uADEd7;+sRlQ0zZYz4S0BG8B+Q{PmR4tj^R`S4x|=^xav^FgYb% zdIc@R8&yckbA?ruJbz~{8L`;b@P42EB6$qRvoy0*rNmHH?kBI z6l`HvktOUw{TFtmHCK%iERIjeIu@efp8t`A;-ou+!wO67E3?&NwwE|!KximN-X9G= zW!w{M3RT8%2@VclO8n?n|DG$fby~zh;|YxDK98G23r}bRA0Vc+)=Cy2MLE=Y|AeKVEc)tVX5wPSx=PZAr@xl^vFGYxRvD~#X<58E{2LFZI?Kgg^ECzD zY@#ODj0)LGY!`DGXH6yR4J-UZC)1{sJOi^CMX*t@AKXCn10EfI=AZ*zu0<6kR`}KV zoAB7js!lsY6N+l)$hJ&k@KX=dg5R*|J~b-*yMqFrrQzvxcWrgZPgj;>#J)hp++3t~ zCQjwttvS}G%pQC6QXExndK|e*Q`!R1#(!W&>n!&@X^~AjZIKmL%oijXO0DE@8eDn# zf_Ow4jC6qK*PmxcA*mV&n&1A+hk~L{cExAMkG1)h?GcfJ$6B8+v)wPo%$ITu_7Hdm zxkdR6B8k&cF!hEeqxt{@1CW99nX8lTEL&}fC3^QC?)8f z5>D4%5I&|x~k95P5oO@^fkc-q`~6svgE}5 zQE9Rawq&74*;j}Z80n!S?6P;yvPxG8iHW1tCpMg1Tm*4qQJbC+l#wwSDi+k7iPrg0 zopZ)2R)g*=^j9+201P7?ynLCMopc2@=g)90MVpSTatd;^bWD+fm{h4M!l9QY5%h%L#5FYbev`1+tmp+==``!J zem9Te$d_?mP10nqlo|01t1?%KfpH5iWTw;zj?qHEWRh2u;wVId)$0yXaNeA1rzVnX zcAc=h-qB5#(wC$<=Cw0jEaNHH?u@HQN~&*cz^r3l{>y{PBKrN2(PmxxT!F@mUtDTM zV3=2poPkDlQ4Ns-&MqY~b5yZEhQYM|AH#{58K;SKEG3PA=cL|#A(`(e$-S7k47;r3 zrX|?v(Jbat2)5cHPE}P~ZDgNG;|f}wQ{(n3eMCsaVL~f`ZJ9n7R>M>fL5JNJ_K0YV zKy3x9&*#vJqSs<~-w>;8T@Wt|w6P;o&Aw}Mo>3j>9Z{c=g@24XNQUz^L{`x8Cto}_ zap^)tTk!KkOXEq9+m4V^0IeQR+&+OaO6`Aq-&*+YR9BZpG>PdiB5AEllvAZQrhDEo zvCpLWb$;PzA{Gd+X~jHi{hFBzoZr@jqE#vXc1)dWIfur^ziTO>9UfZkn486AW(H3j z(+utRoL?Igd`PEN8Ag+hkTwbKHV(|i$aM&AymLDuB($=z%PXqPe(I+s%*e1hN5lx1 zj#JIO#i9%l&#f+CAvcIk=?=sscH4MI?E8?bNKYCxvg^cJO9kVN-f(;C^U}wu8lNEG zv4`@~A(G&$sbkRf-%_{S*G-%cHS%icz4j9bTlRL6Y2Jm*Z&UCwMNYv}5$I~kI!;+4 zhoaK~wo?BOr;Sv3ejTi)I73ZTrKNOQPntndzknD{VxOWb8+Y zeeQfUlxbDfR8dz>Ix=q*go*UeI>UeOxqhkE`rHyW+OSv*2VW`@^Uc*P`D()errX(i z>L#iSft8#WIJ)S8Tx~_llKg)BqZ)CWA@5-|mC<$!Mff+z*Bf@6mD>wTLz8Tldq}8# z_zAe!o+vA|R6$2dseI}9xa3gdlTUBf^u$G#xpC1nG|al~$w}68sv2MAyvboCj1E>W zapf5lN4m`j{u|m9E0%0xF+nTjYy_KIkSP2*JtBO$at$X*5xTo`)tY0dJXP{FX7}t5 ztjbsYu~@%bYuGMw^6G*<4gk5`-m}sQFjzI6wKjHS`-wbqN2YYhm?(T~sT>HKZWLRq zJ=w@zDQCXR;SBFV%j|=y6|oDmxmS@$yTidICfe@Km@z$ET(d2xs7iRtz<_2ZU06h= zU_QOsnMg-(1=cC;C)$%N+vg{petN5`Y8&oR7K=8MW-5b`X4bHUs-> z=id1de*b#j51LRLf&9FOBM9tLWPJRej(ZW+_-8{n#Tip;Gu#$C%@4KALJ)d3x{Q2u zp+xK$(fIJ~{w+uJwLo&fd8`uXIBc(|_p%%bbC`Aetrehv*K1;;gxS7@R$?N;TiIH# z+xEZx`^a|H>pZa>qfSv>K+VFFL}uWFnJ*pXP~J#3W#93*mw~YS*}}q{(`*Zl*M>E> zqXrW+Kgr#-SIc=sl#wcG=F~})S9z>eG_?Om1DgvJfsCB${^SnXt37wVavWf1=ChCu z*?96X=*Po#9K{Ud@;x%;{!P(Q;JHJA;JD|m)W3bW{+nT@_F)28iHGub6El!7Abas- z4i5nc{CLX~C`V?0fspvJo9%bCOWPVnK(9En^$$B{e>jruh_LYvKRK~{wK{HYPJ=cI zkpEk+m%@`578KYpRoL1_v60s428J4VyrQ1@-Eb7BT1o(^=Ft=K@=&Xg#^G9KQV|1V zqu}`s*AKFvQ@&h{*~6})8kZC|PSW(^mcu#Or9_Rj8>$PQSVARYcOe<69h?~1Zej8} z=)F6a{@^PW8MxU4ISbQd@9;pG(uzv`WTItvMjDCg@Zd!d`WbA8mTZO@-8VQkw7E3G z<{ah6$7M?Sj;YMr!}aoe-V{9@i8qSL$B$Wmga%70|7`DuL?VXtWIFu&sx%o(1p^$a zYfYdq38DR&UTF1 z*0AqW7F&9Vry5VCfr;eAe(@<$iY4D z+Sou87X!q>p0<~|+)ed)hlI)^Z?L%zspk_S_ElH`TFlbc-a@yi(dzTF16+@UmUdQ! zrG-f<^igJMvI=>L^?%5Z1=Hf_ns4PMIzSGB8iiBcE6~r&Yg}RKlg;8yfx;zj%f0Z>>pEV#ItY zsjM(D8N$=>gJehRHmc~W$v9gxb*T_2EG7LsZ6VD&?wr)GSdi{}>uklVt+eu3wFAQU ze|f{DOO`f<#eBNi5hsR`oQlOY7BNsY0^h(dXdqkLpN54SG*CZU$t)Wt((}t5wo<1l zM)$le<0(|yML&`#Vx#;#vpd8hC&b%7-epNhP^+^vSUH)+LR+}Kp`ju=)oM{nXo~@& z)8;X7SN3T+Eo|BRCVYEqI!!3ECf{&Fz_cg5}em7AM4_t$2HRP`7K{mA+o!yKqGwkslo9-aXRO>DcxrN=93NPk#NOG6nMqNSqBiu$KMJu?mH zF9|}|%odW(u%i195aBFs>~bsGqnDmO{FlTnf8;?LqE+~#5;4A;mwpIH@eUEdkf?2IR_`MvF131#tTe@V(p8%n}A>^%uWcVY&rjR2F^qOle5 z+xx`Cq(!QCg$|!sp$^*8feaHEN|~^S1U^ zv;juQ*oH%_eai{@5G%B(qa9v(Jm_(o4ehO04D7;4FEXK{Uv$2_`G*78^WH|yI6|9zU8cIsuLou=M1mXcZYy zX$9zQN8P2sSghW;6y|jV+{X;QHR)h8~FZ9 z=f#u6nfC)nUJZd%J~IE-uONXN|FkGp0*Fisd12zef3tNqvp?3n&fja4Ybz*t3ts=VEhesjNqGV#8%Y36+NZ-y^)09rgOrpj zPmN`iv8T6%Q@mArVzQ=+zUIIKht$sQ&Iw+Z)epP=?Msxhj`zUWeAz)d*z2{Uw>O`&?l9Q~RX!x@pD7c~P&Aw6!&?+PDo zmHld!T^LAD_&*VdAcjhii}Rhth_bQi^v4mmyEu3g6G(ud%?#i0;qC{`bBRr;5!p(wht%>_BZ3cF!0x zu)X;}wZGvc5H!-|zYTZNSNrV`$!`0@3X?j4$@WjmZ#k;pA!x}$ElF# zdJ`3fzU!*a@i&o6bxk^I1{#Cdu*eCa6g6dGiZzT`WA=?f7cu}@AkXVRN`|739H*lZ zq>mHXzoxj0J$dAa@*VTp?_FBt^l%GB8*bh|L$03jotNb31pChs|EgdH4881xY5vN{ z3bE2ACpYh>uPR@m8lWodt!Gx8>E#A7Gu#h4{+s2S#6)x{yzvsYV-@ie9U${8aGpVt zAXXkjTd<#|u7uoRop~L+0U6Rl%Z?RcEVD-U{!?TccT=OPzODk03wOWGa`W)bOwUQv z6NjU>@(7dSAC@u{6cxm0WcB@gF^(FYpyLyYr0(=#)t+;LTMj4I0CCn$y~o0L2P;V*4^{rIuQOT9>n+3eYYm_5&{7GKpYG`WVy#?5^$ zQ$MT4PIh7Xx5vO&we<(ZipiNcf8>~vKot5@RaZSw84;d1tn@g`tr{Pn+OW%c!rTq)o)&meDLihI^ zSK?&&SCfIm`UeFC^@+>pzW#f2czUe(MDp^0PqgWmmE>E)HjS8xL0H0RY$sX(>?1Op zy}WO3!NE)yRu6=9)r3n=UsxebW>IG!I@&fY8}pN zdK>YL6_wWY7HX;X?JLgqc)@6kzk|_Ylwvo>y;gNwei4>vCf4)`U`F7Gh9W(y`Ec)L6d6}2P^HT% zEA(x<`I)IiTWP}ueI`s#XZTZMG~2?a-QmSD>v}ty${D^Ln_j1|e%c-@whJ8~1#-fs z8w5DLsXjM{QhH36lt*Lpl`N{Jl3_322D^$)9%``lyR@YZv@*I1+Bbe5M? z<74vXS@+c;flJXAU-=FWlTo*etXP!B8&i&l?X9QAoV?>q>2e1>R@-ym&&jt6>o=B8y0ep|jvD=D@YAAC zrJvs-cOVg(pH-`jTCjvisDV6?J+2GYCB zM}bMmm=iELuqku{>`I$GkKnnfH8X+uQ1yKOO(CXtSF{$0=+XV-C62a&*b%8nm%zPx zxwkj)UhTMW<9T0e5Be!aexN!uXHxeCym!W+a%xnahYe%}Se~sa*bvwsTB#@DCcB0h zSxk9dr?Dn7dH+jAXGiE&Axh>47HBUa6xH!**v`R>)XtdFn z&F)z|W_V&IqYZgWHDl@q9Q8U65CzP4@US{6yxvg6_MBgZusKXuJ)t68cWX%2T|M)y z-w!#(FG4SjAm?S{R9;L~*H`8R;bxDH;@Y8^h!HhX|9-84Ix@qdk9WaIY{3H zh!~nOwwYIzD)PuVbeFP{AsX4`6}pT08QABUI!YW$uTdQSIA48_a7kndes{qmXN1ve zP7)bf%)J2({ZTEB9QXSI{@PDb4&bJ+Hk8M610f*-R*+1sueLrVH<+h;-JLSNNA=|T zvZJ|yA@Gk*=rQ-x7n8X4*QZ9p5T$+k*PdECCF!N?6eav1Il_32s@Tv-Ti2gIiIoxc z=msIU6dse*Qt_8(YvW9qndxj~X*d=!2-i64Dc<)+OX^O1>Q0eLF(WJswku;moNv}7 z_l1b^<-s-eYd*2qls=pFx!4!Pxm>a`-|eAXM*%m4_T~KyBDN39LklUKd1%%b6J=~B z(scn*Aq(4f2;^z_L0yd|P1xfOiT68%!o<`64{w77s9&s_nd~r+^u8yA0BKg`>yioI-G5K!&4bDr| zQ#!O8MkW@dNVJtD{Il&=%HhH(8_*oX7(a$z*K!D+6XO&;oY90ek*R$K-UxUnMNLx!DOK_kR!6 zOFIS`QRL-Bo)3D}w!jjE2W%oB8F@0yBN#7fu2v>S>aXwqDFLqpL)u-G?}3oUL0^6p zw&N8B&-{2NW5LWQoq-*e{}L}QfO&`yCb-^y;xYD*{ppY@TR*3&k3+qec12*?2(nPj zV$7Kv0x@FOF`XT4cc7;ymlu$ni=7#5On9x}-kWI!mNDaCvK-<4CXOqI7lhcNnXX(8 zLfEU8`)>Z~kvtjtu4p-iDG{NS!Mi;i$y8j_uPIsK;0WljQ=u1?+|r+&*E=>zYE%wb zo7)qXtL#Z9=we)dhrJGX|BByIKu^ub!O9;ZNp~)8B2%&#qfx-}%UXhT0$9XkipPB> z1Do;5}tp_?vV%r6#aX)Gyyo9=>~Xv4Vwc1`imsr)} z;-Ab2pU7E%1{##KZ6keUK&X5t1gp}wnxq{XOXK#p?H`NDNL*tiHAhUhM7%nG?zEL_qSkbeAo|G^>UaU zyKY)uEh1DseOq61aZ*IJFTxZ5`5}W+BlIJZS3|>y#WC0}o}0u+nK9J*Y z3VN1p2e!MB%w7gZ2D;`uS7dhctiDbjf_rqK1h62oSL)h}+>WsNcYB3Lb zh(crT1IqH2fE!AbZH$2Zkv!r<1S)YFJ!SUT0xoV?!RLIs>QseC2=fN}EYQ`;$R$x0jZ9!u+{j+%PUrcJ zNQA{CJ{!vQ!tNncvU4Kwnw2jOZN8L9dz!8;b&{e#Ew>ohro*f#DB=O?c;^u@yQmFk2mok>#8Wq!-^F_yFrXgw=IcYF^0H z;3!Y|Y>Jk!-nea0;^LHCa@xk`Y_TU$sCMd(E%!IHMaDgZ%VtRet%3AoT7|~ zpdIm{GLrA&BMLjIyJeLo(IDK%;p0^`;vU76E zD=Uj@JRi^tI~LZ|qym(S@uej>OH1}(*=ym!FyJW|iX$E$p8(oHWdK%7r$m__KxM`B z#+hFnuL65C;;U0i0nQvFi$6TQQ*_bEn>2AF$9)C)EjY6Cmv<3R4w|!tUs3fJIF=F# zl1W6!TWN&VlT$fs!G`hJuX8PoZ`tAVNx(#w4d7^6Z``GrKm1`Wcz;3&AjC?LNI39U zi*K`mYGvQOTaP9~Ty zdHie-8+#2O@WNma;)GVdYR#Ql_RZsr&8T)>6gM(5y80lKtQuBP!41SfeE~xske4(E zlG`3z8k*v}90?MKqP+=>5~^#MR1Wl^kccl|IftUSqSbmPn<2!%z!zgLO+8ffS83q- ziP8Yur5626+@hA@{ZzXGS5GD}vl=+mY>1=Z37T7OW^EEQ(QCav|0^w=5fg+#M@Q!i zm~ee>qMuysbdmbRjvWzjS!b{SB$a1zbVAvbnxgrqTf>{xT@UyEZWY}+=+nFr4(Ib9 z+78vB%m{KUVEps_wkxj(xMed*`}F_~bu; zn*aHJ3o@XHlR6%gvPf|zRh&Vv|9EHPVo`#@;=z7{x8>}^KA3xS!1ExspvXmPA+`lud@W+0AvbKnxV-tu>;4>{Cw zBD!F-;Cr?E(Xhsj+&2H++_E)mp-TqOd;I9A zy9B2}jS1DHt)^lzbM%?y6(o|HeDLsUtb~8`C0(;@ux@5L2U*%AE-#%t4f$yf$uoD| z_aJ$Nio>(G%(9&*qo&&%arU5qjXa-p_bw8VW59GqGO)gkw0rlt

`U#R1=n0`c}Im`Jg^ffTC9P3 z+RGQl`r3cf#MG`O?MSWN@f2rBD_matLv6N`&1)l7e3FJ#!EBj4{hq zV8Ms%dTsAzjG~vm80Z@n!FOM26dWi0GGuh|uK3?`%1xrplSWO--PJHr1aJwdRRq3Rsm}iXS zJmrnoh9(VbHJa~pu?9F5ID*&BogJa1HtD!3ns`;YG0|eXAd$PDz4!SGPffz&j4p&!99<6 zn*u%83EIKBhKBulK8rtQ!p9x{5FtK7t{SZT;lb97%&%`oB=7Xg?Ch zKiy$wEo6VI!;Cy`igU)qAkzE5(c}|oSIdsk^mAfFvTK0)+4LfQqFLN^BngK?xysw*qv=M#1co1<}oHsXvG_G_)`Aopd2k>&mX%*flh?-hA=V-f4PqFoSVm;{r#n2Y2$c<~Y zzYjIu0^=m1rl+@F2s_zzN!8`~u$_mQhc`5!Rm9TM9`{_~H`aIy_{FqW_19Kx6U$Li zpmPO@Ea91do!G~DJeQ}aBU@qYy0No?>9Dd!(5|Tqu$d6BoLowBTL2;k5fUPq{WmTt zj!)`g*|SiNY2|C8T7+x=!;cm1vk&vm_yCwX# zUeV|5^ZE?oupIMM%G*Ou7$w(@yvT_L1l!!W4SUN|^I0T|Z9YF@lW*OYJ zG4!HmPOEd<&*a%KWuh#^HgJ+NHGCO5QwF@a*Ip`h+1lj_gHx>Pf*F!*wlr-E^OV)2 ztqq5xcY@79*eD0>lMK?@lRsC&z>G)b-RS(zA=5LX16;VVO_O!KFyU-w6Rg z8Z+0op^)xM@Tg5Sv-z5_{EJquqEEKRxZvpAVF%SK5s9!5t^a(O)IgLg!<*>y3=5*j z;cTKnnUu3(;Q$T$I5X|OTe-^7KwJqr9663la#jK>oDsP7~OtEkt-pV zqMvwhL;uvo>(DMzPAMQJ!{f0v`Hx2YYv~M4Z+K3bvGF@=OKe2V=sB`HNcI~N0qpoD zSG;#iuNO_KxK^Ub18g=qNiN*P*n=2H0dN;U=!1EAoe5WQu}Et!ly7VNy>7BuTCDOK zu3P_ym3-_|F__4r`scwYyFbqubhPe`+}VKGQSQQLuuLx)HHsbqnhuDgHW?uIHHzWI zOp`0b6-mmya9PN$(Kyo@E_jQlGq{%ZW2&?fw=c0@j`Cb&Vd7H0G02Mq^YHThGr2(v z&unJKU~Q~&GtZb|Ts@Ct5cZv3BZ}zJ)a;V3%f4JoWzkLptEb-3w_?Y0lL$?9})ocDpfcbVkwwXznZiP z;#Ov{c~0_Mo=;MUux_wNVUYXXTLVrxj}d+A+S3b;qYM#`>qu(|PFI}z*%}}uha(gJ zqM~UT^fJvqibV%ASBPR&GlmqgY_UAuZK=~X))QPwOKuesjs?^kGLlGwUdN*8a2A;< zD17Eu$n(AWyYB*T=MP+0gF9s@5|rLKI|<$0cs@J$24bH`QFW)alS&Uwe6F#Y+n;yP zc0MEtvKEz~{+^ z`_G@A&5a3Vepk3xE+L~?!{5Iag=Pp(p1LVL2uPF*h#{>)gd*|(sdw_@N`niMp>U|m zx9#a?Kz9H%X_wv0`CXZf9CfC|i1?GVR;$F)wcY#W0iaT#F5Scm41T!Dy9opFK)xsu z>Vu9MHyZDBs+UCEMheX)(?f9HUp2}aC-C=H7bXO-!cvp@*90Bka09-Ivt$rgIV4sY z0App6d-2Ic+}NKle`r()EMbNdkTHBv@L~ca*niSACKOLq7yM+gkdtBk_Cz?{pw0tm zR6QKhvi38j=y%4JAa&Uebx}ekg(oS(Vt>V(OMia5xp-ppHiRb2^pVo8v_x;4N=qQ_ zg-M7@;t0#c(c-g1J{64GP{bbg5uC9bXb}?6d zXzL+z3CpmG1)7kuvR}=5^WCM6GASN#F7;DE@Pta^NVxOtgv^(h9CEfRbzH1u@j7EP zYl9IFg(2aFhV(3EMx}mUL=*2hn*R8D7yN#`a6pkDt8ub_^|;se>Oj|af6kQ?IF+t$ zLP}a0X9kDqN;l13P1jj~7nzgN6qzXRdUC-)G|`OuXU`csFpq9RSzVdb3kA=RZE*Vk zJf`(jv~YAH`CiK!M3!_Q2+BKTmzj%ygI^b=63~0frok0$6oZKBnNDXha54`;bRJBr zoq;yW{DAl5Q$Vx;mH@$QQzb5zD+SYGuXN{!%GbZ2*H`uT)6MtWsfLjFUK2CG!EzJ zlYQd}K`70pcJo~K!HEXp-prl8?rC_S>~?FrdN0PrqE!!i-`CzQa;fdQA+nRjoD5}B z3Du+7c?IX-x;z{iVsjOOq#}65s;tjlf z>NV|T30v6DcMhpDs=JulV!N>^I0CdsRC}Cc+Gc`14}b;Qeq6g3^4=muM-4?b~%${Cf64=N(}@;jW22*|5Ty= zl3E+VU$z)C=(JN0A%Rgoh0P|&Oqo#1ajSKw0u7DKnu3!{=|_T*A6?t|nEPnTQVzY) zEg2#wYEBqZ(v9wWbR`1tjDU*@++tcA9;gGY1GEYt2Jd%BzXHc-F$kSi7ywrL3B>$9yw9Xf5@xp7o3?W`-QNxb1vor;*xpf85ZC&oWTNHf{HG2N zk3DPoj|h9C2hy(R8e0}-0jf5t~TCNJU#T~~!sxn8Rc1Wqdm>|uiT4)eNE zv6dwL7cY?SFNsR6iwNV*jPdtgR9g2u8LVdGho`{Z=jpM+GM-V_E9f53Xhv%Yc8^Y4MZT01%T42 zAQ-YDo|0gZ2X6vGd#ZR4r9eaU zrF$wT&;~tUk>yI&?be7*aHZnH5f7Qk56C|m7q}+%ImKDL{|8Kfe@e>2D@Bsp34skU z!3VPjVJ9cUf)Sbq z+|bg!{rz7(5?R^WdZ0I#yE66_xRfU2%+D*t83~jCZ%BmssOiiQR7*04&*Yv=Z%*bh zOCIU5&(&9?ty)H)()E+6NED%86UnAp4!Dc7mi#i(3f`~YpMZAzfwxBoDY&$-8iR`y zVND)Ale5FY5KkVXd~m3V*tk^5FaA;9^WU(wMw-M*&zElvK;2%%#P-vs$ELzR*;7O-Q7%!&^b#l)?_@_1_q{s)sfRN!er14j zKN7PFREf-k?;aII4mv%ejP5Reh4klUNh@Zj>OgV;NO9e^}Irs11?Hk%@bn2p!Vqj5P5FyPLJojrD5H<9}XrCPNDEva9h-597IC6 zT_C#0f^ab`WcBHo9kIzwv^UPXt<@(4_zM3$xwr_;f?oUWyo6DRgoY>$EQ+K$wO9%S ztgQPE%)5|N&3EUz_g@Eyz-s*JaenhXOVkNVSDnY6JVBYFky;rRYxc>X`*@skMBBjh ze>a0giaqba^R~>=Ne(J9LCZ`;)^Wb9s5+00#f~^5$rm2qKU{3dQTJ7*5R6SKmPFa= zU(AR}5LTQVRvr|mh09as+O)_Mc%&zNw_^FfLxVG7FP9|>bk>zEjcf|cJI6BBL>(gu z9f(YQ|Jc*f)BUfsHnuk1|CK#jLHp~dlSeD@e>p6dwas)Ls&Q>VO}?By9@l$Nn*|0U zZYzAMVkbN7Sszf29NvzypI$l!adHN04e0 zazz^=TDV6Qa5~38tEyChuUW(QaP^73gOkVo-G8e6>ebIn8BykpnJFRa9oom0sm1J| z&Ke$+v&aUFs?w7$zDyveoimgx7Tc2 zJ-CdDA4;)9KsFzpjrm^{QV4t(xa=^Hjm-e-OWC4S-Rt&u9 +#include +// clang-format on + +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include + +#include "../lidar_odometry_step_1/lidar_odometry.h" +#include "../lidar_odometry_step_1/lidar_odometry_utils.h" +#include +#include +#include + +#include + +#include "tbb/tbb.h" +#include + +#include + +#ifdef _WIN32 +#include "resource.h" +#include // <-- Required for ShellExecuteA +#include +#endif + +/////////////////////////////////////////////////////////////////////////////////// + +std::string winTitle = std::string("Raw data viewer ") + HDMAPPING_CONFIGURE_PROJECT_VERSION; + +std::vector infoLines = { "This program is optional step in MANDEYE process", + "", + "It analyzes LiDAR data created by Mission Recorder", + "Next step will be to load data with 'lidar_odometry_step_1' app" }; + +// App specific shortcuts (using empty dummy until needed) +std::vector appShortcuts(80, { "", "", "" }); + +#define SAMPLE_PERIOD (1.0 / 200.0) +namespace fs = std::filesystem; + +ImVec4 pc_color = ImVec4(1.0f, 0.0f, 0.0f, 1.00f); +ImVec4 pc_color2 = ImVec4(0.0f, 0.0f, 1.0f, 1.00f); + +struct AllData +{ + std::vector> timestamps; + std::vector poses; + std::vector points_local; + std::vector lidar_ids; +}; +std::vector all_data; + +struct ImuData +{ + std::vector timestampLidar; + std::vector timestampRel; // timestampLidar shifted so the dataset starts at 0 [s] + double datasetStart = 0.0; // absolute timestamp of the first IMU sample [s] + std::vector angX; + std::vector angY; + std::vector angZ; + std::vector accX; + std::vector accY; + std::vector accZ; + std::vector yaw; + std::vector pitch; + std::vector roll; +}; +ImuData imu_data_plot; + +std::vector laz_files; +std::vector photos_files; +std::map photo_files_ts; + +double filter_threshold_xy_inner = 0.0; // no filtering for raw viewing +double filter_threshold_xy_outer = 300.0; // no filtering for raw viewing +bool fusionConventionNwu = true; +bool fusionConventionEnu = false; +bool fusionConventionNed = false; +int number_of_points_threshold = 20000; +bool is_init = false; +int index_rendered_points_local = -1; +// std::vector> all_points_local; +// std::vector> all_points_local; +// std::vector> all_lidar_ids; +std::vector indexes_to_filename; + +// cached inputs to the chunking pass below, kept around so number_of_points_threshold +// can be changed and re-applied (rebuildAllData) without reloading files from disk +std::vector> g_points_per_file; +std::vector> g_timestamps_cache; +std::vector g_poses_cache; +double vqf_tauAcc = 0.5; +double wx = 1000000.0; +double wy = 1000000.0; +double wz = 1000000.0; +double wom = 1000000.0; +double wfi = 1000000.0; +double wka = 1000000.0; +// bool is_slerp = false; + +double distance_bucket = 0.5; +double polar_angle_deg = 5; +double azimutal_angle_deg = 5; +double max_distance_lidar = 70.0; +int robust_and_accurate_lidar_odometry_iterations = 20; +bool useMultithread = true; + +std::vector> rgd_nn; +std::vector> mean_cov; +bool show_mean_cov = false; +bool show_rgd_nn = false; +bool show_imu_data = false; +bool show_cameras_data = true; +bool is_settings_gui = false; +bool color_by_intensity = false; +float intensity_gamma = 1.0f; + +namespace photos +{ + cv::Mat imgToShow; + std::string imgToShowFn; + GLuint photo_texture_cam0 = 0; + double nearestTs = 0; + int photo_width_cam0 = 0; + int photo_height_cam0 = 0; +} // namespace photos + +/////////////////////////////////////////////////////////////////////////////////// + +static ImVec2 DisplayImageFit(ImTextureID tex, int tex_w, int tex_h, bool allow_upscale = true) +{ + ImVec2 avail = ImGui::GetContentRegionAvail(); + if (tex_w <= 0 || tex_h <= 0 || avail.x <= 0.0f || avail.y <= 0.0f) + return ImVec2(0, 0); + + float sx = avail.x / float(tex_w); + float sy = avail.y / float(tex_h); + float scale = std::min(sx, sy); + if (!allow_upscale) + scale = std::min(scale, 1.0f); + + ImVec2 disp(scale * tex_w, scale * tex_h); + + ImVec2 cur = ImGui::GetCursorPos(); + ImGui::SetCursorPosX(cur.x + (avail.x - disp.x) * 0.5f); // center horizontally + ImGui::Image(tex, disp); + // cursor Y is already advanced by Image; no restore needed + + return disp; +} + +void render_nearest_photo(double ts) +{ + using namespace photos; + // find closest photo + if (photo_files_ts.empty()) + { + return; + } + uint64_t ts_uint64 = static_cast(ts * 1e9); + auto it = photo_files_ts.lower_bound(ts_uint64); + if (it == photo_files_ts.end()) + { + return; + } + const std::string& photo_file = it->second; + if (photo_file == imgToShowFn) + { + return; + } + spdlog::info("render_nearest_photo: {}", photo_file); + cv::Mat img = cv::imread(photo_file); + if (img.empty()) + { + return; + } + cv::Mat img_rgb; + cv::cvtColor(img, img_rgb, cv::COLOR_BGR2RGB); + imgToShow = img_rgb; + imgToShowFn = photo_file; + photos::nearestTs = double(it->first) / 1e9; + // upload to OpenGL texture + photo_width_cam0 = imgToShow.cols; + photo_height_cam0 = imgToShow.rows; + if (photo_texture_cam0 == 0) + { + glGenTextures(1, &photo_texture_cam0); + } + glBindTexture(GL_TEXTURE_2D, photo_texture_cam0); + glPixelStorei(GL_UNPACK_ALIGNMENT, 1); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); + glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, photo_width_cam0, photo_height_cam0, 0, GL_RGB, GL_UNSIGNED_BYTE, imgToShow.data); + glBindTexture(GL_TEXTURE_2D, 0); +} + +void optimize() +{ +#if 0 + rgd_nn.clear(); + + // NDT::GridParameters rgd_params_sc; + + // rgd_params_sc.resolution_X = distance_bucket; + // rgd_params_sc.resolution_Y = polar_angle_deg; + // rgd_params_sc.resolution_Z = azimutal_angle_deg; + + if (index_rendered_points_local >= 0 && index_rendered_points_local < all_data.size()) + { + // std::vector points_local_sf; + std::vector points_local; + auto tr = all_data[index_rendered_points_local].poses; + auto trmm = all_data[index_rendered_points_local].poses; + + for (size_t i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) + { + auto lower = std::lower_bound(all_data[index_rendered_points_local].timestamps.begin(), all_data[index_rendered_points_local].timestamps.end(), all_data[index_rendered_points_local].points_local[i].timestamp, + [](std::pair lhs, double rhs) -> bool + { return lhs.first < rhs; }); + + int index_pose = std::distance(all_data[index_rendered_points_local].timestamps.begin(), lower) - 1; + + if (index_pose >= 0 && index_pose < all_data[index_rendered_points_local].poses.size()) + { + // Eigen::Affine3d m = all_data[index_rendered_points_local].poses[index_pose]; + // Eigen::Vector3d p = m * all_data[index_rendered_points_local].points_local[i].point; + double r_l = all_data[index_rendered_points_local].points_local[i].point.norm(); + if (r_l > 0.5 && all_data[index_rendered_points_local].points_local[i].index_pose != -1 && r_l < max_distance_lidar) + { + // double polar_angle_deg_l = atan2(all_data[index_rendered_points_local].points_local[i].point.y(), all_data[index_rendered_points_local].points_local[i].point.x()) / M_PI * 180.0; + // double azimutal_angle_deg_l = acos(all_data[index_rendered_points_local].points_local[i].point.z() / r_l) / M_PI * 180.0; + all_data[index_rendered_points_local].points_local[i].index_pose = index_pose; + points_local.push_back(all_data[index_rendered_points_local].points_local[i]); + + /////////////////////////////////////////////////////// + // Point3Di p_sl = all_data[index_rendered_points_local].points_local[i]; + // p_sl.point.x() = r_l; + // p_sl.point.y() = polar_angle_deg_l; + // p_sl.point.z() = azimutal_angle_deg_l; + + // points_local_sf.push_back(p_sl); + } + } + } + + //spdlog::info("points_local.size(): {}", points_local.size()); + // points_local + // points_local_sf + + std::vector + point_cloud_global; + std::vector point_cloud_global_sc; + + for (size_t i = 0; i < points_local.size(); i++) + { + Point3Di pg = points_local[i]; + pg.point = tr[points_local[i].index_pose] * pg.point; + point_cloud_global.push_back(pg); + double r_g = pg.point.norm(); + point_cloud_global_sc.emplace_back(r_g, atan2(pg.point.y(), pg.point.x()) / M_PI * 180.0, acos(pg.point.z() / r_g) / M_PI * 180.0); + } + + NDT::GridParameters rgd_params_sc; + + rgd_params_sc.resolution_X = distance_bucket; + rgd_params_sc.resolution_Y = polar_angle_deg; + rgd_params_sc.resolution_Z = azimutal_angle_deg; + Eigen::Vector3d b(rgd_params_sc.resolution_X, rgd_params_sc.resolution_Y, rgd_params_sc.resolution_Z); + + NDTBucketMapType buckets; + update_rgd_spherical_coordinates(rgd_params_sc, buckets, point_cloud_global, point_cloud_global_sc, false); + //update_rgd(rgd_params_sc, buckets, point_cloud_global, {0, 0, 0}); + + std::vector> tripletListA; + std::vector> tripletListP; + std::vector> tripletListB; + + for (size_t i = 0; i < point_cloud_global.size(); i++) + { + + auto index_of_bucket = get_rgd_index_3d(point_cloud_global_sc[i], b); + //auto index_of_bucket = get_rgd_index_3d(point_cloud_global[i].point, b); + auto bucket_it = buckets.find(index_of_bucket); + // no bucket found + if (bucket_it == buckets.end()) + { + continue; + } + auto& this_bucket = bucket_it->second; + + rgd_nn.emplace_back(point_cloud_global[i].point, this_bucket.mean); + + const Eigen::Matrix3d& infm = this_bucket.cov.inverse(); + const double threshold = 100000.0; + + if ((infm.array() > threshold).any()) + { + continue; + } + if ((infm.array() < -threshold).any()) + { + continue; + } + + const Eigen::Affine3d& m_pose = tr[points_local[i].index_pose]; + //spdlog::info("points_local[i].index_pose {}", points_local[i].index_pose); + const Eigen::Vector3d& p_s = points_local[i].point; + const TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(m_pose); + double delta_x; + double delta_y; + double delta_z; + + //// + Eigen::Matrix jacobian; + // TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix((*mposes)[p.index_pose]); + + point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, + pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, + p_s.x(), p_s.y(), p_s.z(), this_bucket.mean.x(), this_bucket.mean.y(), this_bucket.mean.z()); + + // spdlog::info("delta_x: {}", delta_x); + // spdlog::info("delta_y: {}", delta_y); + // spdlog::info("delta_z: {}", delta_z); + + if (sqrt(delta_x * delta_x + delta_y * delta_y + delta_z * delta_z) < 0.001) + { + continue; + } + + point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z()); + + int c = points_local[i].index_pose * 6; + + // std::mutex &m = my_mutex[0]; // mutexes[intermediate_points_i.index_pose]; + // std::unique_lock lck(m); + int ir = tripletListB.size(); + + for (int row = 0; row < 3; row++) + { + for (int col = 0; col < 6; col++) + { + if (jacobian(row, col) != 0.0) + { + tripletListA.emplace_back(ir + row, c + col, -jacobian(row, col)); + } + } + } + tripletListB.emplace_back(ir, 0, delta_x); + tripletListB.emplace_back(ir + 1, 0, delta_y); + tripletListB.emplace_back(ir + 2, 0, delta_z); + + tripletListP.emplace_back(ir, ir, infm(0, 0)); + tripletListP.emplace_back(ir, ir + 1, infm(0, 1)); + tripletListP.emplace_back(ir, ir + 2, infm(0, 2)); + tripletListP.emplace_back(ir + 1, ir, infm(1, 0)); + tripletListP.emplace_back(ir + 1, ir + 1, infm(1, 1)); + tripletListP.emplace_back(ir + 1, ir + 2, infm(1, 2)); + tripletListP.emplace_back(ir + 2, ir, infm(2, 0)); + tripletListP.emplace_back(ir + 2, ir + 1, infm(2, 1)); + tripletListP.emplace_back(ir + 2, ir + 2, infm(2, 2)); + + /// + } + + std::vector> odo_edges; + for (size_t i = 1; i < tr.size(); i++) + { + odo_edges.emplace_back(i - 1, i); + } + + std::vector poses; + std::vector poses_desired; + + for (size_t i = 0; i < tr.size(); i++) + { + poses.push_back(pose_tait_bryan_from_affine_matrix(tr[i])); + } + for (size_t i = 0; i < trmm.size(); i++) + { + poses_desired.push_back(pose_tait_bryan_from_affine_matrix(trmm[i])); + } + + for (size_t i = 0; i < odo_edges.size(); i++) + { + Eigen::Matrix relative_pose_measurement_odo; + relative_pose_tait_bryan_wc_case1(relative_pose_measurement_odo, + poses_desired[odo_edges[i].first].px, + poses_desired[odo_edges[i].first].py, + poses_desired[odo_edges[i].first].pz, + poses_desired[odo_edges[i].first].om, + poses_desired[odo_edges[i].first].fi, + poses_desired[odo_edges[i].first].ka, + poses_desired[odo_edges[i].second].px, + poses_desired[odo_edges[i].second].py, + poses_desired[odo_edges[i].second].pz, + poses_desired[odo_edges[i].second].om, + poses_desired[odo_edges[i].second].fi, + poses_desired[odo_edges[i].second].ka); + + Eigen::Matrix delta; + relative_pose_obs_eq_tait_bryan_wc_case1( + delta, + poses[odo_edges[i].first].px, + poses[odo_edges[i].first].py, + poses[odo_edges[i].first].pz, + poses[odo_edges[i].first].om, + poses[odo_edges[i].first].fi, + poses[odo_edges[i].first].ka, + poses[odo_edges[i].second].px, + poses[odo_edges[i].second].py, + poses[odo_edges[i].second].pz, + poses[odo_edges[i].second].om, + poses[odo_edges[i].second].fi, + poses[odo_edges[i].second].ka, + relative_pose_measurement_odo(0, 0), + relative_pose_measurement_odo(1, 0), + relative_pose_measurement_odo(2, 0), + relative_pose_measurement_odo(3, 0), + relative_pose_measurement_odo(4, 0), + relative_pose_measurement_odo(5, 0)); + + Eigen::Matrix jacobian; + relative_pose_obs_eq_tait_bryan_wc_case1_jacobian(jacobian, + poses[odo_edges[i].first].px, + poses[odo_edges[i].first].py, + poses[odo_edges[i].first].pz, + poses[odo_edges[i].first].om, + poses[odo_edges[i].first].fi, + poses[odo_edges[i].first].ka, + poses[odo_edges[i].second].px, + poses[odo_edges[i].second].py, + poses[odo_edges[i].second].pz, + poses[odo_edges[i].second].om, + poses[odo_edges[i].second].fi, + poses[odo_edges[i].second].ka); + + int ir = tripletListB.size(); + + int ic_1 = odo_edges[i].first * 6; + int ic_2 = odo_edges[i].second * 6; + + for (size_t row = 0; row < 6; row++) + { + tripletListA.emplace_back(ir + row, ic_1, -jacobian(row, 0)); + tripletListA.emplace_back(ir + row, ic_1 + 1, -jacobian(row, 1)); + tripletListA.emplace_back(ir + row, ic_1 + 2, -jacobian(row, 2)); + tripletListA.emplace_back(ir + row, ic_1 + 3, -jacobian(row, 3)); + tripletListA.emplace_back(ir + row, ic_1 + 4, -jacobian(row, 4)); + tripletListA.emplace_back(ir + row, ic_1 + 5, -jacobian(row, 5)); + + tripletListA.emplace_back(ir + row, ic_2, -jacobian(row, 6)); + tripletListA.emplace_back(ir + row, ic_2 + 1, -jacobian(row, 7)); + tripletListA.emplace_back(ir + row, ic_2 + 2, -jacobian(row, 8)); + tripletListA.emplace_back(ir + row, ic_2 + 3, -jacobian(row, 9)); + tripletListA.emplace_back(ir + row, ic_2 + 4, -jacobian(row, 10)); + tripletListA.emplace_back(ir + row, ic_2 + 5, -jacobian(row, 11)); + } + + tripletListB.emplace_back(ir, 0, delta(0, 0)); + tripletListB.emplace_back(ir + 1, 0, delta(1, 0)); + tripletListB.emplace_back(ir + 2, 0, delta(2, 0)); + tripletListB.emplace_back(ir + 3, 0, delta(3, 0)); + tripletListB.emplace_back(ir + 4, 0, delta(4, 0)); + tripletListB.emplace_back(ir + 5, 0, delta(5, 0)); + + tripletListP.emplace_back(ir, ir, wx); + tripletListP.emplace_back(ir + 1, ir + 1, wy); + tripletListP.emplace_back(ir + 2, ir + 2, wz); + tripletListP.emplace_back(ir + 3, ir + 3, wom); + tripletListP.emplace_back(ir + 4, ir + 4, wfi); + tripletListP.emplace_back(ir + 5, ir + 5, wka); + } + + int ic = 0; + int ir = tripletListB.size(); + tripletListA.emplace_back(ir, ic * 6 + 0, 1); + tripletListA.emplace_back(ir + 1, ic * 6 + 1, 1); + tripletListA.emplace_back(ir + 2, ic * 6 + 2, 1); + tripletListA.emplace_back(ir + 3, ic * 6 + 3, 1); + tripletListA.emplace_back(ir + 4, ic * 6 + 4, 1); + tripletListA.emplace_back(ir + 5, ic * 6 + 5, 1); + + /*tripletListP.emplace_back(ir, ir, 1000000); + tripletListP.emplace_back(ir + 1, ir + 1, 1000000); + tripletListP.emplace_back(ir + 2, ir + 2, 1000000); + tripletListP.emplace_back(ir + 3, ir + 3, 1000000); + tripletListP.emplace_back(ir + 4, ir + 4, 1000000); + tripletListP.emplace_back(ir + 5, ir + 5, 1000000);*/ + tripletListP.emplace_back(ir, ir, 1); + tripletListP.emplace_back(ir + 1, ir + 1, 1); + tripletListP.emplace_back(ir + 2, ir + 2, 1); + tripletListP.emplace_back(ir + 3, ir + 3, 1); + tripletListP.emplace_back(ir + 4, ir + 4, 1); + tripletListP.emplace_back(ir + 5, ir + 5, 1); + + tripletListB.emplace_back(ir, 0, 0); + tripletListB.emplace_back(ir + 1, 0, 0); + tripletListB.emplace_back(ir + 2, 0, 0); + tripletListB.emplace_back(ir + 3, 0, 0); + tripletListB.emplace_back(ir + 4, 0, 0); + tripletListB.emplace_back(ir + 5, 0, 0); + + Eigen::SparseMatrix matA(tripletListB.size(), tr.size() * 6); + Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); + Eigen::SparseMatrix matB(tripletListB.size(), 1); + + matA.setFromTriplets(tripletListA.begin(), tripletListA.end()); + matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); + matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); + + Eigen::SparseMatrix AtPA(tr.size() * 6, tr.size() * 6); + Eigen::SparseMatrix AtPB(tr.size() * 6, 1); + + { + Eigen::SparseMatrix AtP = matA.transpose() * matP; + AtPA = (AtP)*matA; + AtPB = (AtP)*matB; + } + + tripletListA.clear(); + tripletListP.clear(); + tripletListB.clear(); + + // AtPA += AtPAndt.sparseView(); + // AtPB += AtPBndt.sparseView(); + + // Eigen::SparseMatrix AtPA_I(tr.size() * 6, tr.size() * 6); + // AtPA_I.setIdentity(); + // AtPA += AtPA_I; + + Eigen::SimplicialCholesky> + solver(AtPA); + Eigen::SparseMatrix x = solver.solve(AtPB); + std::vector h_x; + for (int k = 0; k < x.outerSize(); ++k) + { + for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) + { + h_x.push_back(it.value()); + + std::cout << it.value() << " "; + } + } + + if (h_x.size() == 6 * tr.size()) + { + int counter = 0; + + for (size_t i = 0; i < tr.size(); i++) + { + TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(tr[i]); + auto prev_pose = pose; + pose.px += h_x[counter++]; + pose.py += h_x[counter++]; + pose.pz += h_x[counter++]; + pose.om += h_x[counter++]; + pose.fi += h_x[counter++]; + pose.ka += h_x[counter++]; + + // Eigen::Vector3d p1(prev_pose.px, prev_pose.py, prev_pose.pz); + // Eigen::Vector3d p2(pose.px, pose.py, pose.pz); + + // if ((p1 - p2).norm() < 1.0) + //{ + tr[i] = affine_matrix_from_pose_tait_bryan(pose); + //} + } + all_data[index_rendered_points_local].poses = tr; + } + } +#endif + + NDT::GridParameters rgd_params_sc; + + rgd_params_sc.resolution_X = distance_bucket; + rgd_params_sc.resolution_Y = polar_angle_deg; + rgd_params_sc.resolution_Z = azimutal_angle_deg; + + if (index_rendered_points_local >= 0 && index_rendered_points_local < all_data.size()) + { + std::vector tr = all_data[index_rendered_points_local].poses; // = worker_data[i].intermediate_trajectory; + std::vector trmm = + all_data[index_rendered_points_local].poses; // = worker_data[i].intermediate_trajectory_motion_model; + + std::vector points_local_sf; + std::vector points_local; + + for (size_t i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) + { + auto lower = std::lower_bound( + all_data[index_rendered_points_local].timestamps.begin(), + all_data[index_rendered_points_local].timestamps.end(), + all_data[index_rendered_points_local].points_local[i].timestamp, + [](std::pair lhs, double rhs) -> bool + { + return lhs.first < rhs; + }); + + int index_pose = std::distance(all_data[index_rendered_points_local].timestamps.begin(), lower) - 1; + + if (index_pose >= 0 && index_pose < all_data[index_rendered_points_local].poses.size()) + { + all_data[index_rendered_points_local].points_local[i].index_pose = index_pose; + // Eigen::Affine3d m = all_data[index_rendered_points_local].poses[index_pose]; + // Eigen::Vector3d p = m * all_data[index_rendered_points_local].points_local[i].point; + double r_l = all_data[index_rendered_points_local].points_local[i].point.norm(); + if (r_l > 0.5 && all_data[index_rendered_points_local].points_local[i].index_pose != -1 && r_l < max_distance_lidar) + { + double polar_angle_deg_l = atan2( + all_data[index_rendered_points_local].points_local[i].point.y(), + all_data[index_rendered_points_local].points_local[i].point.x()) / + M_PI * 180.0; + double azimutal_angle_deg_l = + acos(all_data[index_rendered_points_local].points_local[i].point.z() / r_l) / M_PI * 180.0; + + points_local.push_back(all_data[index_rendered_points_local].points_local[i]); + + /////////////////////////////////////////////////////// + Point3Di p_sl = all_data[index_rendered_points_local].points_local[i]; + p_sl.point.x() = r_l; + p_sl.point.y() = polar_angle_deg_l; + p_sl.point.z() = azimutal_angle_deg_l; + + points_local_sf.push_back(p_sl); + } + } + } + + spdlog::info("optimize_sf2"); + for (int iter = 0; iter < robust_and_accurate_lidar_odometry_iterations; iter++) + { + optimize_sf2(points_local, points_local_sf, tr, trmm, rgd_params_sc, useMultithread, wx, wy, wz, wom, wfi, wka); + trmm = tr; + } + + all_data[index_rendered_points_local].poses = tr; + } + + return; +} + +std::vector> get_nn() +{ + std::vector> nn; + if (index_rendered_points_local >= 0 && index_rendered_points_local < all_data.size()) + { + // for (size_t i = 0; i < all_data[index_rendered_points_local].poses.size(); i++){ + // all_data[index_rendered_points_local].poses[i](0, 3) += 1.0; + // } + + auto worker_data = all_data[index_rendered_points_local]; + + // index data + for (size_t i = 0; i < worker_data.points_local.size(); i++) + { + auto lower = std::lower_bound( + worker_data.timestamps.begin(), + worker_data.timestamps.end(), + worker_data.points_local[i].timestamp, + [](std::pair lhs, double rhs) -> bool + { + return lhs.first < rhs; + }); + + int index_pose = std::distance(worker_data.timestamps.begin(), lower) - 1; + + if (index_pose >= 0 && index_pose < worker_data.poses.size()) + { + worker_data.points_local[i].index_pose = index_pose; + } + else + { + worker_data.points_local[i].index_pose = -1; + } + } + + NDT::GridParameters rgd_params; + // rgd_params.resolution_X = 0.3; // distance bucket + // rgd_params.resolution_Y = 0.3; // polar angle deg + // rgd_params.resolution_Z = 0.3; // azimutal angle deg + + rgd_params.resolution_X = distance_bucket; // distance bucket + rgd_params.resolution_Y = polar_angle_deg; // polar angle deg + rgd_params.resolution_Z = azimutal_angle_deg; // azimutal angle deg + + std::vector point_cloud_global; + std::vector points_local; + + std::vector point_cloud_global_sc; + std::vector points_local_sc; + + for (size_t i = 0; i < worker_data.points_local.size(); i++) + { + double r_l = worker_data.points_local[i].point.norm(); + if (r_l > 0.5 && worker_data.points_local[i].index_pose != -1 && r_l < max_distance_lidar) + { + double polar_angle_deg_l = + atan2(worker_data.points_local[i].point.y(), worker_data.points_local[i].point.x()) / M_PI * 180.0; + double azimutal_angle_deg_l = acos(worker_data.points_local[i].point.z() / r_l) / M_PI * 180.0; + + Eigen::Vector3d pp = worker_data.points_local[i].point; + // pps.x() = r; + // pps.y() = polar_angle_deg; + // pps.z() = azimutal_angle_deg; + // point_cloud_spherical_coordinates.push_back(pps); + + Eigen::Affine3d pose = worker_data.poses[worker_data.points_local[i].index_pose]; + + pp = pose * pp; + + Point3Di pg = worker_data.points_local[i]; + pg.point = pp; + + point_cloud_global.push_back(pg); + points_local.push_back(worker_data.points_local[i]); + + /////////////////////////////////////////////////////// + Point3Di p_sl = worker_data.points_local[i]; + p_sl.point.x() = r_l; + p_sl.point.y() = polar_angle_deg_l; + p_sl.point.z() = azimutal_angle_deg_l; + + points_local_sc.push_back(p_sl); + // + double r_g = pg.point.norm(); + double polar_angle_deg_g = atan2(pg.point.y(), pg.point.x()) / M_PI * 180.0; + double azimutal_angle_deg_g = acos(pg.point.z() / r_l) / M_PI * 180.0; + + Eigen::Vector3d p_sg = worker_data.points_local[i].point; + p_sg.x() = r_g; + p_sg.y() = polar_angle_deg_g; + p_sg.z() = azimutal_angle_deg_g; + + point_cloud_global_sc.push_back(p_sg); + } + } + + NDTBucketMapType buckets; + update_rgd_spherical_coordinates(rgd_params, buckets, point_cloud_global, point_cloud_global_sc); + + ///////////// + // std::vector> nn; + Eigen::Vector3d b(rgd_params.resolution_X, rgd_params.resolution_Y, rgd_params.resolution_Z); + + for (size_t i = 0; i < point_cloud_global_sc.size(); i++) + { + auto index_of_bucket = get_rgd_index_3d(point_cloud_global_sc[i], b); + + auto bucket_it = buckets.find(index_of_bucket); + + if (bucket_it != buckets.end()) + { + auto& this_bucket = bucket_it->second; + this_bucket.number_of_points++; + // const auto &curr_mean = points_global[i].point; + const auto& mean = this_bucket.mean; + + nn.emplace_back(point_cloud_global[i].point, mean); + + ////////////// + } + } + } + return nn; +} + +std::vector> get_mean_cov() +{ + std::vector> mc; + if (index_rendered_points_local >= 0 && index_rendered_points_local < all_data.size()) + { + // for (size_t i = 0; i < all_data[index_rendered_points_local].poses.size(); i++){ + // all_data[index_rendered_points_local].poses[i](0, 3) += 1.0; + // } + + auto worker_data = all_data[index_rendered_points_local]; + + // index data + for (size_t i = 0; i < worker_data.points_local.size(); i++) + { + auto lower = std::lower_bound( + worker_data.timestamps.begin(), + worker_data.timestamps.end(), + worker_data.points_local[i].timestamp, + [](std::pair lhs, double rhs) -> bool + { + return lhs.first < rhs; + }); + + int index_pose = std::distance(worker_data.timestamps.begin(), lower) - 1; + + if (index_pose >= 0 && index_pose < worker_data.poses.size()) + worker_data.points_local[i].index_pose = index_pose; + else + worker_data.points_local[i].index_pose = -1; + } + + NDT::GridParameters rgd_params; + // rgd_params.resolution_X = 0.3; // distance bucket + // rgd_params.resolution_Y = 0.3; // polar angle deg + // rgd_params.resolution_Z = 0.3; // azimutal angle deg + + rgd_params.resolution_X = distance_bucket; // distance bucket + rgd_params.resolution_Y = polar_angle_deg; // polar angle deg + rgd_params.resolution_Z = azimutal_angle_deg; // azimutal angle deg + + std::vector point_cloud_global; + std::vector points_local; + + std::vector point_cloud_global_sc; + std::vector points_local_sc; + + for (size_t i = 0; i < worker_data.points_local.size(); i++) + { + double r_l = worker_data.points_local[i].point.norm(); + if (r_l > 0.5 && worker_data.points_local[i].index_pose != -1 && r_l < max_distance_lidar) + { + double polar_angle_deg_l = + atan2(worker_data.points_local[i].point.y(), worker_data.points_local[i].point.x()) / M_PI * 180.0; + double azimutal_angle_deg_l = acos(worker_data.points_local[i].point.z() / r_l) / M_PI * 180.0; + + Eigen::Vector3d pp = worker_data.points_local[i].point; + // pps.x() = r; + // pps.y() = polar_angle_deg; + // pps.z() = azimutal_angle_deg; + // point_cloud_spherical_coordinates.push_back(pps); + + Eigen::Affine3d pose = worker_data.poses[worker_data.points_local[i].index_pose]; + + pp = pose * pp; + + Point3Di pg = worker_data.points_local[i]; + pg.point = pp; + + point_cloud_global.push_back(pg); + points_local.push_back(worker_data.points_local[i]); + + /////////////////////////////////////////////////////// + Point3Di p_sl = worker_data.points_local[i]; + p_sl.point.x() = r_l; + p_sl.point.y() = polar_angle_deg_l; + p_sl.point.z() = azimutal_angle_deg_l; + + points_local_sc.push_back(p_sl); + // + double r_g = pg.point.norm(); + double polar_angle_deg_g = atan2(pg.point.y(), pg.point.x()) / M_PI * 180.0; + double azimutal_angle_deg_g = acos(pg.point.z() / r_l) / M_PI * 180.0; + + Eigen::Vector3d p_sg = worker_data.points_local[i].point; + p_sg.x() = r_g; + p_sg.y() = polar_angle_deg_g; + p_sg.z() = azimutal_angle_deg_g; + + point_cloud_global_sc.push_back(p_sg); + } + } + + NDTBucketMapType buckets; + update_rgd_spherical_coordinates(rgd_params, buckets, point_cloud_global, point_cloud_global_sc); + + ///////////// + Eigen::Vector3d b(rgd_params.resolution_X, rgd_params.resolution_Y, rgd_params.resolution_Z); + + for (const auto& b : buckets) + { + auto& this_bucket = b.second; + + mc.emplace_back(this_bucket.mean, this_bucket.cov); + } + } + + return mc; +} + +// Re-chunks the already-loaded points (g_points_per_file / g_timestamps_cache / g_poses_cache) +// into all_data using the current number_of_points_threshold. Smaller thresholds mean each +// Index step in the toolbar covers less time, giving finer time navigation. +void rebuildAllData(bool preserve_position) +{ + double preserved_time = 0.0; + bool have_preserved_time = false; + if (preserve_position && index_rendered_points_local >= 0 && index_rendered_points_local < static_cast(all_data.size()) && + !all_data[index_rendered_points_local].timestamps.empty()) + { + preserved_time = all_data[index_rendered_points_local].timestamps.front().first; + have_preserved_time = true; + } + + all_data.clear(); + all_data.shrink_to_fit(); + indexes_to_filename.clear(); + indexes_to_filename.shrink_to_fit(); + + if (number_of_points_threshold < 1) + number_of_points_threshold = 1; + + const auto& pointsPerFile = g_points_per_file; + const auto& timestamps = g_timestamps_cache; + const auto& poses = g_poses_cache; + + int number_of_points = 0; + for (const auto& pp : pointsPerFile) + number_of_points += static_cast(pp.size()); + + spdlog::info("Number of points: {}", number_of_points); + spdlog::info("Start indexing points"); + + std::vector points_local; + std::vector lidar_ids; + + for (size_t i = 0; i < pointsPerFile.size(); i++) + { + std::cout << "Indexed file " << i + 1 << "/" << pointsPerFile.size() << "\r"; + + for (const auto& pp : pointsPerFile[i]) + { + auto lower = std::lower_bound( + timestamps.begin(), + timestamps.end(), + pp.timestamp, + [](std::pair lhs, double rhs) -> bool + { + return lhs.first < rhs; + }); + + int index_pose = static_cast(std::distance(timestamps.begin(), lower)) - 1; + + if (index_pose >= 0 && index_pose < static_cast(poses.size())) + { + points_local.push_back(pp); + lidar_ids.push_back(pp.lidarid); + } + + if (points_local.size() > static_cast(number_of_points_threshold)) + { + indexes_to_filename.push_back(static_cast(i)); + + AllData data; + data.points_local = points_local; + data.lidar_ids = lidar_ids; + + for (size_t k = 0; k < timestamps.size(); k++) + { + if (timestamps[k].first >= points_local[0].timestamp && + timestamps[k].first <= points_local[points_local.size() - 1].timestamp) + { + data.timestamps.push_back(timestamps[k]); + data.poses.push_back(poses[k]); + } + } + + // correct points timestamps + if (data.timestamps.size() > 2) + { + double ts_begin = data.timestamps[0].first; + double ts_step = + (data.timestamps[data.timestamps.size() - 1].first - data.timestamps[0].first) / data.points_local.size(); + + for (size_t pp_i = 0; pp_i < data.points_local.size(); pp_i++) + data.points_local[pp_i].timestamp = ts_begin + pp_i * ts_step; + } + + all_data.push_back(data); + + points_local.clear(); + lidar_ids.clear(); + } + } + } + + spdlog::info("Indexing points finished\n"); + + if (all_data.size() > 0) + { + is_init = true; + index_rendered_points_local = 0; + + if (have_preserved_time) + { + for (size_t i = 0; i < all_data.size(); i++) + { + if (!all_data[i].timestamps.empty() && preserved_time >= all_data[i].timestamps.front().first && + preserved_time <= all_data[i].timestamps.back().first) + { + index_rendered_points_local = static_cast(i); + break; + } + } + } + } + else + { + is_init = false; + index_rendered_points_local = -1; + } +} + +void loadFiles(std::vector input_file_names) +{ + LidarOdometryParams params; // dummy for load_data function + params.save_calibration_validation = false; + params.lazy_load_raw_clouds = false; // the viewer shows every cloud + params.filter_threshold_xy_inner = filter_threshold_xy_inner; + params.filter_threshold_xy_outer = filter_threshold_xy_outer; + + std::vector> pointsPerFile; + Imu imu_data; + + // no files selected, quit loading + if (input_file_names.empty()) + return; + + if (load_data(input_file_names, params, pointsPerFile, imu_data, false)) + { + // clear possible previous data + + all_data.clear(); + all_data.shrink_to_fit(); + + imu_data_plot.timestampLidar.clear(); + imu_data_plot.timestampRel.clear(); + imu_data_plot.datasetStart = 0.0; + imu_data_plot.angX.clear(); + imu_data_plot.angY.clear(); + imu_data_plot.angZ.clear(); + imu_data_plot.accX.clear(); + imu_data_plot.accY.clear(); + imu_data_plot.accZ.clear(); + imu_data_plot.yaw.clear(); + imu_data_plot.pitch.clear(); + imu_data_plot.roll.clear(); + + imu_data_plot.timestampLidar.shrink_to_fit(); + imu_data_plot.timestampRel.shrink_to_fit(); + imu_data_plot.angX.shrink_to_fit(); + imu_data_plot.angY.shrink_to_fit(); + imu_data_plot.angZ.shrink_to_fit(); + imu_data_plot.accX.shrink_to_fit(); + imu_data_plot.accY.shrink_to_fit(); + imu_data_plot.accZ.shrink_to_fit(); + imu_data_plot.yaw.shrink_to_fit(); + imu_data_plot.pitch.shrink_to_fit(); + imu_data_plot.roll.shrink_to_fit(); + + photo_files_ts.clear(); + + laz_files.clear(); + + // specific processing for RAW data viewer + + std::sort(input_file_names.begin(), input_file_names.end()); + for (const auto& fileName : input_file_names) + { + std::string ext = fs::path(fileName).extension().string(); + std::transform(ext.begin(), ext.end(), ext.begin(), ::tolower); + + if (ext == ".laz" || ext == ".las") + laz_files.push_back(fileName); + else if (ext == ".jpg" || ext == ".jpeg") + { + photos_files.push_back(fileName); + // decode filename e.g.: ` cam0_1761264773592270949.jpg` + const std::string filename = fs::path(fileName).stem().string(); + std::string cam_id = filename.substr(0, filename.find("_")); + std::string timestamp = filename.substr(filename.find("_") + 1, filename.size()); + + spdlog::info("cam_id: {}", cam_id); + spdlog::info("timestamp: {}", timestamp); + spdlog::info("filename: {}", filename); + + if (cam_id == "cam0" && !timestamp.empty()) + { + try + { + uint64_t ts = std::stoull(timestamp); + photo_files_ts[ts] = fileName; + } catch (const std::exception& e) + { + spdlog::error("Error parsing timestamp from filename: {} - {}", filename, e.what()); + } + } + } + } + + // rest of RAW data viewer processing + + // VQF initialization + double avg_dt = SAMPLE_PERIOD; + if (imu_data.size() >= 2) + { + const double t0 = std::get<0>(imu_data.front()).first; + const double t1 = std::get<0>(imu_data.back()).first; + const double duration = t1 - t0; + + if (t1 > t0) + avg_dt = duration / static_cast(imu_data.size() - 1); + + if (duration > 24 * 60 * 60) + { + spdlog::error("Session is absurdly long : start time : {}, end time {}", t0, t1); + spdlog::error("Setting rate to {}", SAMPLE_PERIOD); + avg_dt = SAMPLE_PERIOD; + } + } + + VQFParams vqf_params; + vqf_params.tauAcc = vqf_tauAcc > 0.0 ? vqf_tauAcc : 3.0; + // produce warning + if (std::fabs(avg_dt - SAMPLE_PERIOD) > 0.01) + { + spdlog::warn("The dt found is strange : {}", avg_dt); + } + spdlog::info("avg_dt: {}", avg_dt); + VQF vqf(vqf_params, avg_dt); + + std::map> trajectory; + + int counter = 1; + + for (const auto& [timestamp_pair, gyr, acc] : imu_data) + { + const double g = 9.81; + vqf_real_t gyr_vqf[3] = { static_cast(gyr.x()), static_cast(gyr.y()), static_cast(gyr.z()) }; + vqf_real_t acc_vqf[3] = { static_cast(acc.x()) * g, + static_cast(acc.y()) * g, + static_cast(acc.z()) * g }; + + vqf.update(gyr_vqf, acc_vqf); + + vqf_real_t quat[4]; + vqf.getQuat6D(quat); + Eigen::Quaterniond d(quat[0], quat[1], quat[2], quat[3]); + Eigen::Affine3d t{ Eigen::Matrix4d::Identity() }; + t.rotate(d); + + trajectory[timestamp_pair.first] = std::pair(t.matrix(), timestamp_pair.second); + + Eigen::Vector3d euler = d.toRotationMatrix().eulerAngles(0, 1, 2) * (180.0 / M_PI); + counter++; + if (counter % 100 == 0) + { + spdlog::info("[{} of {}]: Roll {}, Pitch {}, Yaw {}", counter, imu_data.size(), euler.x(), euler.y(), euler.z()); + } + + // log it for implot + imu_data_plot.timestampLidar.push_back(timestamp_pair.first); + imu_data_plot.angX.push_back(gyr.x()); + imu_data_plot.angY.push_back(gyr.y()); + imu_data_plot.angZ.push_back(gyr.z()); + imu_data_plot.accX.push_back(acc.x()); + imu_data_plot.accY.push_back(acc.y()); + imu_data_plot.accZ.push_back(acc.z()); + imu_data_plot.yaw.push_back(euler.z()); + imu_data_plot.pitch.push_back(euler.y()); + imu_data_plot.roll.push_back(euler.x()); + } + + // timeline x-axis is shown relative to the first IMU sample so it starts at 0 [s]; + // datasetStart keeps the absolute value for reference / annotations + if (!imu_data_plot.timestampLidar.empty()) + { + imu_data_plot.datasetStart = imu_data_plot.timestampLidar.front(); + imu_data_plot.timestampRel.reserve(imu_data_plot.timestampLidar.size()); + for (double ts : imu_data_plot.timestampLidar) + imu_data_plot.timestampRel.push_back(ts - imu_data_plot.datasetStart); + } + + std::vector> timestamps; + std::vector poses; + for (const auto& t : trajectory) + { + timestamps.emplace_back(t.first, t.second.second); + Eigen::Affine3d m; + m.matrix() = t.second.first; + poses.push_back(m); + } + + g_points_per_file = std::move(pointsPerFile); + g_timestamps_cache = std::move(timestamps); + g_poses_cache = std::move(poses); + + rebuildAllData(false); + } +} + +void openFolder() +{ + std::string input_folder_name; + std::vector input_file_names; + input_folder_name = mandeye::fd::SelectFolder("Select Mandeye data folder"); + + spdlog::info("Selected folder: '{}'", input_folder_name); + + const std::vector subDirs{ "", "CAMERA_0" }; + for (const auto& subdir : subDirs) + { + const auto location = std::filesystem::path(input_folder_name) / subdir; + if (fs::exists(location)) + { + for (const auto& entry : fs::directory_iterator(location)) + if (entry.is_regular_file()) + input_file_names.push_back(entry.path().string()); + } + } + + loadFiles(input_file_names); +} + +void openFiles() +{ + std::vector input_file_names = mandeye::fd::OpenFileDialog("Load all files", mandeye::fd::All_Filter, true); + + loadFiles(input_file_names); +} + +void imu_data_gui() +{ + ImGui::Begin("IMU data", &show_imu_data); + { + if (imu_data_plot.timestampRel.size() > 0) + { + // All timelines below use time relative to the start of the dataset (t = 0 s). + // t0 is that start expressed as an absolute LiDAR timestamp in seconds. + const double t0 = imu_data_plot.datasetStart; + + ImGui::TextDisabled("Time axis is relative to dataset start."); + ImGui::Text("Dataset start (t = 0 s) = %.6f s absolute LiDAR time", t0); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Add this offset to any x-axis value to get the absolute LiDAR timestamp [s]"); + ImGui::Text("Dataset duration: %.3f s", imu_data_plot.timestampRel.back()); + ImGui::Separator(); + + static double x_min = 0.0; + static double x_max = 20.0; + + // currently rendered point block, expressed on the relative time axis + double annotation = 0; + bool has_annotation = false; + if (index_rendered_points_local >= 0 && index_rendered_points_local < all_data.size()) + { + if (all_data[index_rendered_points_local].timestamps.size() > 0) + { + annotation = all_data[index_rendered_points_local].timestamps.front().first - t0; + has_annotation = true; + } + } + + const double* ts = imu_data_plot.timestampRel.data(); + const int ts_n = (int)imu_data_plot.timestampRel.size(); + + if (ImPlot::BeginPlot("IMU - acceleration 'm/s^2", ImVec2(-1, 0))) + { + ImPlot::SetupAxisLimits(ImAxis_X1, x_min, x_max, ImGuiCond_Once); + ImPlot::SetupAxisLinks(ImAxis_X1, &x_min, &x_max); + ImPlot::SetupAxis(ImAxis_X1, "time since dataset start [s]"); + ImPlot::PlotLine("accX", ts, imu_data_plot.accX.data(), ts_n); + ImPlot::PlotLine("accY", ts, imu_data_plot.accY.data(), ts_n); + ImPlot::PlotLine("accZ", ts, imu_data_plot.accZ.data(), ts_n); + if (has_annotation) + ImPlot::TagX(annotation, ImVec4(1, 0, 0, 1)); + ImPlot::EndPlot(); + } + + if (ImPlot::BeginPlot("Imu - gyro", ImVec2(-1, 0))) + { + ImPlot::SetupAxisLimits(ImAxis_X1, x_min, x_max, ImGuiCond_Once); + ImPlot::SetupAxisLinks(ImAxis_X1, &x_min, &x_max); + ImPlot::SetupAxis(ImAxis_X1, "time since dataset start [s]"); + ImPlot::PlotLine("angX", ts, imu_data_plot.angX.data(), ts_n); + ImPlot::PlotLine("angY", ts, imu_data_plot.angY.data(), ts_n); + ImPlot::PlotLine("angZ", ts, imu_data_plot.angZ.data(), ts_n); + if (has_annotation) + ImPlot::TagX(annotation, ImVec4(1, 0, 0, 1)); + ImPlot::EndPlot(); + } + + if (ImPlot::BeginPlot("IMU - AHRS angles [deg]", ImVec2(-1, 0))) + { + ImPlot::SetupAxisLimits(ImAxis_X1, x_min, x_max, ImGuiCond_Once); + ImPlot::SetupAxisLinks(ImAxis_X1, &x_min, &x_max); + ImPlot::SetupAxis(ImAxis_X1, "time since dataset start [s]"); + ImPlot::PlotLine("yaw", ts, imu_data_plot.yaw.data(), ts_n); + ImPlot::PlotLine("pitch", ts, imu_data_plot.pitch.data(), ts_n); + ImPlot::PlotLine("roll", ts, imu_data_plot.roll.data(), ts_n); + if (has_annotation) + ImPlot::TagX(annotation, ImVec4(1, 0, 0, 1)); + ImPlot::EndPlot(); + } + + if (!photo_files_ts.empty() && ImPlot::BeginPlot("Photos")) + { + ImPlot::SetupAxisLimits(ImAxis_X1, x_min, x_max, ImGuiCond_Once); + ImPlot::SetupAxisLinks(ImAxis_X1, &x_min, &x_max); + ImPlot::SetupAxis(ImAxis_X1, "time since dataset start [s]"); + // plot photos timestamps, also relative to the dataset start + std::vector photo_timestamps; + std::vector dummy; + for (const auto& [ts_ns, fn] : photo_files_ts) + { + photo_timestamps.push_back(static_cast(ts_ns) / 1e9 - t0); + dummy.push_back(0.0); + } + ImPlot::PlotScatter("photos", photo_timestamps.data(), dummy.data(), (int)photo_timestamps.size()); + if (has_annotation) + ImPlot::TagX(annotation, ImVec4(1, 0, 0, 1)); + ImPlot::EndPlot(); + } + } + + if (ImGui::Button("Save 'ts gyr_x gyr_y gyr_z acc_x acc_y acc_z yaw_rad pitch_rad roll_rad' to csv")) + { + std::string output_file_name = ""; + output_file_name = mandeye::fd::SaveFileDialog("Save IMU data", {}, ""); + spdlog::info("file to save: '{}'", output_file_name); + + ofstream file; + file.open(output_file_name); + file << std::setprecision(20); + for (size_t i = 0; i < imu_data_plot.timestampLidar.size(); i++) + { + file << imu_data_plot.timestampLidar[i] << " " << imu_data_plot.angX[i] << " " << imu_data_plot.angY[i] << " " + << imu_data_plot.angZ[i] << " " << imu_data_plot.accX[i] << " " << imu_data_plot.accY[i] << " " + << imu_data_plot.accZ[i] << " " << imu_data_plot.yaw[i] << " " << imu_data_plot.pitch[i] << " " + << imu_data_plot.roll[i] << std::endl; + } + + file.close(); + } + } + + ImGui::End(); +} + +void settings_gui() +{ + if (ImGui::Begin("Settings", &is_settings_gui)) + { + ImGui::PushItemWidth(ImGuiNumberWidth); + ImGui::InputInt("number_of_points_threshold", &number_of_points_threshold); + if (number_of_points_threshold < 1) + number_of_points_threshold = 1; + if (ImGui::IsItemHovered()) + { + ImGui::BeginTooltip(); + ImGui::Text("Max number of points per rendered chunk (one 'Index' step)."); + ImGui::Text("Lower = shorter time span per chunk = finer time navigation with the Index slider."); + ImGui::EndTooltip(); + } + ImGui::BeginDisabled(!is_init); + ImGui::SameLine(); + if (ImGui::Button("Rebin")) + rebuildAllData(true); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Re-chunk the already loaded data with the new threshold, keeping the current time position"); + ImGui::EndDisabled(); + if (is_init) + ImGui::Text("Chunks: %d", static_cast(all_data.size())); + + ImGui::InputDouble("VQF tauAcc [s]", &vqf_tauAcc); + if (ImGui::IsItemHovered()) + { + ImGui::BeginTooltip(); + ImGui::Text("VQF accelerometer time constant (tauAcc) in seconds."); + ImGui::Text("Controls how strongly accelerometer corrects the gyroscope-based orientation."); + ImGui::Text("Higher = more gyro trust (stable but may drift). Lower = more acc trust (noisy but no drift)."); + ImGui::EndTooltip(); + } + + // ImGui::Checkbox("is_slerp", &is_slerp); + + ImGui::PopItemWidth(); + + ImGui::BeginDisabled(!is_init); + if (ImGui::Button("Save point cloud")) + { + std::string output_file_name = ""; + output_file_name = mandeye::fd::SaveFileDialog("Save las or laz file", mandeye::fd::LAS_LAZ_filter); + + if (output_file_name.size() > 0) + { + spdlog::info("las or laz file to save: '{}'", output_file_name); + + std::vector pointcloud; + std::vector intensity; + std::vector timestamps; + + /*if (index_rendered_points_local >= 0 && index_rendered_points_local < all_points_local.size()) + { + for (size_t i = 0; i < all_points_local[index_rendered_points_local].size(); i++) + { + pointcloud.emplace_back(all_points_local[index_rendered_points_local][i].point.x(), + all_points_local[index_rendered_points_local][i].point.y(), all_points_local[index_rendered_points_local][i].point.z()); + intensity.push_back(all_points_local[index_rendered_points_local][i].intensity); + timestamps.push_back(all_points_local[index_rendered_points_local][i].timestamp); + } + }*/ + + if (!exportLaz(output_file_name, pointcloud, intensity, timestamps, 0, 0, 0)) + spdlog::error("problem with saving file: '{}'", output_file_name); + } + } + ImGui::EndDisabled(); + + ImGui::Separator(); + + ImGui::PushItemWidth(ImGuiNumberWidth); + ImGui::InputDouble("Distance bucket [m]", &distance_bucket); + ImGui::InputDouble("Polar angle [deg]", &polar_angle_deg); + ImGui::InputDouble("Azimutal angle [deg]", &azimutal_angle_deg); + ImGui::InputDouble("Max distance LiDAR", &max_distance_lidar); + ImGui::InputInt("robust_and_accurate_lidar_odometry_iterations", &robust_and_accurate_lidar_odometry_iterations); + ImGui::Checkbox("Use multithread", &useMultithread); + ImGui::Text("Pose component weights for optimization:"); + if (ImGui::IsItemHovered()) + { + ImGui::BeginTooltip(); + ImGui::Text("Value guideline:"); + ImGui::Text("1e6: almost rigid motion, prior dominates (default)"); + ImGui::Text("1e4: small corrections allowed"); + ImGui::Text("1e2: free alignment"); + ImGui::Text(" 0: motion model ignored"); + ImGui::EndTooltip(); + } + ImGui::InputDouble("wx", &wx); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Translation penaltie for %s", xText); + ImGui::InputDouble("wy", &wy); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Translation penaltie for %s", yText); + ImGui::InputDouble("wz", &wz); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Translation penaltie for %s", zText); + ImGui::InputDouble("wom", &wom); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(omText); + ImGui::InputDouble("wfi", &wfi); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(fiText); + ImGui::InputDouble("wka", &wka); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip(kaText); + ImGui::PopItemWidth(); + + ImGui::NewLine(); + + if (ImGui::Button("Optimize")) + optimize(); + + ImGui::Separator(); + + if (ImGui::Button("Print to console debug text")) + { + double max_diff = 0; + if (index_rendered_points_local >= 0 && index_rendered_points_local < all_data.size()) + { + for (size_t i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) + { + auto lower = std::lower_bound( + all_data[index_rendered_points_local].timestamps.begin(), + all_data[index_rendered_points_local].timestamps.end(), + all_data[index_rendered_points_local].points_local[i].timestamp, + [](std::pair lhs, double rhs) -> bool + { + return lhs.first < rhs; + }); + + int index_pose = std::distance(all_data[index_rendered_points_local].timestamps.begin(), lower) - 1; + + spdlog::info( + "poses {}/{}: point TS [s] - pose TS [s] = ... [s]", + index_pose, + all_data[index_rendered_points_local].poses.size()); + + if (index_pose >= 0 && index_pose < all_data[index_rendered_points_local].poses.size()) + { + spdlog::info( + "{:.20g} - {:.20g} = {:.20g}", + all_data[index_rendered_points_local].points_local[i].timestamp, + all_data[index_rendered_points_local].timestamps[index_pose].first, + all_data[index_rendered_points_local].points_local[i].timestamp - + all_data[index_rendered_points_local].timestamps[index_pose].first); + + if (fabs( + all_data[index_rendered_points_local].points_local[i].timestamp - + all_data[index_rendered_points_local].timestamps[index_pose].first) > max_diff) + { + // spdlog::info("{:.20g}", all_data[index_rendered_points_local].points_local[i].timestamp); + max_diff = fabs( + all_data[index_rendered_points_local].points_local[i].timestamp - + all_data[index_rendered_points_local].timestamps[index_pose].first); + } + } + } + spdlog::info("max_diff: {} [s]", max_diff); + spdlog::info("----------------"); + for (size_t k = 0; k < all_data[index_rendered_points_local].timestamps.size(); k++) + spdlog::info("{:.20g}", all_data[index_rendered_points_local].timestamps[k].first); + } + } + } + + 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(); + + updateCameraTransition(); + + viewLocal = Eigen::Affine3f::Identity(); + + if (!is_ortho) + { + reshape((GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); + + 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(); + + // + /* glPointSize(point_size); + glBegin(GL_POINTS); + if (index_rendered_points_local >= 0 && index_rendered_points_local < all_points_local.size()) + { + for (size_t i = 0; i < all_points_local[index_rendered_points_local].size(); i++) + { + if (all_lidar_ids[index_rendered_points_local][i] == 0) + { + glColor3f(pc_color.x, pc_color.y, pc_color.z); + } + else + { + glColor3f(pc_color2.x, pc_color2.y, pc_color2.z); + } + glVertex3f(all_points_local[index_rendered_points_local][i].point.x(), + all_points_local[index_rendered_points_local][i].point.y(), all_points_local[index_rendered_points_local][i].point.z()); + } + } + glEnd(); + glPointSize(1);*/ + + glPointSize(point_size); + glBegin(GL_POINTS); + if (index_rendered_points_local >= 0 && index_rendered_points_local < all_data.size()) + { + double max_diff = 0.0; + float max_intensity = 0.0f; + for (size_t i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) + { + if (color_by_intensity && all_data[index_rendered_points_local].points_local[i].intensity > max_intensity) + max_intensity = all_data[index_rendered_points_local].points_local[i].intensity; + + auto lower = std::lower_bound( + all_data[index_rendered_points_local].timestamps.begin(), + all_data[index_rendered_points_local].timestamps.end(), + all_data[index_rendered_points_local].points_local[i].timestamp, + [](std::pair lhs, double rhs) -> bool + { + return lhs.first < rhs; + }); + int index_pose = std::distance(all_data[index_rendered_points_local].timestamps.begin(), lower) - 1; + if (index_pose >= 0 && index_pose < all_data[index_rendered_points_local].poses.size()) + { + if (fabs( + all_data[index_rendered_points_local].points_local[i].timestamp - + all_data[index_rendered_points_local].timestamps[index_pose].first) > max_diff) + { + max_diff = fabs( + all_data[index_rendered_points_local].points_local[i].timestamp - + all_data[index_rendered_points_local].timestamps[index_pose].first); + } + } + } + const float inv_max_intensity = max_intensity > 0.0f ? 1.0f / max_intensity : 0.0f; + + for (size_t i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) + { + auto lower = std::lower_bound( + all_data[index_rendered_points_local].timestamps.begin(), + all_data[index_rendered_points_local].timestamps.end(), + all_data[index_rendered_points_local].points_local[i].timestamp, + [](std::pair lhs, double rhs) -> bool + { + return lhs.first < rhs; + }); + + int index_pose = std::distance(all_data[index_rendered_points_local].timestamps.begin(), lower) - 1; + + if (max_diff < 0.1) + { + if (index_pose >= 0 && index_pose < all_data[index_rendered_points_local].poses.size()) + { + // if (fabs(all_data[index_rendered_points_local].timestamps[index_pose].first - + // all_data[index_rendered_points_local].points_local[i].timestamp) < 0.001) + //{ + Eigen::Affine3d m = all_data[index_rendered_points_local].poses[index_pose]; + Eigen::Vector3d p = m * all_data[index_rendered_points_local].points_local[i].point; + + if (color_by_intensity) + { + float ii = all_data[index_rendered_points_local].points_local[i].intensity * inv_max_intensity; + ii = std::clamp(ii, 0.0f, 1.0f); + if (intensity_gamma != 1.0f) + ii = std::pow(ii, intensity_gamma); + glColor3f(ii, ii, ii); + } + else if (all_data[index_rendered_points_local].lidar_ids[i] == 0) + glColor3f(pc_color.x, pc_color.y, pc_color.z); + else + glColor3f(pc_color2.x, pc_color2.y, pc_color2.z); + + glVertex3f(p.x(), p.y(), p.z()); + //} + } + } + } + } + glEnd(); + glPointSize(1); + + if (index_rendered_points_local >= 0 && index_rendered_points_local < all_data.size()) + { + for (size_t i = 0; i < all_data[index_rendered_points_local].points_local.size(); i++) + { + auto lower = std::lower_bound( + all_data[index_rendered_points_local].timestamps.begin(), + all_data[index_rendered_points_local].timestamps.end(), + all_data[index_rendered_points_local].points_local[i].timestamp, + [](std::pair lhs, double rhs) -> bool + { + return lhs.first < rhs; + }); + + int index_pose = std::distance(all_data[index_rendered_points_local].timestamps.begin(), lower) - 1; + + if (index_pose >= 0 && index_pose < all_data[index_rendered_points_local].poses.size()) + { + /*Eigen::Affine3d m = all_data[index_rendered_points_local].poses[index_pose]; + Eigen::Vector3d p = m * all_data[index_rendered_points_local].points_local[i].point; + + if (all_data[index_rendered_points_local].lidar_ids[i] == 0) + { + glColor3f(pc_color.x, pc_color.y, pc_color.z); + } + else + { + glColor3f(pc_color2.x, pc_color2.y, pc_color2.z); + } + glVertex3f(p.x(), p.y(), p.z());*/ + + Eigen::Affine3d m = all_data[index_rendered_points_local].poses[index_pose]; + glLineWidth(2.0); + glBegin(GL_LINES); + glColor3f(1.0f, 0.0f, 0.0f); + glVertex3f(m(0, 3), m(1, 3), m(2, 3)); + glVertex3f(m(0, 3) + m(0, 0), m(1, 3) + m(1, 0), m(2, 3) + m(2, 0)); + + glColor3f(0.0f, 1.0f, 0.0f); + glVertex3f(m(0, 3), m(1, 3), m(2, 3)); + glVertex3f(m(0, 3) + m(0, 1), m(1, 3) + m(1, 1), m(2, 3) + m(2, 1)); + + glColor3f(0.0f, 0.0f, 1.0f); + glVertex3f(m(0, 3), m(1, 3), m(2, 3)); + glVertex3f(m(0, 3) + m(0, 2), m(1, 3) + m(1, 2), m(2, 3) + m(2, 2)); + glEnd(); + glLineWidth(1.0); + break; + } + } + } + + if (show_rgd_nn) + { + glColor3f(0, 0, 0); + glBegin(GL_LINES); + for (const auto& nn : rgd_nn) + { + glVertex3f(nn.first.x(), nn.first.y(), nn.first.z()); + glVertex3f(nn.second.x(), nn.second.y(), nn.second.z()); + } + glEnd(); + } + + if (show_mean_cov) + { + for (const auto& mc : mean_cov) + draw_ellipse(mc.second, mc.first, Eigen::Vector3f(1, 0, 0), 1); + } + + showAxes(); + + ImGui_ImplOpenGL2_NewFrame(); + ImGui_ImplGLUT_NewFrame(); + ImGui::NewFrame(); + + ShowMainDockSpace(); + + view_kbd_shortcuts(); + + if (all_data.size() > 0) + { + if ((!io.KeyCtrl && !io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_RightArrow, true)) || ImGui::IsKeyPressed(ImGuiKey_PageUp, true) || + ImGui::IsKeyPressed(ImGuiKey_KeypadAdd, true)) + index_rendered_points_local += 1; + if ((!io.KeyCtrl && !io.KeyShift && ImGui::IsKeyPressed(ImGuiKey_LeftArrow, true)) || + ImGui::IsKeyPressed(ImGuiKey_PageDown, true) || ImGui::IsKeyPressed(ImGuiKey_KeypadSubtract, true)) + index_rendered_points_local -= 1; + + if (index_rendered_points_local < 0) + index_rendered_points_local = 0; + if (index_rendered_points_local >= all_data.size()) + index_rendered_points_local = all_data.size() - 1; + } + + // ask to load the nearest photo + if (show_cameras_data && index_rendered_points_local >= 0 && index_rendered_points_local < indexes_to_filename.size()) + { + double ts = all_data[index_rendered_points_local].points_local[0].timestamp; // * 1e9; + render_nearest_photo(ts); + } + + if (ImGui::BeginMainMenuBar()) + { + { + if (ImGui::Button("Open folder")) + openFolder(); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Select folder containing files for analyze (Ctrl+O)"); + + ImGui::SameLine(); + + if (ImGui::ArrowButton("##menuArrow", ImGuiDir_Down)) + ImGui::OpenPopup("OpenMenu"); + + if (ImGui::BeginPopup("OpenMenu")) + { + if (ImGui::MenuItem("Open files")) + openFiles(); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Select files for analyze"); + + ImGui::EndPopup(); + } + + ImGui::SameLine(); + ImGui::Dummy(ImVec2(20, 0)); + ImGui::SameLine(); + } + + if (ImGui::BeginMenu("View")) + { + 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; + + ImGui::Separator(); + + 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::BeginDisabled(!is_init); + if (ImGui::MenuItem("Show RGD / NN", nullptr, &show_rgd_nn)) + { + if (show_rgd_nn) + rgd_nn = get_nn(); + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Show Regular/Range Grid Data / Nearest Neighbors"); + + if (ImGui::MenuItem("Show mean covs", nullptr, &show_mean_cov)) + { + if (show_mean_cov) + mean_cov = get_mean_cov(); + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Show the mean and covariance ellipsoid representation"); + + ImGui::MenuItem("Show IMU data", nullptr, &show_imu_data); + ImGui::MenuItem("Show cameras data", nullptr, &show_cameras_data); + + ImGui::MenuItem("Color by intensity", nullptr, &color_by_intensity); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Render points as grayscale from LiDAR intensity (normalized to current block max)"); + if (color_by_intensity) + { + ImGui::SetNextItemWidth(ImGuiNumberWidth); + ImGui::SliderFloat("Intensity gamma", &intensity_gamma, 0.1f, 3.0f, "%.2f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Gamma applied to normalized intensity (<1 brightens, >1 darkens)"); + } + ImGui::EndDisabled(); + + ImGui::Separator(); + + if (ImGui::BeginMenu("Colors")) + { + ImGui::ColorEdit3("Background", (float*)&bg_color, ImGuiColorEditFlags_NoInputs); + ImGui::ColorEdit3("Point cloud 1", (float*)&pc_color, ImGuiColorEditFlags_NoInputs); + ImGui::ColorEdit3("Point cloud 2", (float*)&pc_color2, ImGuiColorEditFlags_NoInputs); + + ImGui::EndMenu(); + } + + ImGui::Separator(); + + ImGui::MenuItem("Settings", nullptr, &is_settings_gui); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Show power user settings window with more parameters"); + + ImGui::EndMenu(); + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Scene view relevant parameters"); + + camMenu(); + + ImGui::SameLine(); + ImGui::Dummy(ImVec2(20, 0)); + ImGui::SameLine(); + + if (all_data.size() > 0) + { + int tempIndex = index_rendered_points_local; + ImGui::Text("Index: "); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Index of rendered points local"); + ImGui::SameLine(); + ImGui::PushItemWidth(ImGuiNumberWidth); + ImGui::SliderInt("##irpls", &tempIndex, 0, static_cast(all_data.size() - 1)); + if (ImGui::IsItemHovered()) + { + ImGui::BeginTooltip(); + std::string fn = laz_files[indexes_to_filename[index_rendered_points_local]]; + ImGui::Text(fn.c_str()); + ImGui::EndTooltip(); + } + ImGui::SameLine(); + ImGui::InputInt("##irpli", &tempIndex, 1, 10); + if (ImGui::IsItemHovered()) + { + ImGui::BeginTooltip(); + std::string fn = laz_files[indexes_to_filename[index_rendered_points_local]]; + ImGui::Text(fn.c_str()); + ImGui::EndTooltip(); + } + ImGui::PopItemWidth(); + + if ((tempIndex >= 0) && (tempIndex < all_data.size())) + index_rendered_points_local = tempIndex; + + ImGui::SameLine(); + double ts = all_data[index_rendered_points_local].points_local[0].timestamp; // * 1e9; + ImGui::Text("Timestamp: %.6f [ns]", ts); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Click to copy to clipboard"); + if (ImGui::IsItemClicked()) + { + char tmp[64]; + snprintf(tmp, sizeof(tmp), "%.6f", ts); + ImGui::SetClipboardText(tmp); + } + + ImGui::SameLine(); + ImGui::Dummy(ImVec2(20, 0)); + ImGui::SameLine(); + } + + ImGui::SameLine(); + ImGui::Text("(%.1f FPS)", ImGui::GetIO().Framerate); + + 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(); + } + + info_window(infoLines, appShortcuts); + + if (compass_ruler) + drawMiniCompassWithRuler(); + + if (show_cameras_data && photos::photo_texture_cam0) + { + if (ImGui::Begin("CAM0")) + { + using namespace photos; + ImGui::Text("fn name: %s ", photos::imgToShowFn.c_str()); + ImGui::Text("timestamp: %s", std::to_string(photos::nearestTs).c_str()); + // get available size + DisplayImageFit((ImTextureID)photos::photo_texture_cam0, photo_width_cam0, photo_height_cam0); + } + + ImGui::End(); + } + + if (show_imu_data) + imu_data_gui(); + + if (is_settings_gui) + settings_gui(); + + ImGui::Render(); + ImGui_ImplOpenGL2_RenderDrawData(ImGui::GetDrawData()); + + glutSwapBuffers(); + glutPostRedisplay(); +} + +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_MIDDLE_BUTTON && state == GLUT_DOWN && io.KeyCtrl) + { + } + + if (state == GLUT_DOWN) + { + mouse_buttons |= 1 << glut_button; + } + 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 where scan files are located (*.csv, *.laz, *.sn)\n" + << " -h, /h, --help, /? Show this help and exit\n\n"; + + return 0; + } + + initGL(&argc, argv, winTitle, display, mouse); + ImPlot::CreateContext(); + + if (argc == 2) + { + std::vector input_file_names; + + if (fs::exists(argv[1])) + { + for (const auto& entry : fs::recursive_directory_iterator(argv[1])) + if (entry.is_regular_file()) + input_file_names.push_back(entry.path().string()); + + loadFiles(input_file_names); + } + } + + glutMainLoop(); + + ImGui_ImplOpenGL2_Shutdown(); + ImGui_ImplGLUT_Shutdown(); + ImGui::DestroyContext(); + ImPlot::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/mandeye_raw_data_viewer_legacy/resource.h b/apps/mandeye_raw_data_viewer_legacy/resource.h new file mode 100644 index 00000000..4f3204ca --- /dev/null +++ b/apps/mandeye_raw_data_viewer_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/mandeye_raw_data_viewer_legacy/resource.rc b/apps/mandeye_raw_data_viewer_legacy/resource.rc new file mode 100644 index 00000000..fcf8049b --- /dev/null +++ b/apps/mandeye_raw_data_viewer_legacy/resource.rc @@ -0,0 +1,50 @@ +// 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 Raw Data Viewer\0" + VALUE "FileVersion", "0.100.1\0" + VALUE "InternalName", "Raw data viewer\0" + VALUE "LegalCopyright", "(c) 2026 github.com/MapsHD/HDMapping\0" + VALUE "OriginalFilename", "mandeye_raw_data_viewer.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 +///////////////////////////////////////////////////////////////////////////// + diff --git a/apps/multi_session_registration/CMakeLists.txt b/apps/multi_session_registration/CMakeLists.txt index dce3214f..ec07752e 100644 --- a/apps/multi_session_registration/CMakeLists.txt +++ b/apps/multi_session_registration/CMakeLists.txt @@ -41,7 +41,9 @@ target_include_directories( target_link_libraries( multi_session_registration_step_3 # PRIVATE ${THIRDPARTY_DIRECTORY}/glew-2.2.0/lib/Release/x64/glew32s.lib - # raylib-based like step 2; core_raylib brings in core, raylib and ScanRenderer. + # raylib-based like step 2; core_raylib brings in core, raylib and ScanRenderer, core_raylib_utils the + # shared raylib replacement for . + core_raylib_utils core_raylib raylib_widgets imgui_raylib diff --git a/apps/multi_session_registration/multi_session_registration.cpp b/apps/multi_session_registration/multi_session_registration.cpp index 6a57918d..49b0fb1c 100644 --- a/apps/multi_session_registration/multi_session_registration.cpp +++ b/apps/multi_session_registration/multi_session_registration.cpp @@ -4105,6 +4105,7 @@ int main(int argc, char* argv[]) mainLoop(); + releaseSessionRenderers(); shutdownGL(); } catch (const std::bad_alloc& e) { diff --git a/apps/multi_session_registration/raylib_utils.cpp b/apps/multi_session_registration/raylib_utils.cpp index d6c667cb..0155ef0a 100644 --- a/apps/multi_session_registration/raylib_utils.cpp +++ b/apps/multi_session_registration/raylib_utils.cpp @@ -1,81 +1,12 @@ #include "raylib_utils.h" -#include "raymath.h" -#include "rlImGui.h" - -#include - -#include -#include -#include -#include -#include - -#include - #include -#include -#include +#include #include #include -raylib_widgets::OrbitCamera camera; - -// GLUT step 3 used 1000 (every point was a glVertex call); GPU buffers draw full density, as in step 2. -int viewer_decimate_point_cloud = 2; -int mouse_old_x = 0, mouse_old_y = 0; -int mouse_buttons = 0; -bool& is_ortho = camera.isOrtho; -bool& lock_z = camera.lockZ; -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; -bool cor_gui = false; -Eigen::Affine3f viewLocal = Eigen::Affine3f::Identity(); - -Eigen::Map rotation_center(&camera.euler.rotationCenter.x); -float& rotate_x = camera.euler.rotateX; -float& rotate_y = camera.euler.rotateY; -float& translate_x = camera.euler.translate.x; -float& translate_y = camera.euler.translate.y; -float& translate_z = camera.euler.translate.z; -Eigen::Map new_rotation_center(&camera.eulerGoal.rotationCenter.x); -float& new_rotate_x = camera.eulerGoal.rotateX; -float& new_rotate_y = camera.eulerGoal.rotateY; -float& new_translate_x = camera.eulerGoal.translate.x; -float& new_translate_y = camera.eulerGoal.translate.y; -float& new_translate_z = camera.eulerGoal.translate.z; -bool& camera_transition_active = camera.eulerTransitionActive; -float* const m_ortho_projection = camera.orthoProjection; -float* const m_ortho_gizmo_view = camera.orthoGizmoView; - namespace { - void (*display_cb)() = nullptr; - void (*mouse_cb)(int, int, int, int) = nullptr; - - Matrix frame_mvp{}; - float current_color[3] = { 1.f, 1.f, 1.f }; - - struct StripVertex - { - Vector3 p; - float c[3]; - }; - std::vector strip; - - struct Label - { - Vector3 p; - std::string text; - Color color; - }; - Vector3 label_pos{}; - float label_color[3] = { 1.f, 1.f, 1.f }; - std::vector