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/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp b/apps/lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp index 4bdf4019..af1779cd 100644 --- a/apps/lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp +++ b/apps/lidar_odometry_step_1/drag_folder_with_mandeye_data_and_drop_here-precision_forestry.cpp @@ -451,7 +451,8 @@ void step1(const std::atomic& loPause, std::string input_folder_name) 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)) + std::atomic parked{ false }; + if (compute_step_2(worker_data, params, ts_failure, loProgress, loPause, full_debug_messages, parked)) step_2_done = true; else { @@ -1603,7 +1604,8 @@ bool step2(std::vector& worker_data, LidarOdometryParams& params, co { double ts_failure = 0.0; std::atomic loProgress; - return compute_step_2(worker_data, params, ts_failure, loProgress, loPause, full_debug_messages); + std::atomic parked{ false }; + return compute_step_2(worker_data, params, ts_failure, loProgress, loPause, full_debug_messages, parked); } void save_results( diff --git a/apps/lidar_odometry_step_1/lidar_odometry.cpp b/apps/lidar_odometry_step_1/lidar_odometry.cpp index 1d8af25a..06b52ef8 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry.cpp @@ -1282,8 +1282,9 @@ std::vector run_lidar_odometry(const std::string& input_dir, LidarOd double ts_failure = 0.0; std::atomic loProgress; + std::atomic parked{ false }; - if (!compute_step_2(worker_data, params, ts_failure, loProgress, pause, true)) + if (!compute_step_2(worker_data, params, ts_failure, loProgress, pause, true, parked)) { std::cout << "Calculation failed at step 2 of lidar odometry, exiting." << std::endl; diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp index 03bb2aff..7cda5cf4 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 -#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" @@ -14,13 +21,14 @@ #include #include #include +#include #include #include -#include #include "toml_io.h" #include #include +#include #include #include #include @@ -28,7 +36,6 @@ #ifdef _WIN32 #include "resource.h" -#include #endif /////////////////////////////////////////////////////////////////////////////////// @@ -135,7 +142,7 @@ 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_indoor = true; bool show_reference_buckets_outdoor = false; bool show_reference_points = false; int dec_reference_points = 100; @@ -155,6 +162,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; @@ -184,6 +192,10 @@ Trajectory trajectory; std::atomic loRunning{ false }; std::atomic loProgress{ 0.0 }; std::atomic loPause{ false }; +std::atomic loParked{ false }; //!< step 2 is waiting on loPause and touches no file (see compute_step_2) +bool loQuitting = false; //!< quit confirmed mid-run: the run is paused, display() exits once it is parked or done +std::string pendingLoadFolder; //!< openData() picked it; display() loads it once the "Loading data" modal is on screen +int pendingLoadFrames = 0; //!< frames left before that load; 0 = none pending std::chrono::time_point loStartTime; std::atomic loElapsedSeconds{ 0.0 }; std::atomic loEstimatedTimeRemaining{ 0.0 }; @@ -619,18 +631,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()) @@ -664,7 +674,7 @@ void step1(const std::atomic& loPause) 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)) + if (compute_step_2(worker_data, params, ts_failure, loProgress, loPause, full_debug_messages, loParked)) step_2_done = true; else { @@ -1150,7 +1160,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; @@ -1737,7 +1747,9 @@ void progress_window() ImGui::NewLine(); - if (!loPause) + if (loQuitting) + ImGui::TextColored(ImVec4(1.f, 0.6f, 0.2f, 1.f), "Quitting: waiting for the current worker to finish..."); + else if (!loPause) { if (ImGui::Button("Pause")) loPause.store(true); @@ -1761,6 +1773,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,7 +1787,19 @@ void progress_window() ImGui::End(); } -void openData() +//! folder: Mandeye data folder to process; empty asks for one with a folder dialog. +//! Loading (step 1) blocks the UI thread, so display() first presents a "Loading data" modal and only +//! then calls loadData(): one frame for the auto-resized modal to measure itself, one to show it. +void openData(const std::string& folder = "") +{ + const std::string input_folder_name = folder.empty() ? mandeye::fd::SelectFolder("Select Mandeye data folder") : folder; + if (input_folder_name.empty()) + return; + pendingLoadFolder = input_folder_name; + pendingLoadFrames = 3; +} + +void loadData(const std::string& input_folder_name) { is_settings_gui = false; info_gui = false; @@ -1785,7 +1810,7 @@ void openData() loEstimatedTimeRemaining.store(0.0); loStartTime = std::chrono::system_clock::now(); - step1(loPause); + step1(input_folder_name, loPause); if (step_1_done) { @@ -1864,7 +1889,8 @@ bool step2(std::vector& worker_data, LidarOdometryParams& params, co { double ts_failure = 0.0; std::atomic loProgress; - return compute_step_2(worker_data, params, ts_failure, loProgress, loPause, full_debug_messages); + std::atomic parked{ false }; + return compute_step_2(worker_data, params, ts_failure, loProgress, loPause, full_debug_messages, parked); } void save_results( @@ -1878,22 +1904,67 @@ void save_results( save_result(worker_data, params, outwd, elapsed_seconds); } +//! Window close: while odometry runs, ask first. A confirmed quit pauses the run and keeps the +//! window open until display() sees the run parked or finished (quit_abandoned_run()). +bool confirm_close() +{ + if (!loRunning) + return true; + if (loQuitting) + return false; + pfd::message message( + "Lidar odometry is running", + "Quit anyway? The run is abandoned: nothing is saved and the cache is deleted.", + pfd::choice::yes_no, + pfd::icon::warning); + if (message.result() != pfd::button::yes) + return false; + loQuitting = true; + loPause.store(true); + return false; +} + +//! The detached run thread can't be joined and still uses the globals, so the process ends +//! without destructors, after removing the cache the parked (or finished) run no longer reads. +static void quit_abandoned_run() +{ + remove_cache(params); + std::_Exit(0); +} + +//! intensityRange() over the buckets' mean intensities; ray-marked buckets (number_of_points == -1) carry none. +static IntensityRange bucketIntensityRange(const NDTBucketMapType& buckets) +{ + IntensityRange range; + bool first = true; + for (const auto& b : buckets) + { + if (b.second.number_of_points == -1) + continue; + range.min = first ? b.second.intensity : std::min(range.min, b.second.intensity); + range.max = first ? b.second.intensity : std::max(range.max, b.second.intensity); + first = false; + } + return range; +} + void display() { + if (loQuitting && (loParked || !loRunning)) + quit_abandoned_run(); + 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 +1979,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 +2033,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 +2083,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 +2105,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 +2141,122 @@ 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); + const IntensityRange range = bucketIntensityRange(params.buckets_indoor); + 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 if (intensity_shading) + { + color3fIntensityShaded(1, 0, 0, b.second.intensity, range); } 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); + const IntensityRange range = bucketIntensityRange(params.buckets_outdoor); + 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 if (intensity_shading) + { + color3fIntensityShaded(0, 0, 1, b.second.intensity, range); } 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,52 +2278,81 @@ 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(); - if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_O, false)) + // 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 && pendingLoadFrames == 0) + { + fs::path folder(dropped[0]); + if (!fs::is_directory(folder)) + folder = folder.parent_path(); + openData(folder.string()); + } + + // The previous frame presented the "Loading data" modal, so it stays on screen while loading blocks. + if (pendingLoadFrames == 1) + { + pendingLoadFrames = 0; + loadData(pendingLoadFolder); + } + else if (pendingLoadFrames > 1) + --pendingLoadFrames; + + if (pendingLoadFrames > 0 && !ImGui::IsPopupOpen("Loading data")) + ImGui::OpenPopup("Loading data"); + ImGui::SetNextWindowPos(ImGui::GetMainViewport()->GetCenter(), ImGuiCond_Appearing, ImVec2(0.5f, 0.5f)); + if (ImGui::BeginPopupModal("Loading data", nullptr, ImGuiWindowFlags_AlwaysAutoResize)) + { + ImGui::Text("Loading data and running step 1 from folder:\n'%s'", pendingLoadFolder.c_str()); + ImGui::TextDisabled("The window does not respond until this finishes, see console for progress."); + if (pendingLoadFrames == 0) + ImGui::CloseCurrentPopup(); + ImGui::EndPopup(); + } + + if (io.KeyCtrl && ImGui::IsKeyPressed(ImGuiKey_O, false) && !loRunning && pendingLoadFrames == 0) { openData(); @@ -2447,7 +2559,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 +2588,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 +2650,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 +2690,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 +2740,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 +2756,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 +2901,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(confirm_close); - 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/lidar_odometry_utils.cpp b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp index 2bc631f7..42359ddf 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp @@ -100,6 +100,7 @@ void update_rgd_ray( { auto& this_bucket = bucket_it->second; this_bucket.number_of_points++; + this_bucket.intensity += (points_global[i].intensity - this_bucket.intensity) / this_bucket.number_of_points; const auto& curr_mean = points_global[i].point; const auto& mean = this_bucket.mean; // buckets[index_of_bucket].mean += (mean - curr_mean) / buckets[index_of_bucket].number_of_points; @@ -165,6 +166,7 @@ void update_rgd_ray( bucket_to_add.cov = Eigen::Matrix3d::Identity() * 0.03 * 0.03; // ToDo move to params bucket_to_add.cov_inverse = bucket_to_add.cov.inverse(); bucket_to_add.number_of_points = 1; + bucket_to_add.intensity = points_global[i].intensity; buckets.emplace(index_of_bucket, bucket_to_add); /*Eigen::Vector3d direction = points_global[i].point - viewport; @@ -296,6 +298,7 @@ void update_rgd_spherical_coordinates( { auto& this_bucket = bucket_it->second; this_bucket.number_of_points++; + this_bucket.intensity += (points_global[i].intensity - this_bucket.intensity) / this_bucket.number_of_points; const auto& curr_mean = points_global[i].point; const auto& mean = this_bucket.mean; // buckets[index_of_bucket].mean += (mean - curr_mean) / buckets[index_of_bucket].number_of_points; @@ -359,6 +362,7 @@ void update_rgd_spherical_coordinates( bucket_to_add.cov = Eigen::Matrix3d::Identity() * 0.03 * 0.03; bucket_to_add.cov_inverse = bucket_to_add.cov.inverse(); bucket_to_add.number_of_points = 1; + bucket_to_add.intensity = points_global[i].intensity; buckets.emplace(index_of_bucket, bucket_to_add); } } diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.h b/apps/lidar_odometry_step_1/lidar_odometry_utils.h index b0699783..027971e9 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.h +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.h @@ -482,13 +482,15 @@ bool process_worker_step_update_rgd_after( LookupStats& lookup_stats, std::vector& intermediate_points); +// parked is true while the run waits on pause between workers: it then touches no file until resumed bool compute_step_2( std::vector& worker_data, LidarOdometryParams& params, double& ts_failure, std::atomic& loProgress, const std::atomic& pause, - bool debugMsg); + bool debugMsg, + std::atomic& parked); // for reconstructing worker data from step 1 output bool loadLaz( diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp index a480b02c..d7ac0e25 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -2390,7 +2390,8 @@ static bool compute_step_2_impl( double& ts_failure, std::atomic& loProgress, const std::atomic& pause, - bool debugMsg); + bool debugMsg, + std::atomic& parked); bool compute_step_2( std::vector& worker_data, @@ -2398,11 +2399,12 @@ bool compute_step_2( double& ts_failure, std::atomic& loProgress, const std::atomic& pause, - bool debugMsg) + bool debugMsg, + std::atomic& parked) { try { - return compute_step_2_impl(worker_data, params, ts_failure, loProgress, pause, debugMsg); + return compute_step_2_impl(worker_data, params, ts_failure, loProgress, pause, debugMsg, parked); } catch (const std::runtime_error& e) // a points_global spill I/O failure; its files are already removed { spdlog::error("{}", e.what()); @@ -2416,7 +2418,8 @@ static bool compute_step_2_impl( double& ts_failure, std::atomic& loProgress, const std::atomic& pause, - bool debugMsg) + bool debugMsg, + std::atomic& parked) { // exit(1); bool debug = false; @@ -2448,11 +2451,13 @@ static bool compute_step_2_impl( if (pause) { + parked.store(true); while (pause) { spdlog::info("pause"); std::this_thread::sleep_for(std::chrono::seconds(1)); } + parked.store(false); } spdlog::stopwatch stopwatch_worker_process_worker_step_1; 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 00000000..14e65190 Binary files /dev/null and b/apps/lidar_odometry_step_1_legacy/icon.ico differ 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..40e1531f --- /dev/null +++ b/apps/lidar_odometry_step_1_legacy/lidar_odometry_gui.cpp @@ -0,0 +1,2835 @@ +#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; + std::atomic parked{ false }; + if (compute_step_2(worker_data, params, ts_failure, loProgress, loPause, full_debug_messages, parked)) + 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; + std::atomic parked{ false }; + return compute_step_2(worker_data, params, ts_failure, loProgress, loPause, full_debug_messages, parked); +} + +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..8513f795 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,13 @@ -// clang-format off -#include -#include -// clang-format on - #include -#include -#include #include #include +#include + +#ifdef _WIN32 +// windows.h (pulled in transitively by the headers below) declares CloseWindow/ShowCursor like raylib.h does. +#define CloseWindow CloseWindow_win32 +#define ShowCursor ShowCursor_win32 +#endif #include #include @@ -17,8 +17,8 @@ #include #include #include +#include #include -#include #include @@ -36,9 +36,9 @@ #include #ifdef _WIN32 +#undef CloseWindow +#undef ShowCursor #include "resource.h" -#include // <-- Required for ShellExecuteA -#include #endif /////////////////////////////////////////////////////////////////////////////////// @@ -131,13 +131,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 +198,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 +1196,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 +1210,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 +1540,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 +1554,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 +1567,11 @@ void display() viewLocal.translate(-rotation_center); - glLoadMatrixf(viewLocal.matrix().data()); + rlMultMatrixf(viewLocal.matrix().data()); } else updateOrthoView(); + captureFrameMatrices(); // /* glPointSize(point_size); @@ -1579,12 +1595,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 +1658,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 +1707,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 +1729,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 +1747,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 +1785,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 +1864,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 +1977,8 @@ void display() info_window(infoLines, appShortcuts); + end3DAndDrawLabels(); + if (compass_ruler) drawMiniCompassWithRuler(); @@ -1977,34 +2002,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 +2061,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 00000000..710913e3 Binary files /dev/null and b/apps/mandeye_raw_data_viewer_legacy/icon.ico differ diff --git a/apps/mandeye_raw_data_viewer_legacy/mandeye_raw_data_viewer.cpp b/apps/mandeye_raw_data_viewer_legacy/mandeye_raw_data_viewer.cpp new file mode 100644 index 00000000..a938e25f --- /dev/null +++ b/apps/mandeye_raw_data_viewer_legacy/mandeye_raw_data_viewer.cpp @@ -0,0 +1,2080 @@ +// clang-format off +#include +#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