From 5bb1111c9bdc7a2c09f04db2f6bcf634280cbf38 Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Mon, 28 Sep 2026 23:16:42 +0200 Subject: [PATCH 1/3] Add IMU dt sanity checks before VQF init Warn when the average IMU sample period is off from 200 Hz and fall back to 200 Hz for sessions spanning more than 24 h. Applied in step 1 and the raw data viewer. Remove unused SAMPLE_PERIOD defines. Co-Authored-By: Claude Opus 5.5 --- ..._data_and_drop_here-precision_forestry.cpp | 2 -- apps/lidar_odometry_step_1/lidar_odometry.cpp | 19 +++++++++++--- apps/lidar_odometry_step_1/lidar_odometry.h | 2 -- .../lidar_odometry_gui.cpp | 2 -- .../livox_mid_360_intrinsic_calibration.cpp | 1 - .../mandeye_mission_recorder_calibration.cpp | 1 - .../mandeye_raw_data_viewer.cpp | 25 +++++++++++++++---- .../mandeye_single_session_viewer.cpp | 1 - 8 files changed, 36 insertions(+), 17 deletions(-) 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 113030ab..9d47f5b7 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 @@ -39,8 +39,6 @@ // 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. -// #define SAMPLE_PERIOD (1.0 / 200.0) - std::string winTitle = std::string("drag_folder_with_mandeye_data_and_drop_here-precision_forestry ") + HDMAPPING_VERSION_STRING; std::vector infoLines = { diff --git a/apps/lidar_odometry_step_1/lidar_odometry.cpp b/apps/lidar_odometry_step_1/lidar_odometry.cpp index e7de94fd..7a1f0653 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry.cpp @@ -317,10 +317,23 @@ void calculate_trajectory(Trajectory& trajectory, Imu& imu_data, LidarOdometryPa 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; + 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 = (t1 - t0) / static_cast(imu_data.size() - 1); + avg_dt = duration / static_cast(imu_data.size() - 1); + + if (duration > 24 * 60 * 60) + { + std::cerr << "ERROR: Session is absurdly long : start time : " << t0 << ", end time " << t1 << std::endl; + std::cerr << "ERROR: Setting rate to " << 1.0 / 200.0 << std::endl; + avg_dt = 1.0 / 200.0; + } + } + if (std::fabs(avg_dt - 1.0 / 200.0) > 0.01) + { + std::cerr << "WARNING: The dt found is strange : " << avg_dt << std::endl; } VQFParams vqf_params = buildVQFParams(params); diff --git a/apps/lidar_odometry_step_1/lidar_odometry.h b/apps/lidar_odometry_step_1/lidar_odometry.h index 4035ab6f..8a64aa11 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.h +++ b/apps/lidar_odometry_step_1/lidar_odometry.h @@ -12,8 +12,6 @@ #include #include -// #define SAMPLE_PERIOD (1.0 / 200.0) - using Trajectory = std::map>; using Imu = std::vector, Eigen::Vector3f, Eigen::Vector3f>>; diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp index e7378fef..73a55901 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp @@ -38,8 +38,6 @@ // 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. -// #define SAMPLE_PERIOD (1.0 / 200.0) - std::string winTitle = std::string("Step 1 (Lidar odometry) ") + HDMAPPING_VERSION_STRING; std::vector infoLines = { diff --git a/apps/livox_mid_360_intrinsic_calibration/livox_mid_360_intrinsic_calibration.cpp b/apps/livox_mid_360_intrinsic_calibration/livox_mid_360_intrinsic_calibration.cpp index 00f5e39a..b5c63842 100644 --- a/apps/livox_mid_360_intrinsic_calibration/livox_mid_360_intrinsic_calibration.cpp +++ b/apps/livox_mid_360_intrinsic_calibration/livox_mid_360_intrinsic_calibration.cpp @@ -30,7 +30,6 @@ #include -#define SAMPLE_PERIOD (1.0 / 200.0) namespace fs = std::filesystem; const uint32_t window_width = 800; diff --git a/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp b/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp index ab54ee17..7fbc53ca 100644 --- a/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp +++ b/apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp @@ -46,7 +46,6 @@ std::vector infoLines = { "This program is optional step in MANDEYE // 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); 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 d72b409b..6b1ba00f 100644 --- a/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp +++ b/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp @@ -1104,16 +1104,31 @@ void loadFiles(std::vector input_file_names) // VQF initialization double avg_dt = SAMPLE_PERIOD; - /*if (imu_data.size() >= 2) + if (imu_data.size() >= 2) { - double t0 = std::get<0>(imu_data.front()).first; - double t1 = std::get<0>(imu_data.back()).first; + 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 = (t1 - t0) / static_cast(imu_data.size() - 1); - }*/ + avg_dt = duration / static_cast(imu_data.size() - 1); + + if (duration > 24 * 60 * 60) + { + spdlog::error("Session is absrudly 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; diff --git a/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp b/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp index fa339884..508785b6 100644 --- a/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp +++ b/apps/mandeye_single_session_viewer/mandeye_single_session_viewer.cpp @@ -126,7 +126,6 @@ static const std::vector appShortcuts = { { "Normal keys", "A", " { "", "Ctrl + right click", "" }, { "", "Ctrl + middle click", "" } }; -#define SAMPLE_PERIOD (1.0 / 200.0) namespace fs = std::filesystem; ImVec4 pc_neigbouring_color = ImVec4(0.5f, 0.5f, 0.5f, 1.0f); From caea7fd465b70fba505cb041b6c2d53056877a40 Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Mon, 28 Sep 2026 23:18:32 +0200 Subject: [PATCH 2/3] Fix typo in raw data viewer log message Co-Authored-By: Claude Opus 5.5 --- apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 6b1ba00f..aee200bd 100644 --- a/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp +++ b/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp @@ -1115,7 +1115,7 @@ void loadFiles(std::vector input_file_names) if (duration > 24 * 60 * 60) { - spdlog::error("Session is absrudly long : start time : {}, end time {}", t0, t1); + spdlog::error("Session is absurdly long : start time : {}, end time {}", t0, t1); spdlog::error("Setting rate to {}", SAMPLE_PERIOD); avg_dt = SAMPLE_PERIOD; } From 9af0c3403cfe2098422f97533fdc87a9cd7a14e5 Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Tue, 29 Sep 2026 00:20:27 +0200 Subject: [PATCH 3/3] make clang happy Signed-off-by: Michal Pelka --- .../lidar_odometry_utils_optimizers.cpp | 17 +++++++++-------- core/src/imu_preintegration.cpp | 2 +- 2 files changed, 10 insertions(+), 9 deletions(-) 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 8bdd3a59..e86efcab 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -2301,22 +2301,23 @@ bool process_worker_step_update_rgd_after( double translation_change = (current_m.translation() - last_m.translation()).norm(); - //std::cout << "translation_change: " << translation_change << std::endl; + // std::cout << "translation_change: " << translation_change << std::endl; if (translation_change < params.in_out_params_indoor.resolution_X * 0.5) { - //std::cout << "skipping update_rgd_hierarchy due to small translation change" << std::endl; + // std::cout << "skipping update_rgd_hierarchy due to small translation change" << std::endl; return true; } Eigen::Affine3d m_rot = worker_data.intermediate_trajectory[0].inverse() * worker_data.intermediate_trajectory.back(); - + TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(m_rot); - //std::cout << "m_rot: " << pose.om * 180.0 / M_PI << ", " << pose.fi * 180.0 / M_PI << ", " << pose.ka * 180.0 / M_PI - // << std::endl; + // std::cout << "m_rot: " << pose.om * 180.0 / M_PI << ", " << pose.fi * 180.0 / M_PI << ", " << pose.ka * 180.0 / M_PI + // << std::endl; - if (fabs(pose.om * 180.0 / M_PI) > 5.0 || fabs(pose.fi * 180.0 / M_PI) > 5.0 || fabs(pose.ka * 180.0 / M_PI) > 5.0){ - //std::cout << "skipping update_rgd_hierarchy due to large rotation change" << std::endl; - return true; + if (fabs(pose.om * 180.0 / M_PI) > 5.0 || fabs(pose.fi * 180.0 / M_PI) > 5.0 || fabs(pose.ka * 180.0 / M_PI) > 5.0) + { + // std::cout << "skipping update_rgd_hierarchy due to large rotation change" << std::endl; + return true; } update_rgd_hierarchy( diff --git a/core/src/imu_preintegration.cpp b/core/src/imu_preintegration.cpp index aac3a549..24901071 100644 --- a/core/src/imu_preintegration.cpp +++ b/core/src/imu_preintegration.cpp @@ -134,7 +134,7 @@ namespace imu_utils static_cast(raw_imu_data[k].accelerometers.z()) }; FusionAhrsUpdateNoMagnetometer(&fusion_ahrs, gyroscope, accelerometer, static_cast(dt)); - + FusionQuaternion quat = FusionAhrsGetQuaternion(&fusion_ahrs); Eigen::Quaterniond q(quat.element.w, quat.element.x, quat.element.y, quat.element.z); orientations.push_back(q.toRotationMatrix());