Restore per-worker VQF for methods 5-7

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
         {

core/include/imu_preintegration.h

@@ -4,6 +4,7 @@
 #include <Eigen/Eigen>
 #include <memory>
 #include <vector>
+#include <vqf.hpp>
 
 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<Eigen::Matrix3d> estimate_orientations(
+    const std::vector<RawIMUData>& 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<RawIMUData>& raw_imu_data,
         const std::vector<Eigen::Affine3d>& new_trajectory,
-        const IntegrationParams& params = IntegrationParams());
+        const IntegrationParams& params = IntegrationParams(),
+        const VQFParams& vqf_params = VQFParams());
 };

core/src/imu_preintegration.cpp

@@ -4,16 +4,96 @@
 #include <cmath>
 #include <iostream>
 
+#include <vqf.hpp>
+
 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<Eigen::Matrix3d> estimate_orientations(
+    const std::vector<RawIMUData>& raw_imu_data,
+    const Eigen::Matrix3d& initial_orientation,
+    const IntegrationParams& params,
+    const VQFParams& vqf_params)
+{
+    std::vector<Eigen::Matrix3d> 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<double>(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<RawIMUData>& raw_imu_data,
     const std::vector<Eigen::Affine3d>& new_trajectory,
-    const IntegrationParams& params)
+    const IntegrationParams& params,
+    const VQFParams& vqf_params)
 {
     ImuPreintegration preint;
     preint.params = params;
@@ -223,16 +304,23 @@ 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<Eigen::Affine3d> 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<GravityCompensatedAcceleration>();
         if (method == PreintegrationMethod::euler_gravity_vqf_vel)
             integration_method = std::make_unique<EulerIntegration>();
         else if (method == PreintegrationMethod::trapezoidal_gravity_vqf_vel)
             integration_method = std::make_unique<TrapezoidalIntegration>();
         else
             integration_method = std::make_unique<KalmanFilterIntegration>();
-        break;
+
+        return preint.preintegrate(raw_imu_data, imu_trajectory, *accel_model, *integration_method);
     }
     default:
         std::cerr << "ImuPreintegration: unknown method " << static_cast<int>(method) << std::endl;