diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp index 86813c99..f4caecf7 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -2456,7 +2456,7 @@ bool process_worker_step_1( } mean_shift = ImuPreintegration::create_and_preintegrate( - method, worker_data.raw_imu_data, new_trajectory, imu_params); + method, worker_data.raw_imu_data, new_trajectory, imu_params, buildVQFParams(params)); } else { diff --git a/core/include/imu_preintegration.h b/core/include/imu_preintegration.h index 554f3994..876a1163 100644 --- a/core/include/imu_preintegration.h +++ b/core/include/imu_preintegration.h @@ -4,6 +4,7 @@ #include #include #include +#include struct IntegrationParams { @@ -22,6 +23,11 @@ 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, + const VQFParams& vqf_params = VQFParams()); } // namespace imu_utils class AccelerationModel @@ -137,5 +143,6 @@ class ImuPreintegration PreintegrationMethod method, const std::vector& raw_imu_data, const std::vector& new_trajectory, - const IntegrationParams& params = IntegrationParams()); + const IntegrationParams& params = IntegrationParams(), + const VQFParams& vqf_params = VQFParams()); }; diff --git a/core/src/imu_preintegration.cpp b/core/src/imu_preintegration.cpp index fecb7efe..5e200ee5 100644 --- a/core/src/imu_preintegration.cpp +++ b/core/src/imu_preintegration.cpp @@ -4,16 +4,96 @@ #include #include +#include + namespace imu_utils { - Eigen::Vector3d convert_accel_to_ms2(const Eigen::Vector3d& raw, bool units_in_g, double g) +Eigen::Vector3d convert_accel_to_ms2(const Eigen::Vector3d& raw, bool units_in_g, double g) +{ + if (units_in_g) + return raw * g; + return raw; +} + +Eigen::Vector3d convert_gyro_to_rads(const Eigen::Vector3d& raw, bool units_in_deg) +{ + if (units_in_deg) + return raw * (M_PI / 180.0); + return raw; +} + +double safe_dt(double t_prev, double t_curr, double max_dt) +{ + double dt = t_curr - t_prev; + if (dt <= 0.0 || std::isnan(dt)) + return 0.0; + return std::min(dt, max_dt); +} + +bool has_nan(const Eigen::Vector3d& v) +{ + return std::isnan(v.x()) || std::isnan(v.y()) || std::isnan(v.z()); +} + +bool is_accel_valid(const Eigen::Vector3d& accel_ms2, double threshold) +{ + return accel_ms2.norm() < threshold && !has_nan(accel_ms2); +} + +std::vector estimate_orientations( + const std::vector& raw_imu_data, + const Eigen::Matrix3d& initial_orientation, + const IntegrationParams& params, + const VQFParams& vqf_params) +{ + std::vector orientations; + orientations.reserve(raw_imu_data.size()); + + // Compute average dt for VQF initialization + double avg_dt = 1.0 / 200.0; // default 200 Hz + if (raw_imu_data.size() >= 2) { - if (units_in_g) - return raw * g; - return raw; + 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); } + VQF vqf(vqf_params, avg_dt); + + 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(orientations.back()); + continue; + } + + // VQF expects: gyro in rad/s, acc in m/s² + // RawIMUData: gyro in rad/s, accel in g + const double g = 9.81; + vqf_real_t gyr[3] = { + raw_imu_data[k].guroscopes.x(), + raw_imu_data[k].guroscopes.y(), + raw_imu_data[k].guroscopes.z() }; + vqf_real_t acc[3] = { + raw_imu_data[k].accelerometers.x() * g, + raw_imu_data[k].accelerometers.y() * g, + raw_imu_data[k].accelerometers.z() * g }; + + vqf.update(gyr, acc); + + vqf_real_t quat[4]; // [w, x, y, z] + vqf.getQuat6D(quat); + Eigen::Quaterniond q(quat[0], quat[1], quat[2], quat[3]); + orientations.push_back(q.toRotationMatrix()); + } + return orientations; +} + } // namespace imu_utils Eigen::Vector3d BodyFrameAcceleration::compute( @@ -189,7 +269,8 @@ Eigen::Vector3d ImuPreintegration::create_and_preintegrate( PreintegrationMethod method, const std::vector& raw_imu_data, const std::vector& new_trajectory, - const IntegrationParams& params) + const IntegrationParams& params, + const VQFParams& vqf_params) { ImuPreintegration preint; preint.params = params; @@ -223,8 +304,14 @@ Eigen::Vector3d ImuPreintegration::create_and_preintegrate( case PreintegrationMethod::trapezoidal_gravity_vqf_vel: case PreintegrationMethod::kalman_gravity_vqf_vel: { - // Use orientations from new_trajectory directly (global VQF from calculate_trajectory, - // anchored to previous worker's SM pose). No per-worker VQF needed. + // Per-worker VQF: estimate local orientations from IMU data + auto orientations = imu_utils::estimate_orientations( + raw_imu_data, new_trajectory[0].rotation(), params, vqf_params); + + std::vector imu_trajectory = new_trajectory; + for (size_t k = 0; k < imu_trajectory.size() && k < orientations.size(); k++) + imu_trajectory[k].linear() = orientations[k]; + accel_model = std::make_unique(); if (method == PreintegrationMethod::euler_gravity_vqf_vel) integration_method = std::make_unique(); @@ -232,7 +319,8 @@ Eigen::Vector3d ImuPreintegration::create_and_preintegrate( integration_method = std::make_unique(); else integration_method = std::make_unique(); - break; + + return preint.preintegrate(raw_imu_data, imu_trajectory, *accel_model, *integration_method); } default: std::cerr << "ImuPreintegration: unknown method " << static_cast(method) << std::endl;