From c750f2422f024a0538c6aa0e70871635b0537080 Mon Sep 17 00:00:00 2001 From: Jakubach Date: Tue, 24 Feb 2026 00:54:51 +0100 Subject: [PATCH 01/11] Add IMU preintegration with multiple integration methods --- .../lidar_odometry_gui.cpp | 7 + .../lidar_odometry_utils.h | 4 + .../lidar_odometry_utils_optimizers.cpp | 64 +++-- apps/lidar_odometry_step_1/toml_io.h | 5 +- core/CMakeLists.txt | 1 + core/include/imu_preintegration.h | 143 ++++++++++ core/src/imu_preintegration.cpp | 257 ++++++++++++++++++ 7 files changed, 462 insertions(+), 19 deletions(-) create mode 100644 core/include/imu_preintegration.h create mode 100644 core/src/imu_preintegration.cpp diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp index 363735ab..e6fa9137 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp @@ -936,6 +936,13 @@ void settings_gui() ImGui::NewLine(); ImGui::Checkbox("Use motion from previous step", ¶ms.use_motion_from_previous_step); + ImGui::Checkbox("Use IMU preintegration", ¶ms.use_imu_preintegration); + if (params.use_imu_preintegration) + { + const char* methods[] = { "Euler (body frame)", "Trapezoidal (body frame)", + "Euler (gravity compensated)", "Trapezoidal (gravity compensated)", "Kalman filter" }; + ImGui::Combo("IMU preintegration method", ¶ms.imu_preintegration_method, methods, IM_ARRAYSIZE(methods)); + } ImGui::InputDouble("AHRS gain", ¶ms.ahrs_gain, 0.0, 0.0, "%.3f"); if (ImGui::IsItemHovered()) { diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.h b/apps/lidar_odometry_step_1/lidar_odometry_utils.h index 34a3ae72..82f4bce3 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.h +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.h @@ -154,6 +154,10 @@ struct LidarOdometryParams bool use_removie_imu_bias_from_first_stationary_scan = false; + // IMU preintegration + bool use_imu_preintegration = true; + int imu_preintegration_method = 3; // 0=euler_body, 1=trapezoidal_body, 2=euler_gravity, 3=trapezoidal_gravity, 4=kalman + // ablation study bool ablation_study_use_planarity = false; bool ablation_study_use_norm = false; 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 bedb70e3..eb632da7 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -1,5 +1,6 @@ #include "lidar_odometry_utils.h" #include +#include #include #include #include @@ -2357,6 +2358,24 @@ bool process_worker_step_1( { UTL_PROFILER_SCOPE("process_worker_step_1"); + { + static int last_logged_method = -2; + int current_state = params.use_imu_preintegration ? params.imu_preintegration_method : -1; + if (current_state != last_logged_method) + { + if (params.use_imu_preintegration) + { + auto method = static_cast(params.imu_preintegration_method); + spdlog::info("IMU preintegration enabled with method: {}", to_string(method)); + } + else + { + spdlog::info("IMU preintegration disabled"); + } + last_logged_method = current_state; + } + } + Eigen::Vector3d mean_shift(0.0, 0.0, 0.0); if (i > 1 && params.use_motion_from_previous_step) { @@ -2394,34 +2413,43 @@ bool process_worker_step_1( // 2].intermediate_trajectory[worker_data[i - 2].intermediate_trajectory.size() - 1].translation(); mean_shift /= // ((worker_data[i - 2].intermediate_trajectory.size()) - 2); - bool use_imu_preintegtation = false; - - if (use_imu_preintegtation) + if (params.use_imu_preintegration) { - // change mean_shift with preintegrated IMU data - // use rotation from std::vector new_trajectory; - // new_trajectory.size() == worker_data[i].raw_imu_data.size(); - // mean_shift = preintegrate_imu(worker_data[i].raw_imu_data); ToDo + IntegrationParams imu_params; + + // Estimate initial velocity from previous trajectory segments + Eigen::Vector3d prev_displacement = + prev_worker_data.intermediate_trajectory[prev_worker_data.intermediate_trajectory.size() - 1].translation() - + prev_prev_worker_data.intermediate_trajectory[prev_prev_worker_data.intermediate_trajectory.size() - 1].translation(); + + if (worker_data.raw_imu_data.size() >= 2) + { + double total_imu_time = worker_data.raw_imu_data.back().timestamp - worker_data.raw_imu_data.front().timestamp; + if (total_imu_time > 0.0) + imu_params.initial_velocity = prev_displacement / total_imu_time; + } + + mean_shift = ImuPreintegration::create_and_preintegrate( + static_cast(params.imu_preintegration_method), + worker_data.raw_imu_data, new_trajectory, imu_params); } else { mean_shift = prev_worker_data.intermediate_trajectory[prev_worker_data.intermediate_trajectory.size() - 1].translation() - prev_prev_worker_data.intermediate_trajectory[prev_prev_worker_data.intermediate_trajectory.size() - 1].translation(); - // mean_shift = worker_data[i - 1].intermediate_trajectory[0].translation() - - // worker_data[i - 2].intermediate_trajectory[0].translation(); mean_shift /= (prev_worker_data.intermediate_trajectory.size()); + } - if (mean_shift.norm() > 1.0) - { - spdlog::warn("mean_shift.norm() > 1.0"); - mean_shift = Eigen::Vector3d(0.0, 0.0, 0.0); - } + if (mean_shift.norm() > 1.0) + { + spdlog::warn("mean_shift.norm() > 1.0, resetting to zero (was: {})", mean_shift.norm()); + mean_shift = Eigen::Vector3d(0.0, 0.0, 0.0); + } - for (int tr = 0; tr < new_trajectory.size(); tr++) - { - new_trajectory[tr].translation() += mean_shift * tr; - } + for (int tr = 0; tr < new_trajectory.size(); tr++) + { + new_trajectory[tr].translation() += mean_shift * tr; } worker_data.intermediate_trajectory = new_trajectory; diff --git a/apps/lidar_odometry_step_1/toml_io.h b/apps/lidar_odometry_step_1/toml_io.h index cf547736..c6f73a37 100644 --- a/apps/lidar_odometry_step_1/toml_io.h +++ b/apps/lidar_odometry_step_1/toml_io.h @@ -82,7 +82,9 @@ class TomlIO { "rgd_sf_sigma_ka_deg", &LidarOdometryParams::rgd_sf_sigma_ka_deg }, { "max_distance_lidar_rigid_sf", &LidarOdometryParams::max_distance_lidar_rigid_sf }, { "current_output_dir", &LidarOdometryParams::current_output_dir }, - { "working_directory_preview", &LidarOdometryParams::working_directory_preview } + { "working_directory_preview", &LidarOdometryParams::working_directory_preview }, + { "use_imu_preintegration", &LidarOdometryParams::use_imu_preintegration }, + { "imu_preintegration_method", &LidarOdometryParams::imu_preintegration_method } }; // Special handling for TaitBryanPose members @@ -144,6 +146,7 @@ class TomlIO "rgd_sf_sigma_om_deg", "rgd_sf_sigma_fi_deg", "rgd_sf_sigma_ka_deg" } }, + { "imu_preintegration", { "use_imu_preintegration", "imu_preintegration_method" } }, { "paths", { "current_output_dir", "working_directory_preview" } }, { "misc", { "clear_color" } } }; diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index d6109a6e..1e02b9ee 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -9,6 +9,7 @@ set(CORE_BASE_SOURCES src/ground_control_points.cpp src/hash_utils.cpp src/icp.cpp + src/imu_preintegration.cpp src/ndt.cpp src/nmea.cpp src/optimization_point_to_point_source_to_target.cpp diff --git a/core/include/imu_preintegration.h b/core/include/imu_preintegration.h new file mode 100644 index 00000000..5e800020 --- /dev/null +++ b/core/include/imu_preintegration.h @@ -0,0 +1,143 @@ +#pragma once + +#include +#include +#include +#include + +struct IntegrationParams +{ + bool accel_units_in_g = true; + bool gyro_units_in_deg_per_sec = true; + double max_acceleration_threshold = 50.0; // m/s^2 + double max_dt_threshold = 0.1; // seconds + Eigen::Vector3d initial_velocity = Eigen::Vector3d::Zero(); +}; + +namespace imu_utils +{ +Eigen::Vector3d convert_accel_to_ms2(const Eigen::Vector3d& raw, bool units_in_g, double g = 9.81); +Eigen::Vector3d convert_gyro_to_rads(const Eigen::Vector3d& raw, bool units_in_deg); +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); +} // namespace imu_utils + +class AccelerationModel +{ +public: + virtual ~AccelerationModel() = default; + + virtual Eigen::Vector3d compute( + const RawIMUData& imu, + const Eigen::Affine3d& pose, + const IntegrationParams& params) const = 0; +}; + +class BodyFrameAcceleration : public AccelerationModel +{ +public: + Eigen::Vector3d compute( + const RawIMUData& imu, + const Eigen::Affine3d& pose, + const IntegrationParams& params) const override; +}; + +class GravityCompensatedAcceleration : public AccelerationModel +{ +public: + double gravity_magnitude = 9.81; + Eigen::Vector3d gravity_vector = Eigen::Vector3d(0.0, 0.0, -9.81); + + Eigen::Vector3d compute( + const RawIMUData& imu, + const Eigen::Affine3d& pose, + const IntegrationParams& params) const override; +}; + +class IntegrationMethod +{ +public: + virtual ~IntegrationMethod() = default; + + virtual Eigen::Vector3d integrate( + const std::vector& raw_imu_data, + const std::vector& new_trajectory, + const AccelerationModel& accel_model, + const IntegrationParams& params) const = 0; +}; + +class EulerIntegration : public IntegrationMethod +{ +public: + Eigen::Vector3d integrate( + const std::vector& raw_imu_data, + const std::vector& new_trajectory, + const AccelerationModel& accel_model, + const IntegrationParams& params) const override; +}; + +class TrapezoidalIntegration : public IntegrationMethod +{ +public: + Eigen::Vector3d integrate( + const std::vector& raw_imu_data, + const std::vector& new_trajectory, + const AccelerationModel& accel_model, + const IntegrationParams& params) const override; +}; + +class KalmanFilterIntegration : public IntegrationMethod +{ +public: + double process_noise_accel = 0.5; + double process_noise_bias = 0.01; + double measurement_noise_accel = 1.0; + Eigen::Vector3d initial_accel_bias = Eigen::Vector3d::Zero(); + + Eigen::Vector3d integrate( + const std::vector& raw_imu_data, + const std::vector& new_trajectory, + const AccelerationModel& accel_model, + const IntegrationParams& params) const override; +}; + +enum class PreintegrationMethod +{ + euler_body_frame = 0, + trapezoidal_body_frame = 1, + euler_gravity_compensated = 2, + trapezoidal_gravity_compensated = 3, + kalman_filter = 4, +}; + +inline const char* to_string(PreintegrationMethod method) +{ + switch (method) + { + case PreintegrationMethod::euler_body_frame: return "Euler (body frame)"; + case PreintegrationMethod::trapezoidal_body_frame: return "Trapezoidal (body frame)"; + case PreintegrationMethod::euler_gravity_compensated: return "Euler (gravity compensated)"; + case PreintegrationMethod::trapezoidal_gravity_compensated: return "Trapezoidal (gravity compensated)"; + case PreintegrationMethod::kalman_filter: return "Kalman filter"; + default: return "unknown"; + } +} + +class ImuPreintegration +{ +public: + IntegrationParams params; + + Eigen::Vector3d preintegrate( + const std::vector& raw_imu_data, + const std::vector& new_trajectory, + const AccelerationModel& accel_model, + const IntegrationMethod& integration_method); + + static Eigen::Vector3d create_and_preintegrate( + PreintegrationMethod method, + const std::vector& raw_imu_data, + const std::vector& new_trajectory, + const IntegrationParams& params = IntegrationParams()); +}; diff --git a/core/src/imu_preintegration.cpp b/core/src/imu_preintegration.cpp new file mode 100644 index 00000000..ff263f3c --- /dev/null +++ b/core/src/imu_preintegration.cpp @@ -0,0 +1,257 @@ +#include + +#include +#include +#include + +namespace imu_utils +{ + +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); +} + +} // namespace imu_utils + +Eigen::Vector3d BodyFrameAcceleration::compute( + const RawIMUData& imu, + const Eigen::Affine3d& /*pose*/, + const IntegrationParams& params) const +{ + return imu_utils::convert_accel_to_ms2(imu.accelerometers, params.accel_units_in_g); +} + +Eigen::Vector3d GravityCompensatedAcceleration::compute( + const RawIMUData& imu, + const Eigen::Affine3d& pose, + const IntegrationParams& params) const +{ + Eigen::Vector3d a_body = imu_utils::convert_accel_to_ms2(imu.accelerometers, params.accel_units_in_g, gravity_magnitude); + Eigen::Matrix3d R = pose.rotation(); + return R * a_body + gravity_vector; +} + +// v_{k} = v_{k-1} + a_k * dt +// p_{k} = p_{k-1} + v_{k} * dt +Eigen::Vector3d EulerIntegration::integrate( + const std::vector& raw_imu_data, + const std::vector& new_trajectory, + const AccelerationModel& accel_model, + const IntegrationParams& params) const +{ + const size_t n = raw_imu_data.size(); + Eigen::Vector3d velocity = params.initial_velocity; + Eigen::Vector3d position = Eigen::Vector3d::Zero(); + + for (size_t k = 1; k < n; k++) + { + double dt = imu_utils::safe_dt(raw_imu_data[k - 1].timestamp, raw_imu_data[k].timestamp, params.max_dt_threshold); + if (dt == 0.0) + continue; + + Eigen::Vector3d accel = accel_model.compute(raw_imu_data[k], new_trajectory[k], params); + if (!imu_utils::is_accel_valid(accel, params.max_acceleration_threshold)) + accel = Eigen::Vector3d::Zero(); + + velocity += accel * dt; + position += velocity * dt; + } + + return position; +} + +// v_{k} = v_{k-1} + 0.5 * (a_{k-1} + a_k) * dt +// p_{k} = p_{k-1} + 0.5 * (v_{k-1} + v_k) * dt +Eigen::Vector3d TrapezoidalIntegration::integrate( + const std::vector& raw_imu_data, + const std::vector& new_trajectory, + const AccelerationModel& accel_model, + const IntegrationParams& params) const +{ + const size_t n = raw_imu_data.size(); + Eigen::Vector3d velocity = params.initial_velocity; + Eigen::Vector3d position = Eigen::Vector3d::Zero(); + + Eigen::Vector3d prev_accel = accel_model.compute(raw_imu_data[0], new_trajectory[0], params); + if (!imu_utils::is_accel_valid(prev_accel, params.max_acceleration_threshold)) + prev_accel = Eigen::Vector3d::Zero(); + + for (size_t k = 1; k < n; k++) + { + double dt = imu_utils::safe_dt(raw_imu_data[k - 1].timestamp, raw_imu_data[k].timestamp, params.max_dt_threshold); + if (dt == 0.0) + continue; + + Eigen::Vector3d curr_accel = accel_model.compute(raw_imu_data[k], new_trajectory[k], params); + if (!imu_utils::is_accel_valid(curr_accel, params.max_acceleration_threshold)) + curr_accel = prev_accel; + + Eigen::Vector3d prev_velocity = velocity; + velocity += 0.5 * (prev_accel + curr_accel) * dt; + position += 0.5 * (prev_velocity + velocity) * dt; + + prev_accel = curr_accel; + } + + return position; +} + +// EKF state: x = [position(3), velocity(3), accel_bias(3)]^T (9-dim) +// F = [I, I*dt, 0; 0, I, -I*dt; 0, 0, I], H = [0, 0, -I] +Eigen::Vector3d KalmanFilterIntegration::integrate( + const std::vector& raw_imu_data, + const std::vector& new_trajectory, + const AccelerationModel& accel_model, + const IntegrationParams& params) const +{ + const size_t n = raw_imu_data.size(); + + Eigen::Matrix state = Eigen::Matrix::Zero(); + state.segment<3>(0) = Eigen::Vector3d::Zero(); + state.segment<3>(3) = params.initial_velocity; + state.segment<3>(6) = initial_accel_bias; + + Eigen::Matrix P = Eigen::Matrix::Identity(); + P.block<3, 3>(0, 0) *= 0.01; + P.block<3, 3>(3, 3) *= 1.0; + P.block<3, 3>(6, 6) *= 0.1; + + double sigma_a = process_noise_accel; + double sigma_b = process_noise_bias; + + for (size_t k = 1; k < n; k++) + { + double dt = imu_utils::safe_dt(raw_imu_data[k - 1].timestamp, raw_imu_data[k].timestamp, params.max_dt_threshold); + if (dt == 0.0) + continue; + + Eigen::Vector3d accel = accel_model.compute(raw_imu_data[k], new_trajectory[k], params); + if (!imu_utils::is_accel_valid(accel, params.max_acceleration_threshold)) + accel = Eigen::Vector3d::Zero(); + + Eigen::Vector3d bias = state.segment<3>(6); + Eigen::Vector3d a_corrected = accel - bias; + + Eigen::Matrix F = Eigen::Matrix::Identity(); + F.block<3, 3>(0, 3) = Eigen::Matrix3d::Identity() * dt; + F.block<3, 3>(3, 6) = -Eigen::Matrix3d::Identity() * dt; + + Eigen::Matrix state_pred; + state_pred.segment<3>(0) = state.segment<3>(0) + state.segment<3>(3) * dt + 0.5 * a_corrected * dt * dt; + state_pred.segment<3>(3) = state.segment<3>(3) + a_corrected * dt; + state_pred.segment<3>(6) = state.segment<3>(6); + + Eigen::Matrix Q = Eigen::Matrix::Zero(); + Q.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity() * (sigma_a * sigma_a * dt * dt * dt * dt / 4.0); + Q.block<3, 3>(3, 3) = Eigen::Matrix3d::Identity() * (sigma_a * sigma_a * dt * dt); + Q.block<3, 3>(6, 6) = Eigen::Matrix3d::Identity() * (sigma_b * sigma_b * dt); + + Eigen::Matrix P_pred = F * P * F.transpose() + Q; + + Eigen::Matrix H = Eigen::Matrix::Zero(); + H.block<3, 3>(0, 6) = -Eigen::Matrix3d::Identity(); + + Eigen::Matrix3d R_meas = Eigen::Matrix3d::Identity() * (measurement_noise_accel * measurement_noise_accel); + Eigen::Matrix3d S = H * P_pred * H.transpose() + R_meas; + Eigen::Matrix K = P_pred * H.transpose() * S.inverse(); + + Eigen::Vector3d innovation = state_pred.segment<3>(6); + + state = state_pred + K * innovation; + P = (Eigen::Matrix::Identity() - K * H) * P_pred; + } + + return state.segment<3>(0); +} + +Eigen::Vector3d ImuPreintegration::preintegrate( + const std::vector& raw_imu_data, + const std::vector& new_trajectory, + const AccelerationModel& accel_model, + const IntegrationMethod& integration_method) +{ + if (raw_imu_data.size() < 2 || new_trajectory.size() < 2) + return Eigen::Vector3d::Zero(); + + if (raw_imu_data.size() != new_trajectory.size()) + return Eigen::Vector3d::Zero(); + + Eigen::Vector3d total_displacement = integration_method.integrate( + raw_imu_data, new_trajectory, accel_model, params); + + if (imu_utils::has_nan(total_displacement)) + return Eigen::Vector3d::Zero(); + + Eigen::Vector3d mean_shift = total_displacement / static_cast(raw_imu_data.size() - 1); + return mean_shift; +} + +Eigen::Vector3d ImuPreintegration::create_and_preintegrate( + PreintegrationMethod method, + const std::vector& raw_imu_data, + const std::vector& new_trajectory, + const IntegrationParams& params) +{ + ImuPreintegration preint; + preint.params = params; + + std::unique_ptr accel_model; + std::unique_ptr integration_method; + + switch (method) + { + case PreintegrationMethod::euler_body_frame: + accel_model = std::make_unique(); + integration_method = std::make_unique(); + break; + case PreintegrationMethod::trapezoidal_body_frame: + accel_model = std::make_unique(); + integration_method = std::make_unique(); + break; + case PreintegrationMethod::euler_gravity_compensated: + accel_model = std::make_unique(); + integration_method = std::make_unique(); + break; + case PreintegrationMethod::trapezoidal_gravity_compensated: + accel_model = std::make_unique(); + integration_method = std::make_unique(); + break; + case PreintegrationMethod::kalman_filter: + accel_model = std::make_unique(); + integration_method = std::make_unique(); + break; + default: + std::cerr << "ImuPreintegration: unknown method " << static_cast(method) << std::endl; + return Eigen::Vector3d::Zero(); + } + + return preint.preintegrate(raw_imu_data, new_trajectory, *accel_model, *integration_method); +} From 56e3440d02a0c1cf72ef9ebc5f76a1d32fac7f24 Mon Sep 17 00:00:00 2001 From: Jakubach Date: Tue, 24 Feb 2026 03:25:14 +0100 Subject: [PATCH 02/11] Add AHRS preintegration methods --- .../lidar_odometry_gui.cpp | 3 +- core/include/imu_preintegration.h | 11 +++ core/src/imu_preintegration.cpp | 71 +++++++++++++++++++ 3 files changed, 84 insertions(+), 1 deletion(-) diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp index e6fa9137..b9340a47 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp @@ -940,7 +940,8 @@ void settings_gui() if (params.use_imu_preintegration) { const char* methods[] = { "Euler (body frame)", "Trapezoidal (body frame)", - "Euler (gravity compensated)", "Trapezoidal (gravity compensated)", "Kalman filter" }; + "Euler (gravity compensated)", "Trapezoidal (gravity compensated)", "Kalman filter", + "Euler (AHRS)", "Trapezoidal (AHRS)", "Kalman (AHRS)" }; ImGui::Combo("IMU preintegration method", ¶ms.imu_preintegration_method, methods, IM_ARRAYSIZE(methods)); } ImGui::InputDouble("AHRS gain", ¶ms.ahrs_gain, 0.0, 0.0, "%.3f"); diff --git a/core/include/imu_preintegration.h b/core/include/imu_preintegration.h index 5e800020..b1dcff26 100644 --- a/core/include/imu_preintegration.h +++ b/core/include/imu_preintegration.h @@ -12,6 +12,7 @@ struct IntegrationParams double max_acceleration_threshold = 50.0; // m/s^2 double max_dt_threshold = 0.1; // seconds Eigen::Vector3d initial_velocity = Eigen::Vector3d::Zero(); + double ahrs_gain = 0.5; }; namespace imu_utils @@ -21,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 @@ -109,6 +114,9 @@ enum class PreintegrationMethod euler_gravity_compensated = 2, trapezoidal_gravity_compensated = 3, kalman_filter = 4, + euler_gyro_gravity_compensated = 5, + trapezoidal_gyro_gravity_compensated = 6, + kalman_gyro_gravity_compensated = 7, }; inline const char* to_string(PreintegrationMethod method) @@ -120,6 +128,9 @@ inline const char* to_string(PreintegrationMethod method) case PreintegrationMethod::euler_gravity_compensated: return "Euler (gravity compensated)"; case PreintegrationMethod::trapezoidal_gravity_compensated: return "Trapezoidal (gravity compensated)"; case PreintegrationMethod::kalman_filter: return "Kalman filter"; + case PreintegrationMethod::euler_gyro_gravity_compensated: return "Euler (AHRS)"; + case PreintegrationMethod::trapezoidal_gyro_gravity_compensated: return "Trapezoidal (AHRS)"; + case PreintegrationMethod::kalman_gyro_gravity_compensated: return "Kalman (AHRS)"; default: return "unknown"; } } diff --git a/core/src/imu_preintegration.cpp b/core/src/imu_preintegration.cpp index ff263f3c..98a091b8 100644 --- a/core/src/imu_preintegration.cpp +++ b/core/src/imu_preintegration.cpp @@ -39,6 +39,56 @@ 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()); + + Eigen::Matrix3d R = initial_orientation; + orientations.push_back(R); + + for (size_t k = 1; k < raw_imu_data.size(); k++) + { + 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(R); + continue; + } + + Eigen::Vector3d omega = convert_gyro_to_rads(raw_imu_data[k].guroscopes, params.gyro_units_in_deg_per_sec); + double angle = omega.norm() * dt; + if (angle > 1e-10) + { + Eigen::Vector3d axis = omega.normalized(); + R = R * Eigen::AngleAxisd(angle, axis).toRotationMatrix(); + } + + if (params.ahrs_gain > 0.0) + { + Eigen::Vector3d a_body = convert_accel_to_ms2(raw_imu_data[k].accelerometers, params.accel_units_in_g); + if (a_body.norm() > 1e-6) + { + Eigen::Vector3d a_norm = a_body.normalized(); + Eigen::Vector3d g_expected = R.transpose() * Eigen::Vector3d(0, 0, -1); + Eigen::Vector3d correction_axis = g_expected.cross(a_norm); + double correction_angle = std::asin(std::min(1.0, correction_axis.norm())); + if (correction_angle > 1e-10) + { + correction_axis.normalize(); + R = R * Eigen::AngleAxisd(params.ahrs_gain * correction_angle, correction_axis).toRotationMatrix(); + } + } + } + + orientations.push_back(R); + } + return orientations; +} + } // namespace imu_utils Eigen::Vector3d BodyFrameAcceleration::compute( @@ -248,6 +298,27 @@ Eigen::Vector3d ImuPreintegration::create_and_preintegrate( accel_model = std::make_unique(); integration_method = std::make_unique(); break; + case PreintegrationMethod::euler_gyro_gravity_compensated: + case PreintegrationMethod::trapezoidal_gyro_gravity_compensated: + case PreintegrationMethod::kalman_gyro_gravity_compensated: + { + 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_gyro_gravity_compensated) + integration_method = std::make_unique(); + else if (method == PreintegrationMethod::trapezoidal_gyro_gravity_compensated) + integration_method = std::make_unique(); + else + integration_method = std::make_unique(); + + return preint.preintegrate(raw_imu_data, imu_trajectory, *accel_model, *integration_method); + } default: std::cerr << "ImuPreintegration: unknown method " << static_cast(method) << std::endl; return Eigen::Vector3d::Zero(); From dc1b5eebe9a1e3f4c072be8dcc33425fec4be0fb Mon Sep 17 00:00:00 2001 From: Jakubach Date: Tue, 24 Feb 2026 03:36:44 +0100 Subject: [PATCH 03/11] Replace complementary filter with Fusion AHRS --- core/CMakeLists.txt | 7 +++-- core/src/imu_preintegration.cpp | 53 +++++++++++++++------------------ 2 files changed, 28 insertions(+), 32 deletions(-) diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 1e02b9ee..30ed1e02 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -46,7 +46,7 @@ function(add_core_target target_name with_gui) add_library(${target_name} STATIC ${SOURCES}) target_compile_definitions(${target_name} PRIVATE ${DEFINES}) - target_link_libraries(${target_name} PRIVATE ${PLATFORM_LASZIP_LIB} ${PLATFORM_MISCELLANEOUS_LIBS} spdlog::spdlog) + target_link_libraries(${target_name} PRIVATE ${PLATFORM_LASZIP_LIB} ${PLATFORM_MISCELLANEOUS_LIBS} spdlog::spdlog Fusion) target_include_directories(${target_name} PRIVATE include ${EIGEN3_INCLUDE_DIR} @@ -54,8 +54,9 @@ function(add_core_target target_name with_gui) ${THIRDPARTY_DIRECTORY}/json/include ${THIRDPARTY_DIRECTORY}/observation_equations/codes ${EXTERNAL_LIBRARIES_DIRECTORY}/include - ) - + ${THIRDPARTY_DIRECTORY}/Fusion/Fusion + ) + target_link_libraries(${target_name} PRIVATE PROJ::proj) if(${with_gui}) diff --git a/core/src/imu_preintegration.cpp b/core/src/imu_preintegration.cpp index 98a091b8..639b97a1 100644 --- a/core/src/imu_preintegration.cpp +++ b/core/src/imu_preintegration.cpp @@ -4,6 +4,8 @@ #include #include +#include + namespace imu_utils { @@ -47,44 +49,37 @@ std::vector estimate_orientations( std::vector orientations; orientations.reserve(raw_imu_data.size()); - Eigen::Matrix3d R = initial_orientation; - orientations.push_back(R); + FusionAhrs ahrs; + FusionAhrsInitialise(&ahrs); + ahrs.settings.convention = FusionConventionNwu; + ahrs.settings.gain = static_cast(params.ahrs_gain); + + orientations.push_back(initial_orientation); for (size_t k = 1; k < raw_imu_data.size(); k++) { 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(R); + orientations.push_back(orientations.back()); continue; } - Eigen::Vector3d omega = convert_gyro_to_rads(raw_imu_data[k].guroscopes, params.gyro_units_in_deg_per_sec); - double angle = omega.norm() * dt; - if (angle > 1e-10) - { - Eigen::Vector3d axis = omega.normalized(); - R = R * Eigen::AngleAxisd(angle, axis).toRotationMatrix(); - } - - if (params.ahrs_gain > 0.0) - { - Eigen::Vector3d a_body = convert_accel_to_ms2(raw_imu_data[k].accelerometers, params.accel_units_in_g); - if (a_body.norm() > 1e-6) - { - Eigen::Vector3d a_norm = a_body.normalized(); - Eigen::Vector3d g_expected = R.transpose() * Eigen::Vector3d(0, 0, -1); - Eigen::Vector3d correction_axis = g_expected.cross(a_norm); - double correction_angle = std::asin(std::min(1.0, correction_axis.norm())); - if (correction_angle > 1e-10) - { - correction_axis.normalize(); - R = R * Eigen::AngleAxisd(params.ahrs_gain * correction_angle, correction_axis).toRotationMatrix(); - } - } - } - - orientations.push_back(R); + // RawIMUData: gyro in deg/s, accel in g — matches Fusion expectations + const FusionVector gyroscope = { + static_cast(raw_imu_data[k].guroscopes.x()), + static_cast(raw_imu_data[k].guroscopes.y()), + static_cast(raw_imu_data[k].guroscopes.z()) }; + const FusionVector accelerometer = { + static_cast(raw_imu_data[k].accelerometers.x()), + static_cast(raw_imu_data[k].accelerometers.y()), + static_cast(raw_imu_data[k].accelerometers.z()) }; + + FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, static_cast(dt)); + + FusionQuaternion quat = FusionAhrsGetQuaternion(&ahrs); + Eigen::Quaterniond q(quat.element.w, quat.element.x, quat.element.y, quat.element.z); + orientations.push_back(q.toRotationMatrix()); } return orientations; } From 5c410609dd8787ae4e8265f3dc717f1a3f55c563 Mon Sep 17 00:00:00 2001 From: Jakubach Date: Tue, 3 Mar 2026 00:59:36 +0100 Subject: [PATCH 04/11] Fix Kalman filter: use velocity observation instead of bias observation --- core/include/imu_preintegration.h | 2 +- core/src/imu_preintegration.cpp | 9 +++++---- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/core/include/imu_preintegration.h b/core/include/imu_preintegration.h index b1dcff26..4b9880fb 100644 --- a/core/include/imu_preintegration.h +++ b/core/include/imu_preintegration.h @@ -97,7 +97,7 @@ class KalmanFilterIntegration : public IntegrationMethod public: double process_noise_accel = 0.5; double process_noise_bias = 0.01; - double measurement_noise_accel = 1.0; + double measurement_noise_velocity = 1.0; Eigen::Vector3d initial_accel_bias = Eigen::Vector3d::Zero(); Eigen::Vector3d integrate( diff --git a/core/src/imu_preintegration.cpp b/core/src/imu_preintegration.cpp index 639b97a1..52c059ac 100644 --- a/core/src/imu_preintegration.cpp +++ b/core/src/imu_preintegration.cpp @@ -170,7 +170,8 @@ Eigen::Vector3d TrapezoidalIntegration::integrate( } // EKF state: x = [position(3), velocity(3), accel_bias(3)]^T (9-dim) -// F = [I, I*dt, 0; 0, I, -I*dt; 0, 0, I], H = [0, 0, -I] +// F = [I, I*dt, 0; 0, I, -I*dt; 0, 0, I] +// Measurement: velocity constraint H = [0, I, 0], z = initial_velocity Eigen::Vector3d KalmanFilterIntegration::integrate( const std::vector& raw_imu_data, const std::vector& new_trajectory, @@ -222,13 +223,13 @@ Eigen::Vector3d KalmanFilterIntegration::integrate( Eigen::Matrix P_pred = F * P * F.transpose() + Q; Eigen::Matrix H = Eigen::Matrix::Zero(); - H.block<3, 3>(0, 6) = -Eigen::Matrix3d::Identity(); + H.block<3, 3>(0, 3) = Eigen::Matrix3d::Identity(); - Eigen::Matrix3d R_meas = Eigen::Matrix3d::Identity() * (measurement_noise_accel * measurement_noise_accel); + Eigen::Matrix3d R_meas = Eigen::Matrix3d::Identity() * (measurement_noise_velocity * measurement_noise_velocity); Eigen::Matrix3d S = H * P_pred * H.transpose() + R_meas; Eigen::Matrix K = P_pred * H.transpose() * S.inverse(); - Eigen::Vector3d innovation = state_pred.segment<3>(6); + Eigen::Vector3d innovation = params.initial_velocity - state_pred.segment<3>(3); state = state_pred + K * innovation; P = (Eigen::Matrix::Identity() - K * H) * P_pred; From f8f19fed954aa5dfa773c0ac25b857ae63c10c0b Mon Sep 17 00:00:00 2001 From: Jakubach Date: Sun, 8 Mar 2026 01:28:36 +0100 Subject: [PATCH 05/11] Replace Fusion AHRS with VQF orientation estimation Swap out the Fusion complementary filter for VQF (Versatile Quaternion-based Filter) across all apps and core IMU preintegration. VQF provides better out-of-the-box accuracy with gyroscope bias estimation and a more robust inclination correction approach. --- CMakeLists.txt | 2 +- apps/compare_trajectories/CMakeLists.txt | 4 +- apps/concatenate_multi_livox/CMakeLists.txt | 2 +- .../concatenate_multi_livox.cpp | 4 +- apps/lidar_odometry_step_1/CMakeLists.txt | 8 +- ..._data_and_drop_here-precision_forestry.cpp | 2 +- apps/lidar_odometry_step_1/lidar_odometry.cpp | 119 ++++++------------ apps/lidar_odometry_step_1/lidar_odometry.h | 2 +- .../lidar_odometry_gui.cpp | 77 +++++------- .../lidar_odometry_utils.cpp | 37 +++--- .../lidar_odometry_utils.h | 4 +- .../CMakeLists.txt | 4 +- .../CMakeLists.txt | 4 +- apps/mandeye_raw_data_viewer/CMakeLists.txt | 4 +- .../mandeye_raw_data_viewer.cpp | 118 +++++++---------- .../CMakeLists.txt | 4 +- .../CMakeLists.txt | 2 +- apps/quick_start_demo/CMakeLists.txt | 4 +- apps/quick_start_demo/quick_start_demo.cpp | 47 ++++--- core/CMakeLists.txt | 4 +- core/src/imu_preintegration.cpp | 50 +++++--- pybind/CMakeLists.txt | 12 +- 22 files changed, 212 insertions(+), 302 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a77873c9..e7a5962d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -434,7 +434,7 @@ else() endif() -add_subdirectory(${THIRDPARTY_DIRECTORY}/Fusion/Fusion) +add_subdirectory(${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp) add_subdirectory(core) add_subdirectory(core_hd_mapping) diff --git a/apps/compare_trajectories/CMakeLists.txt b/apps/compare_trajectories/CMakeLists.txt index afc1b698..862882bf 100644 --- a/apps/compare_trajectories/CMakeLists.txt +++ b/apps/compare_trajectories/CMakeLists.txt @@ -25,11 +25,11 @@ target_include_directories( ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master ${LASZIP_INCLUDE_DIR}/LASzip/include ${THIRDPARTY_DIRECTORY}/observation_equations/codes - ${THIRDPARTY_DIRECTORY}/Fusion/Fusion) + ${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp) target_link_libraries( mandeye_compare_trajectories - PRIVATE Fusion + PRIVATE vqf unordered_dense::unordered_dense spdlog::spdlog ${FREEGLUT_LIBRARY} diff --git a/apps/concatenate_multi_livox/CMakeLists.txt b/apps/concatenate_multi_livox/CMakeLists.txt index ce887016..644a9b62 100644 --- a/apps/concatenate_multi_livox/CMakeLists.txt +++ b/apps/concatenate_multi_livox/CMakeLists.txt @@ -20,7 +20,7 @@ target_include_directories( ${LASZIP_INCLUDE_DIR}/LASzip/include ${THIRDPARTY_DIRECTORY}/glew-cmake/include ${THIRDPARTY_DIRECTORY}/observation_equations/codes - ${THIRDPARTY_DIRECTORY}/Fusion/Fusion + ${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp ${FREEGLUT_INCLUDE_DIR}) target_link_libraries(concatenate_multi_livox PRIVATE core diff --git a/apps/concatenate_multi_livox/concatenate_multi_livox.cpp b/apps/concatenate_multi_livox/concatenate_multi_livox.cpp index 7022a5ef..d059c1d5 100644 --- a/apps/concatenate_multi_livox/concatenate_multi_livox.cpp +++ b/apps/concatenate_multi_livox/concatenate_multi_livox.cpp @@ -139,8 +139,8 @@ int main(int argc, char* argv[]) for (const auto& [timestamp, gyro, accel] : imu) { out << static_cast(1e9 * timestamp.first) << " " << static_cast(1e9 * timestamp.second) << " " - << accel.axis.x << " " << accel.axis.y << " " << accel.axis.z << " " << gyro.axis.x << " " << gyro.axis.y << " " - << gyro.axis.z << "\n"; + << accel.x() << " " << accel.y() << " " << accel.z() << " " << gyro.x() << " " << gyro.y() << " " + << gyro.z() << "\n"; } std::cout << "Saved IMU data to: " << output_path_csv.string() << std::endl; } diff --git a/apps/lidar_odometry_step_1/CMakeLists.txt b/apps/lidar_odometry_step_1/CMakeLists.txt index 341a7a3f..68ac48c0 100644 --- a/apps/lidar_odometry_step_1/CMakeLists.txt +++ b/apps/lidar_odometry_step_1/CMakeLists.txt @@ -42,11 +42,11 @@ target_include_directories( ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master ${LASZIP_INCLUDE_DIR}/LASzip/include ${THIRDPARTY_DIRECTORY}/observation_equations/codes - ${THIRDPARTY_DIRECTORY}/Fusion/Fusion) + ${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp) target_link_libraries( lidar_odometry_step_1 - PRIVATE Fusion + PRIVATE vqf unordered_dense::unordered_dense spdlog::spdlog UTL::include @@ -99,11 +99,11 @@ target_include_directories( ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master ${LASZIP_INCLUDE_DIR}/LASzip/include ${THIRDPARTY_DIRECTORY}/observation_equations/codes - ${THIRDPARTY_DIRECTORY}/Fusion/Fusion) + ${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp) target_link_libraries( drag_folder_with_mandeye_data_and_drop_here-precision_forestry - PRIVATE Fusion + PRIVATE vqf unordered_dense::unordered_dense spdlog::spdlog UTL::include 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 0e2608bb..16b5e465 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 @@ -730,7 +730,7 @@ void settings_gui() if (fusionConvention < 0 || fusionConvention > 2) fusionConvention = 0; - ImGui::Text("Fusion convention: "); + ImGui::Text("AHRS convention: "); if (ImGui::IsItemHovered()) ImGui::SetTooltip( "Coordinate system conventions for sensor fusion defining how the axes are oriented relative to world frame"); diff --git a/apps/lidar_odometry_step_1/lidar_odometry.cpp b/apps/lidar_odometry_step_1/lidar_odometry.cpp index acc02811..45ff8b39 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry.cpp @@ -7,8 +7,6 @@ namespace fs = std::filesystem; -const float RAD_TO_DEG = 180.0f / static_cast(M_PI); - bool load_data( std::vector& input_file_names, LidarOdometryParams& params, @@ -207,7 +205,7 @@ bool load_data( std::size_t minSize = std::min(laz_files.size(), csv_files.size()); // Each thread loads into its own local vector - std::vector, FusionVector, FusionVector>>> imuChunks(minSize); + std::vector imuChunks(minSize); tbb::parallel_for( tbb::blocked_range(0, minSize), @@ -327,106 +325,63 @@ void calculate_trajectory( bool use_remove_imu_bias_from_first_stationary_scan) { UTL_PROFILER_SCOPE("calculate_trajectory"); - FusionAhrs ahrs; - FusionAhrsInitialise(&ahrs); - - if (fusionConventionNwu) - ahrs.settings.convention = FusionConventionNwu; - else if (fusionConventionEnu) - ahrs.settings.convention = FusionConventionEnu; - else if (fusionConventionNed) - ahrs.settings.convention = FusionConventionNed; - ahrs.settings.gain = ahrs_gain; - int counter = 1; + int counter = 1; double previous_time_stamp = 0.0; - static bool first = true; - static double last_ts; - std::cout << "start calculating trajectory.." << std::endl; - double bias_gyr_x = 0.0; - double bias_gyr_y = 0.0; - double bias_gyr_z = 0.0; - - if (use_remove_imu_bias_from_first_stationary_scan) + // Compute average dt for VQF initialization + double avg_dt = 1.0 / 200.0; + if (imu_data.size() >= 2) { - if (imu_data.size() > 1000) - { - std::vector gyr_x; - std::vector gyr_y; - std::vector gyr_z; - - for (int i = 0; i < 1000; i++) - { - const auto& [timestamp_pair, gyr, acc] = imu_data[i]; - - gyr_x.push_back(gyr.axis.x * RAD_TO_DEG); - gyr_y.push_back(gyr.axis.y * RAD_TO_DEG); - gyr_z.push_back(gyr.axis.z * RAD_TO_DEG); - } - - // Median without full sort - std::nth_element(gyr_x.begin(), gyr_x.begin() + 500, gyr_x.end()); - std::nth_element(gyr_y.begin(), gyr_y.begin() + 500, gyr_y.end()); - std::nth_element(gyr_z.begin(), gyr_z.begin() + 500, gyr_z.end()); - - bias_gyr_x = gyr_x[500]; - bias_gyr_y = gyr_y[500]; - bias_gyr_z = gyr_z[500]; - } - - std::cout << "------------------" << std::endl; - std::cout << "GYRO BIAS" << std::endl; - std::cout << "bias_gyr_x [deg/s]: " << bias_gyr_x << std::endl; - std::cout << "bias_gyr_y [deg/s]: " << bias_gyr_y << std::endl; - std::cout << "bias_gyr_z [deg/s]: " << bias_gyr_z << std::endl; - std::cout << "------------------" << std::endl; + 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; + vqf_params.tauAcc = ahrs_gain > 0.0 ? ahrs_gain : 3.0; + // VQF handles gyro bias estimation internally via Kalman filter + VQF vqf(vqf_params, avg_dt); + std::cout << "AHRS: VQF (tauAcc=" << vqf_params.tauAcc << ", dt=" << avg_dt << ", samples=" << imu_data.size() << ")" << std::endl; + for (const auto& [timestamp_pair, gyr, acc] : imu_data) { - const FusionVector gyroscope = { static_cast(gyr.axis.x * RAD_TO_DEG) - static_cast(bias_gyr_x), - static_cast(gyr.axis.y * RAD_TO_DEG) - static_cast(bias_gyr_y), - static_cast(gyr.axis.z * RAD_TO_DEG) - static_cast(bias_gyr_z) }; - const FusionVector accelerometer = { acc.axis.x, acc.axis.y, acc.axis.z }; - - if (first) - { - FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, 1.0 / 200.0); - first = false; - // last_ts = timestamp_pair.first; - } - else - { - double curr_ts = timestamp_pair.first; - - double ts_diff = curr_ts - last_ts; - - FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, ts_diff); - } - - last_ts = timestamp_pair.first; - - FusionQuaternion quat = FusionAhrsGetQuaternion(&ahrs); - - Eigen::Quaterniond d{ quat.element.w, quat.element.x, quat.element.y, quat.element.z }; + // IMU data: gyro in rad/s, acc in g + // VQF expects: gyro in rad/s, acc in m/s² + 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); + // Store gyro/acc in original units for RawIMUData RawIMUData rawImuData{ timestamp_pair.first, - { accelerometer.axis.x, accelerometer.axis.y, accelerometer.axis.z }, - { gyroscope.axis.x, gyroscope.axis.y, gyroscope.axis.z } }; // check timestamp + { static_cast(acc.x()), static_cast(acc.y()), static_cast(acc.z()) }, + { static_cast(gyr.x()), static_cast(gyr.y()), static_cast(gyr.z()) } }; trajectory[timestamp_pair.first] = std::make_tuple(t.matrix(), timestamp_pair.second, rawImuData); if (debugMsg) { - const FusionEuler euler = FusionQuaternionToEuler(quat); counter++; if (counter % 100 == 0) { - std::cout << "Roll " << euler.angle.roll << ", Pitch " << euler.angle.pitch << ", Yaw " << euler.angle.yaw << " [" + 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; } } diff --git a/apps/lidar_odometry_step_1/lidar_odometry.h b/apps/lidar_odometry_step_1/lidar_odometry.h index e080ecb8..c8aa33b3 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.h +++ b/apps/lidar_odometry_step_1/lidar_odometry.h @@ -15,7 +15,7 @@ // #define SAMPLE_PERIOD (1.0 / 200.0) using Trajectory = std::map>; -using Imu = std::vector, FusionVector, FusionVector>>; +using Imu = std::vector, Eigen::Vector3f, Eigen::Vector3f>>; bool load_data( std::vector& input_file_names, diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp index b9340a47..56ea8d42 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp @@ -465,7 +465,7 @@ void alternative_approach() } std::cout << "loading imu" << std::endl; - std::vector, FusionVector, FusionVector>> imu_data; + Imu imu_data; std::for_each(std::begin(csv_files), std::end(csv_files), [&imu_data](const std::string& fn) { @@ -473,67 +473,50 @@ void alternative_approach() std::cout << fn << std::endl; imu_data.insert(std::end(imu_data), std::begin(imu), std::end(imu)); }); - FusionAhrs ahrs; - FusionAhrsInitialise(&ahrs); - - if (params.fusionConventionNwu) - { - ahrs.settings.convention = FusionConventionNwu; - } - if (params.fusionConventionEnu) - { - ahrs.settings.convention = FusionConventionEnu; - } - if (params.fusionConventionNed) + // Compute average dt for VQF initialization + double avg_dt = 1.0 / 200.0; + if (imu_data.size() >= 2) { - ahrs.settings.convention = FusionConventionNed; + 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; + vqf_params.tauAcc = params.ahrs_gain > 0.0 ? params.ahrs_gain : 3.0; + VQF vqf(vqf_params, avg_dt); + std::map trajectory; int counter = 1; - static bool first = true; - static double last_ts; for (const auto& [timestamp_pair, gyr, acc] : imu_data) { - const FusionVector gyroscope = { static_cast(gyr.axis.x * RAD_TO_DEG), static_cast(gyr.axis.y * RAD_TO_DEG), static_cast(gyr.axis.z * RAD_TO_DEG) }; - const FusionVector accelerometer = { acc.axis.x, acc.axis.y, acc.axis.z }; - - //FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, SAMPLE_PERIOD); - if (first) - { - FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, 1.0 / 200.0); - first = false; - } - else - { - double curr_ts = timestamp_pair.first; - double ts_diff = curr_ts - last_ts; - if (ts_diff < 0.01) - { - FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, ts_diff); - } - else - { - std::cout << "IMU TS jump!!!" << std::endl; - FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, 1.0 / 200.0); - } - } - last_ts = timestamp_pair.first; - - FusionQuaternion quat = FusionAhrsGetQuaternion(&ahrs); - - Eigen::Quaterniond d{ quat.element.w, quat.element.x, quat.element.y, quat.element.z }; + 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] = t.matrix(); - const FusionEuler euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs)); counter++; if (counter % 100 == 0) { - std::cout << << "Roll " << euler.angle.roll<< ", Pitch " << euler.angle.pitch<< ", Yaw " << euler.angle.yaw<< " [" << counter++ << " of " << imu_data.size() << "]"<< std::endl; + 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; } } @@ -910,7 +893,7 @@ void settings_gui() if (fusionConvention < 0 || fusionConvention > 2) fusionConvention = 0; - ImGui::Text("Fusion convention: "); + ImGui::Text("AHRS convention: "); if (ImGui::IsItemHovered()) ImGui::SetTooltip( "Coordinate system conventions for sensor fusion defining how the axes are oriented relative to world frame"); diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp index b95cc2ba..9690faf7 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp @@ -346,9 +346,9 @@ bool save_poses(const std::string& file_name, const std::vector return true; } -std::vector, FusionVector, FusionVector>> load_imu(const std::string& imu_file, int imuToUse) +std::vector, Eigen::Vector3f, Eigen::Vector3f>> load_imu(const std::string& imu_file, int imuToUse) { - std::vector, FusionVector, FusionVector>> all_data; + std::vector, Eigen::Vector3f, Eigen::Vector3f>> all_data; csv::CSVFormat format; format.delimiter({ ' ', ',', '\t' }); @@ -396,14 +396,14 @@ std::vector, FusionVector, FusionVector>> l { double timestamp = row["timestamp"].get(); double timestampUnix = row["timestampUnix"].get(); - FusionVector gyr; - gyr.axis.x = row["gyroX"].get(); - gyr.axis.y = row["gyroY"].get(); - gyr.axis.z = row["gyroZ"].get(); - FusionVector acc; - acc.axis.x = row["accX"].get(); - acc.axis.y = row["accY"].get(); - acc.axis.z = row["accZ"].get(); + Eigen::Vector3f gyr( + row["gyroX"].get(), + row["gyroY"].get(), + row["gyroZ"].get()); + Eigen::Vector3f acc( + row["accX"].get(), + row["accY"].get(), + row["accZ"].get()); all_data.emplace_back(std::pair(timestamp / 1e9, timestampUnix / 1e9), gyr, acc); } } @@ -433,15 +433,14 @@ std::vector, FusionVector, FusionVector>> l if (data[0] > 0 && imuId == imuToUse) { - FusionVector gyr; - gyr.axis.x = data[1]; - gyr.axis.y = data[2]; - gyr.axis.z = data[3]; - - FusionVector acc; - acc.axis.x = data[4]; - acc.axis.y = data[5]; - acc.axis.z = data[6]; + Eigen::Vector3f gyr( + static_cast(data[1]), + static_cast(data[2]), + static_cast(data[3])); + Eigen::Vector3f acc( + static_cast(data[4]), + static_cast(data[5]), + static_cast(data[6])); all_data.emplace_back(std::pair(data[0] / 1e9, timestampUnix / 1e9), gyr, acc); } diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.h b/apps/lidar_odometry_step_1/lidar_odometry_utils.h index 82f4bce3..53d1ddb7 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.h +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.h @@ -11,7 +11,7 @@ #include #include -#include +#include #include #include #include @@ -212,7 +212,7 @@ void update_rgd_spherical_coordinates( //! @param imu_file - path to file with IMU data //! @param imuToUse - id number of IMU to use, the same index as in pointcloud return by @ref load_point_cloud //! @return vector of tuples (std::pair, angular_velocity, linear_acceleration) -std::vector, FusionVector, FusionVector>> load_imu(const std::string& imu_file, int imuToUse); +std::vector, Eigen::Vector3f, Eigen::Vector3f>> load_imu(const std::string& imu_file, int imuToUse); //! This function load point cloud from LAS/LAZ file. //! Optionally it can apply extrinsic calibration to each point. diff --git a/apps/livox_mid_360_intrinsic_calibration/CMakeLists.txt b/apps/livox_mid_360_intrinsic_calibration/CMakeLists.txt index ae2a6317..d8343f2b 100644 --- a/apps/livox_mid_360_intrinsic_calibration/CMakeLists.txt +++ b/apps/livox_mid_360_intrinsic_calibration/CMakeLists.txt @@ -26,13 +26,13 @@ target_include_directories( ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master ${LASZIP_INCLUDE_DIR}/LASzip/include ${THIRDPARTY_DIRECTORY}/observation_equations/codes - ${THIRDPARTY_DIRECTORY}/Fusion/Fusion) + ${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp) target_compile_definitions(livox_mid_360_intrinsic_calibration PRIVATE UTL_PROFILER_DISABLE) target_link_libraries( livox_mid_360_intrinsic_calibration - PRIVATE Fusion + PRIVATE vqf unordered_dense::unordered_dense spdlog::spdlog UTL::include diff --git a/apps/mandeye_mission_recorder_calibration/CMakeLists.txt b/apps/mandeye_mission_recorder_calibration/CMakeLists.txt index 360fd81b..5e757086 100644 --- a/apps/mandeye_mission_recorder_calibration/CMakeLists.txt +++ b/apps/mandeye_mission_recorder_calibration/CMakeLists.txt @@ -36,13 +36,13 @@ target_include_directories( ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master ${LASZIP_INCLUDE_DIR}/LASzip/include ${THIRDPARTY_DIRECTORY}/observation_equations/codes - ${THIRDPARTY_DIRECTORY}/Fusion/Fusion) + ${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp) target_compile_definitions(mandeye_mission_recorder_calibration PRIVATE UTL_PROFILER_DISABLE) target_link_libraries( mandeye_mission_recorder_calibration - PRIVATE Fusion + PRIVATE vqf unordered_dense::unordered_dense spdlog::spdlog UTL::include diff --git a/apps/mandeye_raw_data_viewer/CMakeLists.txt b/apps/mandeye_raw_data_viewer/CMakeLists.txt index 971360d3..e41f2072 100644 --- a/apps/mandeye_raw_data_viewer/CMakeLists.txt +++ b/apps/mandeye_raw_data_viewer/CMakeLists.txt @@ -39,13 +39,13 @@ target_include_directories( ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master ${LASZIP_INCLUDE_DIR}/LASzip/include ${THIRDPARTY_DIRECTORY}/observation_equations/codes - ${THIRDPARTY_DIRECTORY}/Fusion/Fusion) + ${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp) target_compile_definitions(mandeye_raw_data_viewer PRIVATE UTL_PROFILER_DISABLE) target_link_libraries( mandeye_raw_data_viewer - PRIVATE Fusion + PRIVATE vqf unordered_dense::unordered_dense spdlog::spdlog UTL::include 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 66be38b4..60c0a443 100644 --- a/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp +++ b/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp @@ -887,7 +887,7 @@ void loadFiles(std::vector input_file_names) params.filter_threshold_xy_outer = filter_threshold_xy_outer; std::vector> pointsPerFile; - std::vector, FusionVector, FusionVector>> imu_data; + Imu imu_data; // no files selected, quit loading if (input_file_names.empty()) @@ -964,81 +964,47 @@ void loadFiles(std::vector input_file_names) // rest of RAW data viewer processing - FusionAhrs ahrs; - FusionAhrsInitialise(&ahrs); - - if (fusionConventionNwu) - ahrs.settings.convention = FusionConventionNwu; - - if (fusionConventionEnu) - ahrs.settings.convention = FusionConventionEnu; - - if (fusionConventionNed) - ahrs.settings.convention = FusionConventionNed; + // VQF initialization + double avg_dt = SAMPLE_PERIOD; + 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); + } - ahrs.settings.gain = ahrs_gain; + VQFParams vqf_params; + vqf_params.tauAcc = ahrs_gain > 0.0 ? ahrs_gain : 3.0; + VQF vqf(vqf_params, avg_dt); std::map> trajectory; int counter = 1; - static bool first = true; - - static double last_ts; for (const auto& [timestamp_pair, gyr, acc] : imu_data) { - const FusionVector gyroscope = { static_cast(gyr.axis.x * 180.0 / M_PI), - static_cast(gyr.axis.y * 180.0 / M_PI), - static_cast(gyr.axis.z * 180.0 / M_PI) }; - // const FusionVector gyroscope = {static_cast(gyr.axis.x), static_cast(gyr.axis.y), - // static_cast(gyr.axis.z)}; - const FusionVector accelerometer = { acc.axis.x, acc.axis.y, acc.axis.z }; - - if (first) - { - FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, SAMPLE_PERIOD); - first = false; - // last_ts = timestamp_pair.first; - } - else - { - double curr_ts = timestamp_pair.first; - - double ts_diff = curr_ts - last_ts; - - FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, ts_diff); - - /*if (ts_diff < 0) - { - spdlog::warning("WARNING!!!!"); - spdlog::warning("WARNING!!!!"); - spdlog::warning("WARNING!!!!"); - spdlog::warning("WARNING!!!!"); - spdlog::warning("WARNING!!!!"); - spdlog::warning("WARNING!!!!"); - } - - if (ts_diff < 0.01) - { - FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, ts_diff); - } - else - { - FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, SAMPLE_PERIOD); - }*/ - } - - last_ts = timestamp_pair.first; - // - - FusionQuaternion quat = FusionAhrsGetQuaternion(&ahrs); - - Eigen::Quaterniond d{ quat.element.w, quat.element.x, quat.element.y, quat.element.z }; + 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); - const FusionEuler euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs)); + + Eigen::Vector3d euler = d.toRotationMatrix().eulerAngles(0, 1, 2) * (180.0 / M_PI); counter++; if (counter % 100 == 0) { @@ -1046,22 +1012,22 @@ void loadFiles(std::vector input_file_names) "[{} of {}]: Roll {}, Pitch {}, Yaw {}", counter++, imu_data.size(), - euler.angle.roll, - euler.angle.pitch, - euler.angle.yaw); + 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.axis.x); - imu_data_plot.angY.push_back(gyr.axis.y); - imu_data_plot.angZ.push_back(gyr.axis.z); - imu_data_plot.accX.push_back(acc.axis.x); - imu_data_plot.accY.push_back(acc.axis.y); - imu_data_plot.accZ.push_back(acc.axis.z); - imu_data_plot.yaw.push_back(euler.angle.yaw); - imu_data_plot.pitch.push_back(euler.angle.pitch); - imu_data_plot.roll.push_back(euler.angle.roll); + 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()); } std::vector> timestamps; diff --git a/apps/mandeye_single_session_viewer/CMakeLists.txt b/apps/mandeye_single_session_viewer/CMakeLists.txt index 57396796..0313b5fa 100644 --- a/apps/mandeye_single_session_viewer/CMakeLists.txt +++ b/apps/mandeye_single_session_viewer/CMakeLists.txt @@ -35,11 +35,11 @@ target_include_directories( ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master ${LASZIP_INCLUDE_DIR}/LASzip/include ${THIRDPARTY_DIRECTORY}/observation_equations/codes - ${THIRDPARTY_DIRECTORY}/Fusion/Fusion) + ${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp) target_link_libraries( mandeye_single_session_viewer - PRIVATE Fusion + PRIVATE vqf unordered_dense::unordered_dense spdlog::spdlog ${FREEGLUT_LIBRARY} diff --git a/apps/multi_view_tls_registration/CMakeLists.txt b/apps/multi_view_tls_registration/CMakeLists.txt index 476b40b7..b1386a2a 100644 --- a/apps/multi_view_tls_registration/CMakeLists.txt +++ b/apps/multi_view_tls_registration/CMakeLists.txt @@ -37,7 +37,7 @@ target_include_directories( ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master ${LASZIP_INCLUDE_DIR}/LASzip/include ${THIRDPARTY_DIRECTORY}/observation_equations/codes - ${THIRDPARTY_DIRECTORY}/Fusion/Fusion + ${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp ${EXTERNAL_LIBRARIES_DIRECTORY}/include) target_link_libraries( diff --git a/apps/quick_start_demo/CMakeLists.txt b/apps/quick_start_demo/CMakeLists.txt index 429563f8..b14c9576 100644 --- a/apps/quick_start_demo/CMakeLists.txt +++ b/apps/quick_start_demo/CMakeLists.txt @@ -26,13 +26,13 @@ target_include_directories( ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master ${LASZIP_INCLUDE_DIR}/LASzip/include ${THIRDPARTY_DIRECTORY}/observation_equations/codes - ${THIRDPARTY_DIRECTORY}/Fusion/Fusion) + ${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp) target_compile_definitions(quick_start_demo PRIVATE UTL_PROFILER_DISABLE) target_link_libraries( quick_start_demo - PRIVATE Fusion + PRIVATE vqf spdlog::spdlog UTL::include ${FREEGLUT_LIBRARY} diff --git a/apps/quick_start_demo/quick_start_demo.cpp b/apps/quick_start_demo/quick_start_demo.cpp index 0caa445d..b0f4cfa6 100644 --- a/apps/quick_start_demo/quick_start_demo.cpp +++ b/apps/quick_start_demo/quick_start_demo.cpp @@ -832,7 +832,7 @@ int main(int argc, char *argv[]) std::cout << input_file_names[i] << std::endl; } std::cout << "loading imu" << std::endl; - std::vector, FusionVector, FusionVector>> imu_data; + std::vector, Eigen::Vector3f, Eigen::Vector3f>> imu_data; for (size_t fileNo = 0; fileNo < csv_files.size(); fileNo++) { @@ -896,44 +896,39 @@ int main(int argc, char *argv[]) }); std::cout << "std::transform finished" << std::endl; - FusionAhrs ahrs; - FusionAhrsInitialise(&ahrs); - - if (fusionConventionNwu) - { - ahrs.settings.convention = FusionConventionNwu; - } - if (fusionConventionEnu) - { - ahrs.settings.convention = FusionConventionEnu; - } - if (fusionConventionNed) - { - ahrs.settings.convention = FusionConventionNed; - } + // VQF initialization + VQFParams vqf_params; + VQF vqf(vqf_params, SAMPLE_PERIOD); std::map> trajectory; int counter = 1; for (const auto &[timestamp_pair, gyr, acc] : imu_data) { - const FusionVector gyroscope = {static_cast(gyr.axis.x * 180.0 / M_PI), static_cast(gyr.axis.y * 180.0 / M_PI), static_cast(gyr.axis.z * 180.0 / M_PI)}; - const FusionVector accelerometer = {acc.axis.x, acc.axis.y, acc.axis.z}; - - FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, SAMPLE_PERIOD); - - FusionQuaternion quat = FusionAhrsGetQuaternion(&ahrs); - - Eigen::Quaterniond d{quat.element.w, quat.element.x, quat.element.y, quat.element.z}; + 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); - const FusionEuler euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs)); counter++; if (counter % 100 == 0) { - std::cout << "Roll " << euler.angle.roll<< ", Pitch " << euler.angle.pitch<< ", Yaw " << euler.angle.yaw<< " [" << counter++ << " of " << imu_data.size() << "]"<< std::endl; + 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; } } diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 30ed1e02..eb54f12b 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -46,7 +46,7 @@ function(add_core_target target_name with_gui) add_library(${target_name} STATIC ${SOURCES}) target_compile_definitions(${target_name} PRIVATE ${DEFINES}) - target_link_libraries(${target_name} PRIVATE ${PLATFORM_LASZIP_LIB} ${PLATFORM_MISCELLANEOUS_LIBS} spdlog::spdlog Fusion) + target_link_libraries(${target_name} PRIVATE ${PLATFORM_LASZIP_LIB} ${PLATFORM_MISCELLANEOUS_LIBS} spdlog::spdlog vqf) target_include_directories(${target_name} PRIVATE include ${EIGEN3_INCLUDE_DIR} @@ -54,7 +54,7 @@ function(add_core_target target_name with_gui) ${THIRDPARTY_DIRECTORY}/json/include ${THIRDPARTY_DIRECTORY}/observation_equations/codes ${EXTERNAL_LIBRARIES_DIRECTORY}/include - ${THIRDPARTY_DIRECTORY}/Fusion/Fusion + ${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp ) target_link_libraries(${target_name} PRIVATE PROJ::proj) diff --git a/core/src/imu_preintegration.cpp b/core/src/imu_preintegration.cpp index 52c059ac..f7dac02d 100644 --- a/core/src/imu_preintegration.cpp +++ b/core/src/imu_preintegration.cpp @@ -4,7 +4,7 @@ #include #include -#include +#include namespace imu_utils { @@ -49,10 +49,18 @@ std::vector estimate_orientations( std::vector orientations; orientations.reserve(raw_imu_data.size()); - FusionAhrs ahrs; - FusionAhrsInitialise(&ahrs); - ahrs.settings.convention = FusionConventionNwu; - ahrs.settings.gain = static_cast(params.ahrs_gain); + // 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.ahrs_gain > 0.0 ? params.ahrs_gain : 3.0; + VQF vqf(vqf_params, avg_dt); orientations.push_back(initial_orientation); @@ -65,20 +73,24 @@ std::vector estimate_orientations( continue; } - // RawIMUData: gyro in deg/s, accel in g — matches Fusion expectations - const FusionVector gyroscope = { - static_cast(raw_imu_data[k].guroscopes.x()), - static_cast(raw_imu_data[k].guroscopes.y()), - static_cast(raw_imu_data[k].guroscopes.z()) }; - const FusionVector accelerometer = { - static_cast(raw_imu_data[k].accelerometers.x()), - static_cast(raw_imu_data[k].accelerometers.y()), - static_cast(raw_imu_data[k].accelerometers.z()) }; - - FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, static_cast(dt)); - - FusionQuaternion quat = FusionAhrsGetQuaternion(&ahrs); - Eigen::Quaterniond q(quat.element.w, quat.element.x, quat.element.y, quat.element.z); + // VQF expects: gyro in rad/s, acc in m/s² + // RawIMUData: gyro in deg/s, accel in g + const double deg2rad = M_PI / 180.0; + const double g = 9.81; + vqf_real_t gyr[3] = { + raw_imu_data[k].guroscopes.x() * deg2rad, + raw_imu_data[k].guroscopes.y() * deg2rad, + raw_imu_data[k].guroscopes.z() * deg2rad }; + 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; diff --git a/pybind/CMakeLists.txt b/pybind/CMakeLists.txt index 4f93b3e2..a08647a7 100644 --- a/pybind/CMakeLists.txt +++ b/pybind/CMakeLists.txt @@ -76,7 +76,7 @@ target_include_directories(core_py ${REPOSITORY_DIRECTORY}/core_hd_mapping/include ${EIGEN3_INCLUDE_DIR} ${LASZIP_INCLUDE_DIR}/LASzip/include - ${THIRDPARTY_DIRECTORY}/Fusion/Fusion + ${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp ${THIRDPARTY_DIRECTORY}/json/include ${THIRDPARTY_DIRECTORY} ${THIRDPARTY_DIRECTORY}/glm @@ -90,7 +90,7 @@ target_include_directories(lidar_odometry_py ${REPOSITORY_DIRECTORY}/core_hd_mapping/include ${EIGEN3_INCLUDE_DIR} ${LASZIP_INCLUDE_DIR}/LASzip/include - ${THIRDPARTY_DIRECTORY}/Fusion/Fusion + ${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp ${THIRDPARTY_DIRECTORY}/json/include ${THIRDPARTY_DIRECTORY}/tomlplusplus/include ${THIRDPARTY_DIRECTORY} @@ -105,7 +105,7 @@ target_include_directories(multi_view_tls_registration_py ${REPOSITORY_DIRECTORY}/core_hd_mapping/include ${EIGEN3_INCLUDE_DIR} ${LASZIP_INCLUDE_DIR}/LASzip/include - ${THIRDPARTY_DIRECTORY}/Fusion/Fusion + ${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp ${THIRDPARTY_DIRECTORY}/json/include ${THIRDPARTY_DIRECTORY} ${THIRDPARTY_DIRECTORY}/glm @@ -116,7 +116,7 @@ target_link_libraries(core_py PRIVATE pybind11::module core_no_gui ${PLATFORM_LASZIP_LIB} - Fusion + vqf ${PLATFORM_MISCELLANEOUS_LIBS} ) @@ -128,7 +128,7 @@ target_link_libraries(lidar_odometry_py spdlog::spdlog UTL::include ${PLATFORM_LASZIP_LIB} - Fusion + vqf ${PLATFORM_MISCELLANEOUS_LIBS} ) @@ -136,7 +136,7 @@ target_link_libraries(multi_view_tls_registration_py PRIVATE pybind11::module core_no_gui ${PLATFORM_LASZIP_LIB} - Fusion + vqf ${PLATFORM_MISCELLANEOUS_LIBS} ) From 46a3b37fda39b524a6dbf7b58b5ea8e86d00ede1 Mon Sep 17 00:00:00 2001 From: Jakubach Date: Sun, 8 Mar 2026 02:03:04 +0100 Subject: [PATCH 06/11] Fix double counter increment in debug output and set VQF tauAcc in quick_start_demo --- apps/lidar_odometry_step_1/lidar_odometry.cpp | 2 +- apps/lidar_odometry_step_1/lidar_odometry_gui.cpp | 2 +- apps/lidar_odometry_step_1/lidar_odometry_utils.h | 4 ++-- apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp | 2 +- apps/quick_start_demo/quick_start_demo.cpp | 3 ++- 5 files changed, 7 insertions(+), 6 deletions(-) diff --git a/apps/lidar_odometry_step_1/lidar_odometry.cpp b/apps/lidar_odometry_step_1/lidar_odometry.cpp index 45ff8b39..ce12ce7c 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry.cpp @@ -382,7 +382,7 @@ void calculate_trajectory( { 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; + << counter << " of " << imu_data.size() << "]" << 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 56ea8d42..0ec44219 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp @@ -516,7 +516,7 @@ void alternative_approach() 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; + std::cout << "Roll " << euler.x() << ", Pitch " << euler.y() << ", Yaw " << euler.z() << " [" << counter << " of " << imu_data.size() << "]" << std::endl; } } diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.h b/apps/lidar_odometry_step_1/lidar_odometry_utils.h index 53d1ddb7..821111ce 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.h +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.h @@ -65,11 +65,11 @@ struct LidarOdometryParams double threshould_output_filter = 0.5; // for export --> all points xyz.norm() < threshould_output_filter will be removed int min_counter_concatenated_trajectory_nodes = 10; // for export - // Madgwick filter + // AHRS (VQF) — fusionConvention params kept for TOML backwards compatibility bool fusionConventionNwu = true; bool fusionConventionEnu = false; bool fusionConventionNed = false; - double ahrs_gain = 0.5; + double ahrs_gain = 0.5; // VQF tauAcc parameter // lidar odometry control bool use_motion_from_previous_step = true; 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 60c0a443..9ee917c9 100644 --- a/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp +++ b/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp @@ -1010,7 +1010,7 @@ void loadFiles(std::vector input_file_names) { spdlog::info( "[{} of {}]: Roll {}, Pitch {}, Yaw {}", - counter++, + counter, imu_data.size(), euler.x(), euler.y(), diff --git a/apps/quick_start_demo/quick_start_demo.cpp b/apps/quick_start_demo/quick_start_demo.cpp index b0f4cfa6..1f9779e9 100644 --- a/apps/quick_start_demo/quick_start_demo.cpp +++ b/apps/quick_start_demo/quick_start_demo.cpp @@ -898,6 +898,7 @@ int main(int argc, char *argv[]) // VQF initialization VQFParams vqf_params; + vqf_params.tauAcc = 3.0; VQF vqf(vqf_params, SAMPLE_PERIOD); std::map> trajectory; @@ -928,7 +929,7 @@ int main(int argc, char *argv[]) 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; + std::cout << "Roll " << euler.x() << ", Pitch " << euler.y() << ", Yaw " << euler.z() << " [" << counter << " of " << imu_data.size() << "]" << std::endl; } } From 4769c8ab2c442718779460ba2824b529d1062f4c Mon Sep 17 00:00:00 2001 From: Jakubach Date: Sun, 8 Mar 2026 02:03:37 +0100 Subject: [PATCH 07/11] Add VQF library as git submodule --- .gitmodules | 3 +++ 3rdparty/vqf | 1 + 2 files changed, 4 insertions(+) create mode 160000 3rdparty/vqf diff --git a/.gitmodules b/.gitmodules index da3afb9d..cb81a74b 100644 --- a/.gitmodules +++ b/.gitmodules @@ -53,3 +53,6 @@ [submodule "3rdparty/UTL"] path = 3rdparty/UTL url = https://github.com/DmitriBogdanov/UTL.git +[submodule "3rdparty/vqf"] + path = 3rdparty/vqf + url = https://github.com/dlaidig/vqf.git diff --git a/3rdparty/vqf b/3rdparty/vqf new file mode 160000 index 00000000..b66ff218 --- /dev/null +++ b/3rdparty/vqf @@ -0,0 +1 @@ +Subproject commit b66ff218128ab0993b4ac19b538a4a57863c3382 From ad01c8fb4645ef8c4fd6e1e9a939b1a81f903af2 Mon Sep 17 00:00:00 2001 From: Jakubach Date: Sun, 8 Mar 2026 02:09:58 +0100 Subject: [PATCH 08/11] Change default preintegration method to Trapezoidal (AHRS/VQF) --- apps/lidar_odometry_step_1/lidar_odometry_utils.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.h b/apps/lidar_odometry_step_1/lidar_odometry_utils.h index 821111ce..a8c1ada7 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.h +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.h @@ -156,7 +156,7 @@ struct LidarOdometryParams // IMU preintegration bool use_imu_preintegration = true; - int imu_preintegration_method = 3; // 0=euler_body, 1=trapezoidal_body, 2=euler_gravity, 3=trapezoidal_gravity, 4=kalman + int imu_preintegration_method = 6; // 0=euler_body, 1=trapezoidal_body, 2=euler_gravity, 3=trapezoidal_gravity, 4=kalman, 5=euler_ahrs, 6=trapezoidal_ahrs, 7=kalman_ahrs // ablation study bool ablation_study_use_planarity = false; From 955bd296c9ea69671c28ae0adc08cb51bda77a99 Mon Sep 17 00:00:00 2001 From: Jakubach Date: Sun, 8 Mar 2026 02:37:58 +0100 Subject: [PATCH 09/11] Rename ahrs_gain to vqf_tauAcc, update GUI labels and tooltips, set default to 0.5s --- ...eye_data_and_drop_here-precision_forestry.cpp | 14 ++++++-------- apps/lidar_odometry_step_1/lidar_odometry.cpp | 6 +++--- apps/lidar_odometry_step_1/lidar_odometry.h | 2 +- .../lidar_odometry_step_1/lidar_odometry_gui.cpp | 16 +++++++--------- .../lidar_odometry_step_1/lidar_odometry_utils.h | 2 +- apps/lidar_odometry_step_1/toml_io.h | 4 ++-- .../mandeye_raw_data_viewer.cpp | 12 ++++++------ core/include/imu_preintegration.h | 2 +- core/src/imu_preintegration.cpp | 2 +- pybind/lidar_odometry_binding.cpp | 2 +- 10 files changed, 29 insertions(+), 33 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 16b5e465..9c2a0869 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 @@ -449,7 +449,7 @@ void step1(const std::atomic& loPause, std::string input_folder_name) params.fusionConventionNwu, params.fusionConventionEnu, params.fusionConventionNed, - params.ahrs_gain, + params.vqf_tauAcc, full_debug_messages, params.use_removie_imu_bias_from_first_stationary_scan); compute_step_1(pointsPerFile, params, trajectory, worker_data, loPause); @@ -756,15 +756,13 @@ void settings_gui() ImGui::NewLine(); ImGui::Checkbox("Use motion from previous step", ¶ms.use_motion_from_previous_step); - ImGui::InputDouble("AHRS gain", ¶ms.ahrs_gain, 0.0, 0.0, "%.3f"); + ImGui::InputDouble("VQF tauAcc [s]", ¶ms.vqf_tauAcc, 0.0, 0.0, "%.3f"); if (ImGui::IsItemHovered()) { ImGui::BeginTooltip(); - ImGui::Text("Attitude and Heading Reference System gain:"); - ImGui::Text( - "How strongly the accelerometer/magnetometer corrections influence the orientation estimate versus gyroscope " - "integration"); - ImGui::Text("Larger value means faster response to changes in orientation, but more noise"); + 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(); } @@ -1438,7 +1436,7 @@ void step1( params.fusionConventionNwu, params.fusionConventionEnu, params.fusionConventionNed, - params.ahrs_gain, + params.vqf_tauAcc, full_debug_messages, params.use_removie_imu_bias_from_first_stationary_scan); compute_step_1(pointsPerFile, params, trajectory, worker_data, loPause); diff --git a/apps/lidar_odometry_step_1/lidar_odometry.cpp b/apps/lidar_odometry_step_1/lidar_odometry.cpp index ce12ce7c..7f9a09aa 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry.cpp @@ -320,7 +320,7 @@ void calculate_trajectory( bool fusionConventionNwu, bool fusionConventionEnu, bool fusionConventionNed, - double ahrs_gain, + double vqf_tauAcc, bool debugMsg, bool use_remove_imu_bias_from_first_stationary_scan) { @@ -342,7 +342,7 @@ void calculate_trajectory( } VQFParams vqf_params; - vqf_params.tauAcc = ahrs_gain > 0.0 ? ahrs_gain : 3.0; + vqf_params.tauAcc = vqf_tauAcc > 0.0 ? vqf_tauAcc : 3.0; // VQF handles gyro bias estimation internally via Kalman filter VQF vqf(vqf_params, avg_dt); std::cout << "AHRS: VQF (tauAcc=" << vqf_params.tauAcc << ", dt=" << avg_dt << ", samples=" << imu_data.size() << ")" << std::endl; @@ -1026,7 +1026,7 @@ std::vector run_lidar_odometry(const std::string& input_dir, LidarOd params.fusionConventionNwu, params.fusionConventionEnu, params.fusionConventionNed, - params.ahrs_gain, + params.vqf_tauAcc, true, params.use_removie_imu_bias_from_first_stationary_scan); diff --git a/apps/lidar_odometry_step_1/lidar_odometry.h b/apps/lidar_odometry_step_1/lidar_odometry.h index c8aa33b3..780c2603 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.h +++ b/apps/lidar_odometry_step_1/lidar_odometry.h @@ -29,7 +29,7 @@ void calculate_trajectory( bool fusionConventionNwu, bool fusionConventionEnu, bool fusionConventionNed, - double ahrs_gain, + double vqf_tauAcc, bool debugMsg, bool use_removie_imu_bias_from_first_stationary_scan); bool compute_step_1( diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp index 0ec44219..ca027a21 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp @@ -484,7 +484,7 @@ void alternative_approach() } VQFParams vqf_params; - vqf_params.tauAcc = params.ahrs_gain > 0.0 ? params.ahrs_gain : 3.0; + vqf_params.tauAcc = params.vqf_tauAcc > 0.0 ? params.vqf_tauAcc : 3.0; VQF vqf(vqf_params, avg_dt); std::map trajectory; @@ -612,7 +612,7 @@ void step1(const std::atomic& loPause) params.fusionConventionNwu, params.fusionConventionEnu, params.fusionConventionNed, - params.ahrs_gain, + params.vqf_tauAcc, full_debug_messages, params.use_removie_imu_bias_from_first_stationary_scan); compute_step_1(pointsPerFile, params, trajectory, worker_data, loPause); @@ -927,15 +927,13 @@ void settings_gui() "Euler (AHRS)", "Trapezoidal (AHRS)", "Kalman (AHRS)" }; ImGui::Combo("IMU preintegration method", ¶ms.imu_preintegration_method, methods, IM_ARRAYSIZE(methods)); } - ImGui::InputDouble("AHRS gain", ¶ms.ahrs_gain, 0.0, 0.0, "%.3f"); + ImGui::InputDouble("VQF tauAcc [s]", ¶ms.vqf_tauAcc, 0.0, 0.0, "%.3f"); if (ImGui::IsItemHovered()) { ImGui::BeginTooltip(); - ImGui::Text("Attitude and Heading Reference System gain:"); - ImGui::Text( - "How strongly the accelerometer/magnetometer corrections influence the orientation estimate versus gyroscope " - "integration"); - ImGui::Text("Larger value means faster response to changes in orientation, but more noise"); + 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(); } @@ -1641,7 +1639,7 @@ void step1( params.fusionConventionNwu, params.fusionConventionEnu, params.fusionConventionNed, - params.ahrs_gain, + params.vqf_tauAcc, full_debug_messages, params.use_removie_imu_bias_from_first_stationary_scan); compute_step_1(pointsPerFile, params, trajectory, worker_data, loPause); diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.h b/apps/lidar_odometry_step_1/lidar_odometry_utils.h index a8c1ada7..97340d31 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.h +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.h @@ -69,7 +69,7 @@ struct LidarOdometryParams bool fusionConventionNwu = true; bool fusionConventionEnu = false; bool fusionConventionNed = false; - double ahrs_gain = 0.5; // VQF tauAcc parameter + double vqf_tauAcc = 0.5; // VQF accelerometer time constant [s] (higher = more gyro trust) // lidar odometry control bool use_motion_from_previous_step = true; diff --git a/apps/lidar_odometry_step_1/toml_io.h b/apps/lidar_odometry_step_1/toml_io.h index c6f73a37..528d3022 100644 --- a/apps/lidar_odometry_step_1/toml_io.h +++ b/apps/lidar_odometry_step_1/toml_io.h @@ -37,7 +37,7 @@ class TomlIO { "fusionConventionNwu", &LidarOdometryParams::fusionConventionNwu }, { "fusionConventionEnu", &LidarOdometryParams::fusionConventionEnu }, { "fusionConventionNed", &LidarOdometryParams::fusionConventionNed }, - { "ahrs_gain", &LidarOdometryParams::ahrs_gain }, + { "vqf_tauAcc", &LidarOdometryParams::vqf_tauAcc }, { "use_motion_from_previous_step", &LidarOdometryParams::use_motion_from_previous_step }, { "nr_iter", &LidarOdometryParams::nr_iter }, { "sliding_window_trajectory_length_threshold", &LidarOdometryParams::sliding_window_trajectory_length_threshold }, @@ -103,7 +103,7 @@ class TomlIO "decimation", "threshould_output_filter", "min_counter_concatenated_trajectory_nodes" } }, - { "madgwick_filter", { "fusionConventionNwu", "fusionConventionEnu", "fusionConventionNed", "ahrs_gain" } }, + { "ahrs_vqf", { "fusionConventionNwu", "fusionConventionEnu", "fusionConventionNed", "vqf_tauAcc" } }, { "lidar_odometry_control", { "use_motion_from_previous_step", "nr_iter", 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 9ee917c9..99070f49 100644 --- a/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp +++ b/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp @@ -97,7 +97,7 @@ int index_rendered_points_local = -1; // std::vector> all_points_local; // std::vector> all_lidar_ids; std::vector indexes_to_filename; -double ahrs_gain = 0.5; +double vqf_tauAcc = 0.5; double wx = 1000000.0; double wy = 1000000.0; double wz = 1000000.0; @@ -975,7 +975,7 @@ void loadFiles(std::vector input_file_names) } VQFParams vqf_params; - vqf_params.tauAcc = ahrs_gain > 0.0 ? ahrs_gain : 3.0; + vqf_params.tauAcc = vqf_tauAcc > 0.0 ? vqf_tauAcc : 3.0; VQF vqf(vqf_params, avg_dt); std::map> trajectory; @@ -1305,13 +1305,13 @@ void settings_gui() number_of_points_threshold = 0; } - ImGui::InputDouble("AHRS gain", &ahrs_gain); + ImGui::InputDouble("VQF tauAcc [s]", &vqf_tauAcc); if (ImGui::IsItemHovered()) { ImGui::BeginTooltip(); - ImGui::Text("Parameter for AHRS (Attitude and Heading Reference System) filter"); - ImGui::Text("Controls how strongly the filter corrects its orientation estimate"); - ImGui::Text("using accelerometer and magnetometer data, relative to the gyroscope integration"); + 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(); } diff --git a/core/include/imu_preintegration.h b/core/include/imu_preintegration.h index 4b9880fb..87938312 100644 --- a/core/include/imu_preintegration.h +++ b/core/include/imu_preintegration.h @@ -12,7 +12,7 @@ struct IntegrationParams double max_acceleration_threshold = 50.0; // m/s^2 double max_dt_threshold = 0.1; // seconds Eigen::Vector3d initial_velocity = Eigen::Vector3d::Zero(); - double ahrs_gain = 0.5; + double vqf_tauAcc = 0.5; // VQF accelerometer time constant [s] }; namespace imu_utils diff --git a/core/src/imu_preintegration.cpp b/core/src/imu_preintegration.cpp index f7dac02d..e800efbd 100644 --- a/core/src/imu_preintegration.cpp +++ b/core/src/imu_preintegration.cpp @@ -59,7 +59,7 @@ std::vector estimate_orientations( } VQFParams vqf_params; - vqf_params.tauAcc = params.ahrs_gain > 0.0 ? params.ahrs_gain : 3.0; + vqf_params.tauAcc = params.vqf_tauAcc > 0.0 ? params.vqf_tauAcc : 3.0; VQF vqf(vqf_params, avg_dt); orientations.push_back(initial_orientation); diff --git a/pybind/lidar_odometry_binding.cpp b/pybind/lidar_odometry_binding.cpp index fc1f581c..87c4aa02 100644 --- a/pybind/lidar_odometry_binding.cpp +++ b/pybind/lidar_odometry_binding.cpp @@ -100,7 +100,7 @@ PYBIND11_MODULE(lidar_odometry_py, m) .def_readwrite("use_mutliple_gaussian", &LidarOdometryParams::use_mutliple_gaussian) .def_readwrite("num_constistency_iter", &LidarOdometryParams::num_constistency_iter) .def_readwrite("threshould_output_filter", &LidarOdometryParams::threshould_output_filter) - .def_readwrite("ahrs_gain", &LidarOdometryParams::ahrs_gain) + .def_readwrite("vqf_tauAcc", &LidarOdometryParams::vqf_tauAcc) .def_readwrite("threshold_nr_poses", &LidarOdometryParams::threshold_nr_poses) .def_readwrite("current_output_dir", &LidarOdometryParams::current_output_dir) //.def_readwrite("min_counter", &LidarOdometryParams::min_counter) From 1c7851431f6e2613b23bcaf14f1b0687a0eb241c Mon Sep 17 00:00:00 2001 From: Jakubach Date: Sun, 8 Mar 2026 17:21:03 +0100 Subject: [PATCH 10/11] SM-independent initial_velocity for IMU preintegration Direction from VQF AHRS heading, speed from IMU preintegration displacement of previous worker. --- .../lidar_odometry_utils_optimizers.cpp | 24 +++++++++++++++---- 1 file changed, 19 insertions(+), 5 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 eb632da7..2775f691 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -2417,16 +2417,30 @@ bool process_worker_step_1( { IntegrationParams imu_params; - // Estimate initial velocity from previous trajectory segments - Eigen::Vector3d prev_displacement = - prev_worker_data.intermediate_trajectory[prev_worker_data.intermediate_trajectory.size() - 1].translation() - - prev_prev_worker_data.intermediate_trajectory[prev_prev_worker_data.intermediate_trajectory.size() - 1].translation(); + // Estimate initial velocity fully SM-independent: + // - Direction: AHRS orientation (VQF, not SM-optimized) + // - Speed: from previous worker's MOTION MODEL displacement (IMU prediction, not SM result) + Eigen::Vector3d prev_mm_displacement = + prev_worker_data.intermediate_trajectory_motion_model.back().translation() - + prev_worker_data.intermediate_trajectory_motion_model.front().translation(); if (worker_data.raw_imu_data.size() >= 2) { double total_imu_time = worker_data.raw_imu_data.back().timestamp - worker_data.raw_imu_data.front().timestamp; if (total_imu_time > 0.0) - imu_params.initial_velocity = prev_displacement / total_imu_time; + { + double speed = prev_mm_displacement.norm() / total_imu_time; + + // Use AHRS orientation from current worker (original VQF, not SM-optimized) + // to determine forward direction — breaks SM feedback loop + Eigen::Matrix3d R_ahrs = worker_data.intermediate_trajectory[0].linear(); + Eigen::Vector3d forward_global = R_ahrs * Eigen::Vector3d(1, 0, 0); + forward_global.z() = 0; // project to horizontal plane + if (forward_global.norm() > 1e-6) + imu_params.initial_velocity = forward_global.normalized() * speed; + else + imu_params.initial_velocity = Eigen::Vector3d::Zero(); + } } mean_shift = ImuPreintegration::create_and_preintegrate( From fbff2d344e2169e88ab5f39b18934890aa914d58 Mon Sep 17 00:00:00 2001 From: Jakubach Date: Sun, 8 Mar 2026 17:43:43 +0100 Subject: [PATCH 11/11] Expose all VQF parameters in GUI and TOML - All VQF params (bias estimation, rest detection, magnetometer) exposed with tooltips based on VQF paper descriptions - Magnetometer params hidden behind "Use magnetometer" checkbox - Refactored calculate_trajectory() to take LidarOdometryParams - Clearer preintegration method names in GUI - Added buildVQFParams() helper for consistent VQF initialization --- ..._data_and_drop_here-precision_forestry.cpp | 112 ++++++++++++-- apps/lidar_odometry_step_1/lidar_odometry.cpp | 29 ++-- apps/lidar_odometry_step_1/lidar_odometry.h | 8 +- .../lidar_odometry_gui.cpp | 138 +++++++++++++++--- .../lidar_odometry_utils.h | 70 ++++++++- apps/lidar_odometry_step_1/toml_io.h | 38 ++++- core/include/imu_preintegration.h | 16 +- 7 files changed, 348 insertions(+), 63 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 9c2a0869..f396d0c6 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 @@ -446,12 +446,8 @@ void step1(const std::atomic& loPause, std::string input_folder_name) calculate_trajectory( trajectory, imu_data, - params.fusionConventionNwu, - params.fusionConventionEnu, - params.fusionConventionNed, - params.vqf_tauAcc, - full_debug_messages, - params.use_removie_imu_bias_from_first_stationary_scan); + params, + full_debug_messages); compute_step_1(pointsPerFile, params, trajectory, worker_data, loPause); step_1_done = true; } @@ -766,6 +762,102 @@ void settings_gui() 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::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("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::TreePop(); + } + + if (params.vqf_restBiasEstEnabled && ImGui::TreeNode("VQF Rest Detection")) + { + 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(); + } + ImGui::PopItemWidth(); } ImGui::NewLine(); @@ -1433,12 +1525,8 @@ void step1( calculate_trajectory( trajectory, imu_data, - params.fusionConventionNwu, - params.fusionConventionEnu, - params.fusionConventionNed, - params.vqf_tauAcc, - full_debug_messages, - params.use_removie_imu_bias_from_first_stationary_scan); + params, + full_debug_messages); compute_step_1(pointsPerFile, params, trajectory, worker_data, loPause); std::cout << "step_1_done" << std::endl; } diff --git a/apps/lidar_odometry_step_1/lidar_odometry.cpp b/apps/lidar_odometry_step_1/lidar_odometry.cpp index 7f9a09aa..2917fd17 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry.cpp @@ -317,12 +317,8 @@ bool load_data( void calculate_trajectory( Trajectory& trajectory, Imu& imu_data, - bool fusionConventionNwu, - bool fusionConventionEnu, - bool fusionConventionNed, - double vqf_tauAcc, - bool debugMsg, - bool use_remove_imu_bias_from_first_stationary_scan) + const LidarOdometryParams& params, + bool debugMsg) { UTL_PROFILER_SCOPE("calculate_trajectory"); @@ -341,11 +337,11 @@ void calculate_trajectory( avg_dt = (t1 - t0) / static_cast(imu_data.size() - 1); } - VQFParams vqf_params; - vqf_params.tauAcc = vqf_tauAcc > 0.0 ? vqf_tauAcc : 3.0; - // VQF handles gyro bias estimation internally via Kalman filter + VQFParams vqf_params = buildVQFParams(params); VQF vqf(vqf_params, avg_dt); - std::cout << "AHRS: VQF (tauAcc=" << vqf_params.tauAcc << ", dt=" << avg_dt << ", samples=" << imu_data.size() << ")" << std::endl; + std::cout << "AHRS: VQF (tauAcc=" << vqf_params.tauAcc + << ", useMag=" << (params.vqf_useMagnetometer ? "true" : "false") + << ", dt=" << avg_dt << ", samples=" << imu_data.size() << ")" << std::endl; for (const auto& [timestamp_pair, gyr, acc] : imu_data) { @@ -364,7 +360,10 @@ void calculate_trajectory( vqf.update(gyr_vqf, acc_vqf); vqf_real_t quat[4]; - vqf.getQuat6D(quat); + 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); @@ -1023,12 +1022,8 @@ std::vector run_lidar_odometry(const std::string& input_dir, LidarOd calculate_trajectory( trajectory, imu_data, - params.fusionConventionNwu, - params.fusionConventionEnu, - params.fusionConventionNed, - params.vqf_tauAcc, - true, - params.use_removie_imu_bias_from_first_stationary_scan); + params, + true); std::atomic pause{ false }; diff --git a/apps/lidar_odometry_step_1/lidar_odometry.h b/apps/lidar_odometry_step_1/lidar_odometry.h index 780c2603..a5ee1a27 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.h +++ b/apps/lidar_odometry_step_1/lidar_odometry.h @@ -26,12 +26,8 @@ bool load_data( void calculate_trajectory( Trajectory& trajectory, Imu& imu_data, - bool fusionConventionNwu, - bool fusionConventionEnu, - bool fusionConventionNed, - double vqf_tauAcc, - bool debugMsg, - bool use_removie_imu_bias_from_first_stationary_scan); + const LidarOdometryParams& params, + bool debugMsg); bool compute_step_1( const std::vector>& pointsPerFile, LidarOdometryParams& params, diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp index ca027a21..965d3add 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp @@ -483,8 +483,7 @@ void alternative_approach() avg_dt = (t1 - t0) / static_cast(imu_data.size() - 1); } - VQFParams vqf_params; - vqf_params.tauAcc = params.vqf_tauAcc > 0.0 ? params.vqf_tauAcc : 3.0; + VQFParams vqf_params = buildVQFParams(params); VQF vqf(vqf_params, avg_dt); std::map trajectory; @@ -506,7 +505,10 @@ void alternative_approach() vqf.update(gyr_vqf, acc_vqf); vqf_real_t quat[4]; - vqf.getQuat6D(quat); + 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); @@ -609,12 +611,8 @@ void step1(const std::atomic& loPause) calculate_trajectory( trajectory, imu_data, - params.fusionConventionNwu, - params.fusionConventionEnu, - params.fusionConventionNed, - params.vqf_tauAcc, - full_debug_messages, - params.use_removie_imu_bias_from_first_stationary_scan); + params, + full_debug_messages); compute_step_1(pointsPerFile, params, trajectory, worker_data, loPause); step_1_done = true; } @@ -922,9 +920,15 @@ void settings_gui() ImGui::Checkbox("Use IMU preintegration", ¶ms.use_imu_preintegration); if (params.use_imu_preintegration) { - const char* methods[] = { "Euler (body frame)", "Trapezoidal (body frame)", - "Euler (gravity compensated)", "Trapezoidal (gravity compensated)", "Kalman filter", - "Euler (AHRS)", "Trapezoidal (AHRS)", "Kalman (AHRS)" }; + const char* methods[] = { + "Euler, no gravity compensation", + "Trapezoidal, no gravity compensation", + "Euler, gravity comp. (initial trajectory orientations)", + "Trapezoidal, gravity comp. (initial trajectory orientations)", + "Kalman, gravity comp. (initial trajectory orientations)", + "Euler, gravity comp. (per-worker VQF orientations)", + "Trapezoidal, gravity comp. (per-worker VQF orientations)", + "Kalman, gravity comp. (per-worker VQF orientations)" }; ImGui::Combo("IMU preintegration method", ¶ms.imu_preintegration_method, methods, IM_ARRAYSIZE(methods)); } ImGui::InputDouble("VQF tauAcc [s]", ¶ms.vqf_tauAcc, 0.0, 0.0, "%.3f"); @@ -937,6 +941,108 @@ void settings_gui() 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::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("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::TreePop(); + } + + if (params.vqf_restBiasEstEnabled && ImGui::TreeNode("VQF Rest Detection")) + { + 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(); + } + ImGui::PopItemWidth(); } ImGui::NewLine(); @@ -1636,12 +1742,8 @@ void step1( calculate_trajectory( trajectory, imu_data, - params.fusionConventionNwu, - params.fusionConventionEnu, - params.fusionConventionNed, - params.vqf_tauAcc, - full_debug_messages, - params.use_removie_imu_bias_from_first_stationary_scan); + params, + full_debug_messages); compute_step_1(pointsPerFile, params, trajectory, worker_data, loPause); std::cout << "step_1_done" << std::endl; } diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.h b/apps/lidar_odometry_step_1/lidar_odometry_utils.h index 97340d31..18567a21 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.h +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.h @@ -69,7 +69,40 @@ struct LidarOdometryParams bool fusionConventionNwu = true; bool fusionConventionEnu = false; bool fusionConventionNed = false; - double vqf_tauAcc = 0.5; // VQF accelerometer time constant [s] (higher = more gyro trust) + + // VQF core + double vqf_tauAcc = 0.5; // accelerometer time constant [s] (higher = more gyro trust) + + // VQF gyroscope bias estimation + bool vqf_motionBiasEstEnabled = true; // estimate gyro bias during motion + bool vqf_restBiasEstEnabled = true; // estimate gyro bias during rest + double vqf_biasSigmaInit = 0.5; // initial bias uncertainty [°/s] + double vqf_biasForgettingTime = 100.0; // time for uncertainty to grow 0→0.1 °/s [s] + double vqf_biasClip = 2.0; // max expected gyro bias [°/s] + double vqf_biasSigmaMotion = 0.1; // converged bias uncertainty during motion [°/s] + double vqf_biasVerticalForgettingFactor = 0.0001; // forgetting for unobservable vertical bias + double vqf_biasSigmaRest = 0.03; // converged bias uncertainty during rest [°/s] + + // VQF rest detection + double vqf_restMinT = 1.5; // time threshold for rest detection [s] + double vqf_restFilterTau = 0.5; // LP filter time constant for rest detection [s] + double vqf_restThGyr = 2.0; // gyro threshold for rest detection [°/s] + double vqf_restThAcc = 0.5; // acc threshold for rest detection [m/s²] + + // VQF magnetometer (only used when vqf_useMagnetometer is true) + bool vqf_useMagnetometer = false; // use 9D mode (with magnetometer) instead of 6D + double vqf_tauMag = 9.0; // magnetometer time constant [s] + bool vqf_magDistRejectionEnabled = true; // magnetic disturbance detection & rejection + double vqf_magCurrentTau = 0.05; // LP filter for current mag norm/dip [s] + double vqf_magRefTau = 20.0; // adjustment time for mag reference [s] + double vqf_magNormTh = 0.1; // relative threshold for mag field strength + double vqf_magDipTh = 10.0; // threshold for mag dip angle [°] + double vqf_magNewTime = 20.0; // time to accept new mag field [s] + double vqf_magNewFirstTime = 5.0; // time to accept first mag field [s] + double vqf_magNewMinGyr = 20.0; // min angular velocity for mag acceptance [°/s] + double vqf_magMinUndisturbedTime = 0.5; // min undisturbed time [s] + double vqf_magMaxRejectionTime = 60.0; // max full mag rejection duration [s] + double vqf_magRejectionFactor = 2.0; // slowdown factor for heading correction // lidar odometry control bool use_motion_from_previous_step = true; @@ -167,6 +200,41 @@ struct LidarOdometryParams bool save_index_pose = false; }; +inline VQFParams buildVQFParams(const LidarOdometryParams& p) +{ + VQFParams vp; + vp.tauAcc = p.vqf_tauAcc > 0.0 ? p.vqf_tauAcc : 3.0; + vp.tauMag = p.vqf_tauMag; +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + vp.motionBiasEstEnabled = p.vqf_motionBiasEstEnabled; +#endif + vp.restBiasEstEnabled = p.vqf_restBiasEstEnabled; + vp.magDistRejectionEnabled = p.vqf_magDistRejectionEnabled; + vp.biasSigmaInit = p.vqf_biasSigmaInit; + vp.biasForgettingTime = p.vqf_biasForgettingTime; + vp.biasClip = p.vqf_biasClip; +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + vp.biasSigmaMotion = p.vqf_biasSigmaMotion; + vp.biasVerticalForgettingFactor = p.vqf_biasVerticalForgettingFactor; +#endif + vp.biasSigmaRest = p.vqf_biasSigmaRest; + vp.restMinT = p.vqf_restMinT; + vp.restFilterTau = p.vqf_restFilterTau; + vp.restThGyr = p.vqf_restThGyr; + vp.restThAcc = p.vqf_restThAcc; + vp.magCurrentTau = p.vqf_magCurrentTau; + vp.magRefTau = p.vqf_magRefTau; + vp.magNormTh = p.vqf_magNormTh; + vp.magDipTh = p.vqf_magDipTh; + vp.magNewTime = p.vqf_magNewTime; + vp.magNewFirstTime = p.vqf_magNewFirstTime; + vp.magNewMinGyr = p.vqf_magNewMinGyr; + vp.magMinUndisturbedTime = p.vqf_magMinUndisturbedTime; + vp.magMaxRejectionTime = p.vqf_magMaxRejectionTime; + vp.magRejectionFactor = p.vqf_magRejectionFactor; + return vp; +} + // this function finds interpolated pose between two poses according to query_time Eigen::Matrix4d getInterpolatedPose(const std::map& trajectory, double query_time); diff --git a/apps/lidar_odometry_step_1/toml_io.h b/apps/lidar_odometry_step_1/toml_io.h index 528d3022..30793c9e 100644 --- a/apps/lidar_odometry_step_1/toml_io.h +++ b/apps/lidar_odometry_step_1/toml_io.h @@ -38,6 +38,31 @@ class TomlIO { "fusionConventionEnu", &LidarOdometryParams::fusionConventionEnu }, { "fusionConventionNed", &LidarOdometryParams::fusionConventionNed }, { "vqf_tauAcc", &LidarOdometryParams::vqf_tauAcc }, + { "vqf_motionBiasEstEnabled", &LidarOdometryParams::vqf_motionBiasEstEnabled }, + { "vqf_restBiasEstEnabled", &LidarOdometryParams::vqf_restBiasEstEnabled }, + { "vqf_biasSigmaInit", &LidarOdometryParams::vqf_biasSigmaInit }, + { "vqf_biasForgettingTime", &LidarOdometryParams::vqf_biasForgettingTime }, + { "vqf_biasClip", &LidarOdometryParams::vqf_biasClip }, + { "vqf_biasSigmaMotion", &LidarOdometryParams::vqf_biasSigmaMotion }, + { "vqf_biasVerticalForgettingFactor", &LidarOdometryParams::vqf_biasVerticalForgettingFactor }, + { "vqf_biasSigmaRest", &LidarOdometryParams::vqf_biasSigmaRest }, + { "vqf_restMinT", &LidarOdometryParams::vqf_restMinT }, + { "vqf_restFilterTau", &LidarOdometryParams::vqf_restFilterTau }, + { "vqf_restThGyr", &LidarOdometryParams::vqf_restThGyr }, + { "vqf_restThAcc", &LidarOdometryParams::vqf_restThAcc }, + { "vqf_useMagnetometer", &LidarOdometryParams::vqf_useMagnetometer }, + { "vqf_tauMag", &LidarOdometryParams::vqf_tauMag }, + { "vqf_magDistRejectionEnabled", &LidarOdometryParams::vqf_magDistRejectionEnabled }, + { "vqf_magCurrentTau", &LidarOdometryParams::vqf_magCurrentTau }, + { "vqf_magRefTau", &LidarOdometryParams::vqf_magRefTau }, + { "vqf_magNormTh", &LidarOdometryParams::vqf_magNormTh }, + { "vqf_magDipTh", &LidarOdometryParams::vqf_magDipTh }, + { "vqf_magNewTime", &LidarOdometryParams::vqf_magNewTime }, + { "vqf_magNewFirstTime", &LidarOdometryParams::vqf_magNewFirstTime }, + { "vqf_magNewMinGyr", &LidarOdometryParams::vqf_magNewMinGyr }, + { "vqf_magMinUndisturbedTime", &LidarOdometryParams::vqf_magMinUndisturbedTime }, + { "vqf_magMaxRejectionTime", &LidarOdometryParams::vqf_magMaxRejectionTime }, + { "vqf_magRejectionFactor", &LidarOdometryParams::vqf_magRejectionFactor }, { "use_motion_from_previous_step", &LidarOdometryParams::use_motion_from_previous_step }, { "nr_iter", &LidarOdometryParams::nr_iter }, { "sliding_window_trajectory_length_threshold", &LidarOdometryParams::sliding_window_trajectory_length_threshold }, @@ -103,7 +128,18 @@ class TomlIO "decimation", "threshould_output_filter", "min_counter_concatenated_trajectory_nodes" } }, - { "ahrs_vqf", { "fusionConventionNwu", "fusionConventionEnu", "fusionConventionNed", "vqf_tauAcc" } }, + { "ahrs_vqf", { + "fusionConventionNwu", "fusionConventionEnu", "fusionConventionNed", + "vqf_tauAcc", + "vqf_motionBiasEstEnabled", "vqf_restBiasEstEnabled", + "vqf_biasSigmaInit", "vqf_biasForgettingTime", "vqf_biasClip", + "vqf_biasSigmaMotion", "vqf_biasVerticalForgettingFactor", "vqf_biasSigmaRest", + "vqf_restMinT", "vqf_restFilterTau", "vqf_restThGyr", "vqf_restThAcc", + "vqf_useMagnetometer", "vqf_tauMag", "vqf_magDistRejectionEnabled", + "vqf_magCurrentTau", "vqf_magRefTau", "vqf_magNormTh", "vqf_magDipTh", + "vqf_magNewTime", "vqf_magNewFirstTime", "vqf_magNewMinGyr", + "vqf_magMinUndisturbedTime", "vqf_magMaxRejectionTime", "vqf_magRejectionFactor" + } }, { "lidar_odometry_control", { "use_motion_from_previous_step", "nr_iter", diff --git a/core/include/imu_preintegration.h b/core/include/imu_preintegration.h index 87938312..7fa27f02 100644 --- a/core/include/imu_preintegration.h +++ b/core/include/imu_preintegration.h @@ -123,14 +123,14 @@ inline const char* to_string(PreintegrationMethod method) { switch (method) { - case PreintegrationMethod::euler_body_frame: return "Euler (body frame)"; - case PreintegrationMethod::trapezoidal_body_frame: return "Trapezoidal (body frame)"; - case PreintegrationMethod::euler_gravity_compensated: return "Euler (gravity compensated)"; - case PreintegrationMethod::trapezoidal_gravity_compensated: return "Trapezoidal (gravity compensated)"; - case PreintegrationMethod::kalman_filter: return "Kalman filter"; - case PreintegrationMethod::euler_gyro_gravity_compensated: return "Euler (AHRS)"; - case PreintegrationMethod::trapezoidal_gyro_gravity_compensated: return "Trapezoidal (AHRS)"; - case PreintegrationMethod::kalman_gyro_gravity_compensated: return "Kalman (AHRS)"; + case PreintegrationMethod::euler_body_frame: return "Euler, no gravity compensation"; + case PreintegrationMethod::trapezoidal_body_frame: return "Trapezoidal, no gravity compensation"; + case PreintegrationMethod::euler_gravity_compensated: return "Euler, gravity comp. (initial trajectory orientations)"; + case PreintegrationMethod::trapezoidal_gravity_compensated: return "Trapezoidal, gravity comp. (initial trajectory orientations)"; + case PreintegrationMethod::kalman_filter: return "Kalman, gravity comp. (initial trajectory orientations)"; + case PreintegrationMethod::euler_gyro_gravity_compensated: return "Euler, gravity comp. (per-worker VQF orientations)"; + case PreintegrationMethod::trapezoidal_gyro_gravity_compensated: return "Trapezoidal, gravity comp. (per-worker VQF orientations)"; + case PreintegrationMethod::kalman_gyro_gravity_compensated: return "Kalman, gravity comp. (per-worker VQF orientations)"; default: return "unknown"; } }