From d73283fff9df47cf04d620a290bbc5fb970ea18d Mon Sep 17 00:00:00 2001 From: Jakubach Date: Sun, 8 Mar 2026 23:42:01 +0100 Subject: [PATCH 1/2] Restore per-worker VQF orientation estimation for methods 5-7 Re-add estimate_orientations() that was removed in PR #392. Methods 5-7 (VQF velocity) now use a local VQF instance per worker to estimate orientations from IMU data, instead of reusing the global VQF orientations. Also restore missing imu_utils function definitions (safe_dt, has_nan, is_accel_valid, convert_gyro_to_rads). --- core/include/imu_preintegration.h | 4 ++ core/src/imu_preintegration.cpp | 102 ++++++++++++++++++++++++++++-- 2 files changed, 99 insertions(+), 7 deletions(-) diff --git a/core/include/imu_preintegration.h b/core/include/imu_preintegration.h index 554f3994..12be5707 100644 --- a/core/include/imu_preintegration.h +++ b/core/include/imu_preintegration.h @@ -22,6 +22,10 @@ Eigen::Vector3d convert_gyro_to_rads(const Eigen::Vector3d& raw, bool units_in_d double safe_dt(double t_prev, double t_curr, double max_dt); bool has_nan(const Eigen::Vector3d& v); bool is_accel_valid(const Eigen::Vector3d& accel_ms2, double threshold); +std::vector estimate_orientations( + const std::vector& raw_imu_data, + const Eigen::Matrix3d& initial_orientation, + const IntegrationParams& params); } // namespace imu_utils class AccelerationModel diff --git a/core/src/imu_preintegration.cpp b/core/src/imu_preintegration.cpp index fecb7efe..f7e0c592 100644 --- a/core/src/imu_preintegration.cpp +++ b/core/src/imu_preintegration.cpp @@ -4,15 +4,96 @@ #include #include +#include + namespace imu_utils { - Eigen::Vector3d convert_accel_to_ms2(const Eigen::Vector3d& raw, bool units_in_g, double g) +Eigen::Vector3d convert_accel_to_ms2(const Eigen::Vector3d& raw, bool units_in_g, double g) +{ + if (units_in_g) + return raw * g; + return raw; +} + +Eigen::Vector3d convert_gyro_to_rads(const Eigen::Vector3d& raw, bool units_in_deg) +{ + if (units_in_deg) + return raw * (M_PI / 180.0); + return raw; +} + +double safe_dt(double t_prev, double t_curr, double max_dt) +{ + double dt = t_curr - t_prev; + if (dt <= 0.0 || std::isnan(dt)) + return 0.0; + return std::min(dt, max_dt); +} + +bool has_nan(const Eigen::Vector3d& v) +{ + return std::isnan(v.x()) || std::isnan(v.y()) || std::isnan(v.z()); +} + +bool is_accel_valid(const Eigen::Vector3d& accel_ms2, double threshold) +{ + return accel_ms2.norm() < threshold && !has_nan(accel_ms2); +} + +std::vector estimate_orientations( + const std::vector& raw_imu_data, + const Eigen::Matrix3d& initial_orientation, + const IntegrationParams& params) +{ + std::vector orientations; + orientations.reserve(raw_imu_data.size()); + + // Compute average dt for VQF initialization + double avg_dt = 1.0 / 200.0; // default 200 Hz + if (raw_imu_data.size() >= 2) + { + double total_time = raw_imu_data.back().timestamp - raw_imu_data.front().timestamp; + if (total_time > 0.0) + avg_dt = total_time / static_cast(raw_imu_data.size() - 1); + } + + VQFParams vqf_params; + vqf_params.tauAcc = params.vqf_tauAcc > 0.0 ? params.vqf_tauAcc : 3.0; + VQF vqf(vqf_params, avg_dt); + + orientations.push_back(initial_orientation); + + for (size_t k = 1; k < raw_imu_data.size(); k++) { - if (units_in_g) - return raw * g; - return raw; + double dt = safe_dt(raw_imu_data[k - 1].timestamp, raw_imu_data[k].timestamp, params.max_dt_threshold); + if (dt == 0.0) + { + orientations.push_back(orientations.back()); + continue; + } + + // VQF expects: gyro in rad/s, acc in m/s² + // RawIMUData: gyro in rad/s, accel in g + const double g = 9.81; + vqf_real_t gyr[3] = { + raw_imu_data[k].guroscopes.x(), + raw_imu_data[k].guroscopes.y(), + raw_imu_data[k].guroscopes.z() }; + vqf_real_t acc[3] = { + raw_imu_data[k].accelerometers.x() * g, + raw_imu_data[k].accelerometers.y() * g, + raw_imu_data[k].accelerometers.z() * g }; + + vqf.update(gyr, acc); + + vqf_real_t quat[4]; // [w, x, y, z] + vqf.getQuat6D(quat); + Eigen::Quaterniond q(quat[0], quat[1], quat[2], quat[3]); + orientations.push_back(q.toRotationMatrix()); } + return orientations; +} } // namespace imu_utils @@ -223,8 +304,14 @@ Eigen::Vector3d ImuPreintegration::create_and_preintegrate( case PreintegrationMethod::trapezoidal_gravity_vqf_vel: case PreintegrationMethod::kalman_gravity_vqf_vel: { - // Use orientations from new_trajectory directly (global VQF from calculate_trajectory, - // anchored to previous worker's SM pose). No per-worker VQF needed. + // Per-worker VQF: estimate local orientations from IMU data + auto orientations = imu_utils::estimate_orientations( + raw_imu_data, new_trajectory[0].rotation(), params); + + std::vector imu_trajectory = new_trajectory; + for (size_t k = 0; k < imu_trajectory.size() && k < orientations.size(); k++) + imu_trajectory[k].linear() = orientations[k]; + accel_model = std::make_unique(); if (method == PreintegrationMethod::euler_gravity_vqf_vel) integration_method = std::make_unique(); @@ -232,7 +319,8 @@ Eigen::Vector3d ImuPreintegration::create_and_preintegrate( integration_method = std::make_unique(); else integration_method = std::make_unique(); - break; + + return preint.preintegrate(raw_imu_data, imu_trajectory, *accel_model, *integration_method); } default: std::cerr << "ImuPreintegration: unknown method " << static_cast(method) << std::endl; From 5eb419e6a78618a3761cb513f86aac52db8a29eb Mon Sep 17 00:00:00 2001 From: Jakubach Date: Sun, 8 Mar 2026 23:49:46 +0100 Subject: [PATCH 2/2] Pass full VQF config to per-worker VQF in estimate_orientations Local VQF now uses the same parameters as global VQF (from GUI/TOML) instead of hardcoded defaults. Only tauAcc was passed before. --- .../lidar_odometry_utils_optimizers.cpp | 2 +- core/include/imu_preintegration.h | 7 +++++-- core/src/imu_preintegration.cpp | 10 +++++----- 3 files changed, 11 insertions(+), 8 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 86813c99..f4caecf7 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -2456,7 +2456,7 @@ bool process_worker_step_1( } mean_shift = ImuPreintegration::create_and_preintegrate( - method, worker_data.raw_imu_data, new_trajectory, imu_params); + method, worker_data.raw_imu_data, new_trajectory, imu_params, buildVQFParams(params)); } else { diff --git a/core/include/imu_preintegration.h b/core/include/imu_preintegration.h index 12be5707..876a1163 100644 --- a/core/include/imu_preintegration.h +++ b/core/include/imu_preintegration.h @@ -4,6 +4,7 @@ #include #include #include +#include struct IntegrationParams { @@ -25,7 +26,8 @@ bool is_accel_valid(const Eigen::Vector3d& accel_ms2, double threshold); std::vector estimate_orientations( const std::vector& raw_imu_data, const Eigen::Matrix3d& initial_orientation, - const IntegrationParams& params); + const IntegrationParams& params, + const VQFParams& vqf_params = VQFParams()); } // namespace imu_utils class AccelerationModel @@ -141,5 +143,6 @@ class ImuPreintegration PreintegrationMethod method, const std::vector& raw_imu_data, const std::vector& new_trajectory, - const IntegrationParams& params = IntegrationParams()); + const IntegrationParams& params = IntegrationParams(), + const VQFParams& vqf_params = VQFParams()); }; diff --git a/core/src/imu_preintegration.cpp b/core/src/imu_preintegration.cpp index f7e0c592..5e200ee5 100644 --- a/core/src/imu_preintegration.cpp +++ b/core/src/imu_preintegration.cpp @@ -44,7 +44,8 @@ bool is_accel_valid(const Eigen::Vector3d& accel_ms2, double threshold) std::vector estimate_orientations( const std::vector& raw_imu_data, const Eigen::Matrix3d& initial_orientation, - const IntegrationParams& params) + const IntegrationParams& params, + const VQFParams& vqf_params) { std::vector orientations; orientations.reserve(raw_imu_data.size()); @@ -58,8 +59,6 @@ std::vector estimate_orientations( avg_dt = total_time / static_cast(raw_imu_data.size() - 1); } - VQFParams vqf_params; - vqf_params.tauAcc = params.vqf_tauAcc > 0.0 ? params.vqf_tauAcc : 3.0; VQF vqf(vqf_params, avg_dt); orientations.push_back(initial_orientation); @@ -270,7 +269,8 @@ Eigen::Vector3d ImuPreintegration::create_and_preintegrate( PreintegrationMethod method, const std::vector& raw_imu_data, const std::vector& new_trajectory, - const IntegrationParams& params) + const IntegrationParams& params, + const VQFParams& vqf_params) { ImuPreintegration preint; preint.params = params; @@ -306,7 +306,7 @@ Eigen::Vector3d ImuPreintegration::create_and_preintegrate( { // Per-worker VQF: estimate local orientations from IMU data auto orientations = imu_utils::estimate_orientations( - raw_imu_data, new_trajectory[0].rotation(), params); + raw_imu_data, new_trajectory[0].rotation(), params, vqf_params); std::vector imu_trajectory = new_trajectory; for (size_t k = 0; k < imu_trajectory.size() && k < orientations.size(); k++)