Disable IMU preintegration by default

apps/lidar_odometry_step_1/lidar_odometry_gui.cpp

@@ -145,6 +145,7 @@ int dec_reference_points = 100;
 bool show_initial_points = true;
 bool show_trajectory = true;
 bool show_trajectory_as_axes = false;
+bool show_prediction_vectors = false;
 bool show_covs_indoor = false;
 bool show_covs_outdoor = false;
 int dec_covs = 10;
@@ -916,14 +917,15 @@ void settings_gui()
             ImGui::Checkbox("Use IMU preintegration", &params.use_imu_preintegration);
             if (params.use_imu_preintegration)
             {
-                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)" };
+                const char* methods[] = {
+                    "Euler, no gravity comp., SM velocity",
+                    "Trapezoidal, no gravity comp., SM velocity",
+                    "Euler, gravity comp., SM velocity",
+                    "Trapezoidal, gravity comp., SM velocity",
+                    "Kalman, gravity comp., SM velocity",
+                    "Euler, gravity comp., VQF velocity",
+                    "Trapezoidal, gravity comp., VQF velocity",
+                    "Kalman, gravity comp., VQF velocity" };
                 ImGui::Combo("IMU preintegration method", &params.imu_preintegration_method, methods, IM_ARRAYSIZE(methods));
             }
             ImGui::InputDouble("VQF tauAcc [s]", &params.vqf_tauAcc, 0.0, 0.0, "%.3f");
@@ -959,21 +961,6 @@ void settings_gui()
                             "decays to zero. Default: 0.0001");
                 }
 
-                ImGui::Checkbox("Rest bias estimation", &params.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]", &params.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]", &params.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");
@@ -988,31 +975,29 @@ void settings_gui()
                         "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]", &params.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]", &params.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]", &params.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]", &params.vqf_restThAcc, 0.0, 0.0, "%.2f");
+                ImGui::Checkbox("Rest bias estimation", &params.vqf_restBiasEstEnabled);
                 if (ImGui::IsItemHovered())
-                    ImGui::SetTooltip(
-                        "Acceleration threshold for rest detection.\nDeviation norm between measurement and reference must be below "
-                        "threshold.\nDefault: 0.5");
+                    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]", &params.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("Rest min time [s]", &params.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]", &params.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]", &params.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]", &params.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();
             }
 
@@ -1919,6 +1904,28 @@ void display()
         glEnd();
     }
 
+    if (show_prediction_vectors)
+    {
+        glDisable(GL_DEPTH_TEST);
+        glLineWidth(3.0f);
+        glBegin(GL_LINES);
+        for (const auto& wd : worker_data)
+        {
+            if (wd.intermediate_trajectory.empty() || wd.imu_prediction_vector.norm() < 1e-6)
+                continue;
+
+            Eigen::Vector3d start = wd.intermediate_trajectory.front().translation();
+            Eigen::Vector3d end = start + wd.imu_prediction_vector;
+
+            glColor3f(1.0f, 0.0f, 1.0f);  // magenta
+            glVertex3d(start.x(), start.y(), start.z());
+            glVertex3d(end.x(), end.y(), end.z());
+        }
+        glEnd();
+        glLineWidth(1.0f);
+        glEnable(GL_DEPTH_TEST);
+    }
+
     if (show_trajectory)
     {
         glPointSize(3);
@@ -2262,6 +2269,7 @@ void display()
                 ImGui::MenuItem("Show initial points", nullptr, &show_initial_points);
                 ImGui::MenuItem("Show trajectory", nullptr, &show_trajectory);
                 ImGui::MenuItem("Show trajectory as axes", nullptr, &show_trajectory_as_axes);
+                ImGui::MenuItem("Show prediction vectors", nullptr, &show_prediction_vectors);
                 ImGui::MenuItem("Show compass/ruler", "key C", &compass_ruler);
 
                 ImGui::MenuItem("Lock Z", "Shift + Z", &lock_z, !is_ortho);

apps/lidar_odometry_step_1/lidar_odometry_utils.h

@@ -188,9 +188,8 @@ struct LidarOdometryParams
     bool use_removie_imu_bias_from_first_stationary_scan = false;
 
     // IMU preintegration
-    bool use_imu_preintegration = true;
-    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
+    bool use_imu_preintegration = false;
+    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;

apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp

@@ -2416,41 +2416,54 @@ bool process_worker_step_1(
         if (params.use_imu_preintegration)
         {
             IntegrationParams imu_params;
+            auto method = static_cast<PreintegrationMethod>(params.imu_preintegration_method);
 
-            // 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();
-
+            // Compute IMU time span for velocity estimation
+            double total_imu_time = 0.0;
             if (worker_data.raw_imu_data.size() >= 2)
+                total_imu_time = worker_data.raw_imu_data.back().timestamp - worker_data.raw_imu_data.front().timestamp;
+
+            if (total_imu_time > 0.0)
             {
-                double total_imu_time = worker_data.raw_imu_data.back().timestamp - worker_data.raw_imu_data.front().timestamp;
-                if (total_imu_time > 0.0)
+                bool uses_vqf_velocity = (method == PreintegrationMethod::euler_gravity_vqf_vel ||
+                                          method == PreintegrationMethod::trapezoidal_gravity_vqf_vel ||
+                                          method == PreintegrationMethod::kalman_gravity_vqf_vel);
+
+                if (uses_vqf_velocity)
                 {
+                    // Methods 5-7: SM-independent velocity
+                    // Direction from VQF AHRS, speed from motion model displacement
+                    Eigen::Vector3d prev_mm_displacement =
+                        prev_worker_data.intermediate_trajectory_motion_model.back().translation() -
+                        prev_worker_data.intermediate_trajectory_motion_model.front().translation();
                     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
+                    Eigen::Vector3d forward_global = worker_data.intermediate_trajectory[0].linear() * Eigen::Vector3d(1, 0, 0);
+                    forward_global.z() = 0;
                     if (forward_global.norm() > 1e-6)
                         imu_params.initial_velocity = forward_global.normalized() * speed;
                     else
                         imu_params.initial_velocity = Eigen::Vector3d::Zero();
                 }
+                else
+                {
+                    // Methods 0-4: velocity from previous SM trajectory
+                    Eigen::Vector3d prev_displacement =
+                        prev_worker_data.intermediate_trajectory.back().translation() -
+                        prev_prev_worker_data.intermediate_trajectory.back().translation();
+                    imu_params.initial_velocity = prev_displacement / total_imu_time;
+                }
             }
 
             mean_shift = ImuPreintegration::create_and_preintegrate(
-                static_cast<PreintegrationMethod>(params.imu_preintegration_method), worker_data.raw_imu_data, new_trajectory, imu_params);
+                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 /= (prev_worker_data.intermediate_trajectory.size());
+            // No preintegration: simple velocity from previous two workers
+            mean_shift = (prev_worker_data.intermediate_trajectory.back().translation() -
+                          prev_prev_worker_data.intermediate_trajectory.back().translation()) /
+                         static_cast<double>(prev_worker_data.intermediate_trajectory.size());
         }
 
         if (mean_shift.norm() > 1.0)
@@ -2459,6 +2472,9 @@ bool process_worker_step_1(
             mean_shift = Eigen::Vector3d(0.0, 0.0, 0.0);
         }
 
+        // Store prediction vector for visualization (total displacement over worker)
+        worker_data.imu_prediction_vector = mean_shift * static_cast<double>(new_trajectory.size());
+
         for (int tr = 0; tr < new_trajectory.size(); tr++)
         {
             new_trajectory[tr].translation() += mean_shift * tr;

core/include/Core/structures.h

@@ -426,5 +426,6 @@ struct WorkerData
     std::vector<std::pair<double, double>> intermediate_trajectory_timestamps;
     std::vector<Eigen::Vector3d> imu_om_fi_ka;
     std::vector<RawIMUData> raw_imu_data;
+    Eigen::Vector3d imu_prediction_vector = Eigen::Vector3d::Zero();
     bool show = false;
 };

core/include/imu_preintegration.h

@@ -17,13 +17,11 @@ struct IntegrationParams
 
 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);
-    std::vector<Eigen::Matrix3d> estimate_orientations(
-        const std::vector<RawIMUData>& raw_imu_data, const Eigen::Matrix3d& initial_orientation, const IntegrationParams& params);
+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
@@ -98,38 +96,29 @@ class KalmanFilterIntegration : public IntegrationMethod
 
 enum class PreintegrationMethod
 {
-    euler_body_frame = 0,
-    trapezoidal_body_frame = 1,
-    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,
+    euler_no_gravity_sm_vel = 0,
+    trapezoidal_no_gravity_sm_vel = 1,
+    euler_gravity_sm_vel = 2,
+    trapezoidal_gravity_sm_vel = 3,
+    kalman_gravity_sm_vel = 4,
+    euler_gravity_vqf_vel = 5,
+    trapezoidal_gravity_vqf_vel = 6,
+    kalman_gravity_vqf_vel = 7,
 };
 
 inline const char* to_string(PreintegrationMethod method)
 {
     switch (method)
     {
-    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";
+    case PreintegrationMethod::euler_no_gravity_sm_vel: return "Euler, no gravity comp., SM velocity";
+    case PreintegrationMethod::trapezoidal_no_gravity_sm_vel: return "Trapezoidal, no gravity comp., SM velocity";
+    case PreintegrationMethod::euler_gravity_sm_vel: return "Euler, gravity comp., SM velocity";
+    case PreintegrationMethod::trapezoidal_gravity_sm_vel: return "Trapezoidal, gravity comp., SM velocity";
+    case PreintegrationMethod::kalman_gravity_sm_vel: return "Kalman, gravity comp., SM velocity";
+    case PreintegrationMethod::euler_gravity_vqf_vel: return "Euler, gravity comp., VQF velocity";
+    case PreintegrationMethod::trapezoidal_gravity_vqf_vel: return "Trapezoidal, gravity comp., VQF velocity";
+    case PreintegrationMethod::kalman_gravity_vqf_vel: return "Kalman, gravity comp., VQF velocity";
+    default: return "unknown";
     }
 }