From 931f1d6321ca1b94ecd75c2fca21e44812ea83bd Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Fri, 30 Jan 2026 09:01:05 +0100 Subject: [PATCH] Simple changes: - allwos diffrent number of csv files and lidar_odometry (for Murata) - fix around camera in raw mandeye_raw_data_viewer Signed-off-by: Michal Pelka --- apps/lidar_odometry_step_1/lidar_odometry.cpp | 14 +-------- .../lidar_odometry_utils.cpp | 4 +-- .../mandeye_raw_data_viewer.cpp | 30 ++++++++----------- 3 files changed, 15 insertions(+), 33 deletions(-) diff --git a/apps/lidar_odometry_step_1/lidar_odometry.cpp b/apps/lidar_odometry_step_1/lidar_odometry.cpp index 1391a4e6..efbf6800 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry.cpp @@ -14,7 +14,6 @@ bool load_data( bool debugMsg) { std::sort(input_file_names.begin(), input_file_names.end()); - std::vector csv_files, laz_files; std::string sn_file; std::string calibrationFile; @@ -81,11 +80,6 @@ bool load_data( { std::cout << "\n!!! there is no CSV file for: " << laz_files[i] << " !!! \n\n"; - if (i < (laz_files.size() - 1)) - { - std::cout << "!!! can't continue with missmatched middle file pairs.. !!!\n\n"; - return false; - } } } @@ -107,12 +101,6 @@ bool load_data( if (!found) { std::cout << "\n!!! there is no LAZ file for: " << csv_files[i] << " !!! \n\n"; - - if (i < (csv_files.size() - 1)) - { - std::cout << "!!! can't continue with missmatched middle file pairs.. !!!\n\n"; - return false; - } } } @@ -121,7 +109,7 @@ bool load_data( std::string working_directory = fs::path(input_file_names.front()).parent_path().string(); if (laz_files.size() != csv_files.size()) - std::cout << "!!! file number issue. continuing but ignoring missmatched last file pair.. !!!\n" << std::endl; + std::cout << "!!! file number issue. continuing can lead to incomplete data!!!\n" << std::endl; // check if folder exists! if (!fs::exists(working_directory)) diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp index 1642814c..877c1ce2 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp @@ -514,9 +514,9 @@ std::vector, FusionVector, FusionVector>> l myfile.close(); } } - } catch (...) + } catch (const std::exception& e) { - std::cout << "load_imu error for file: '" << imu_file << "'" << std::endl; + std::cout << "load_imu error for file: '" << imu_file << "'" << e.what() << std::endl; // return all_data; } 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 f6e436c7..168e4050 100644 --- a/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp +++ b/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp @@ -120,7 +120,7 @@ std::vector> mean_cov; bool show_mean_cov = false; bool show_rgd_nn = false; bool show_imu_data = false; - +bool show_cameras_data = false; bool is_settings_gui = false; namespace photos @@ -1351,21 +1351,6 @@ void settings_gui() ImGui::PopItemWidth(); - 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; - - if (laz_files.size() > 0) - { - if (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); - } - } - ImGui::BeginDisabled(!is_init); if (ImGui::Button("Save point cloud")) { @@ -1715,6 +1700,14 @@ void display() 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()) { { @@ -1798,6 +1791,7 @@ void display() 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::EndDisabled(); ImGui::Separator(); @@ -1901,7 +1895,7 @@ void display() if (compass_ruler) drawMiniCompassWithRuler(); - if (photos::photo_texture_cam0) + if (show_cameras_data && photos::photo_texture_cam0) { if (ImGui::Begin("CAM0")) { @@ -1994,7 +1988,7 @@ int main(int argc, char* argv[]) if (fs::exists(argv[1])) { - for (const auto& entry : fs::directory_iterator(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());