From 00969020d64d883c8447fcfd9ede2e17e5edf509 Mon Sep 17 00:00:00 2001 From: Jakubach Date: Sun, 8 Mar 2026 20:39:30 +0100 Subject: [PATCH 1/5] Disable IMU preintegration by default, fix methods 2-4 regression, cleanup - Default use_imu_preintegration=false (safe default matching pre-preintegration behavior) - VQF bias estimation stays enabled by default (improves orientation quality) - Fix initial_velocity: methods 0-4 use SM-based velocity, methods 5-7 use VQF velocity - Fix gyro unit mismatch in estimate_orientations (rad/s, not deg/s) - Update method names in GUI and to_string to show velocity source - Reorganize GUI: rest detection params under checkbox, remove unnecessary TreeNode - Remove unused Fusion/Madgwick 3rdparty submodule --- 3rdparty/Fusion | 1 - .../lidar_odometry_gui.cpp | 65 +++++++++---------- .../lidar_odometry_utils.h | 2 +- .../lidar_odometry_utils_optimizers.cpp | 65 ++++++++++++------- core/include/imu_preintegration.h | 16 ++--- core/src/imu_preintegration.cpp | 9 ++- 6 files changed, 86 insertions(+), 72 deletions(-) delete mode 160000 3rdparty/Fusion diff --git a/3rdparty/Fusion b/3rdparty/Fusion deleted file mode 160000 index 53c67a79..00000000 --- a/3rdparty/Fusion +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 53c67a79e0d08b82ffa975af28631dbb2a68529c diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp index 965d3add..924b6dea 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp @@ -921,14 +921,14 @@ void settings_gui() 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)" }; + "Euler, no gravity comp., SM velocity", + "Trapezoidal, no gravity comp., SM velocity", + "Euler, gravity comp. (initial traj. orient.), SM velocity", + "Trapezoidal, gravity comp. (initial traj. orient.), SM velocity", + "Kalman, gravity comp. (initial traj. orient.), SM velocity", + "Euler, gravity comp. (per-worker VQF orient.), VQF velocity", + "Trapezoidal, gravity comp. (per-worker VQF orient.), VQF velocity", + "Kalman, gravity comp. (per-worker VQF orient.), VQF velocity" }; 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"); @@ -957,17 +957,6 @@ void settings_gui() 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"); @@ -978,23 +967,29 @@ void settings_gui() 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"); + ImGui::Checkbox("Rest bias estimation", ¶ms.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]", ¶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("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(); } diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.h b/apps/lidar_odometry_step_1/lidar_odometry_utils.h index 18567a21..5fd5f79f 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.h +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.h @@ -188,7 +188,7 @@ struct LidarOdometryParams bool use_removie_imu_bias_from_first_stationary_scan = false; // IMU preintegration - bool use_imu_preintegration = true; + 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 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 2775f691..82ebec55 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -2417,34 +2417,55 @@ bool process_worker_step_1( { IntegrationParams imu_params; - // 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) + auto method = static_cast(params.imu_preintegration_method); + bool is_per_worker_vqf = (method == PreintegrationMethod::euler_gyro_gravity_compensated || + method == PreintegrationMethod::trapezoidal_gyro_gravity_compensated || + method == PreintegrationMethod::kalman_gyro_gravity_compensated); + + if (is_per_worker_vqf) { - double total_imu_time = worker_data.raw_imu_data.back().timestamp - worker_data.raw_imu_data.front().timestamp; - if (total_imu_time > 0.0) + // SM-independent initial_velocity for per-worker VQF methods: + // - 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) + { + double speed = prev_mm_displacement.norm() / total_imu_time; + + 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; + if (forward_global.norm() > 1e-6) + imu_params.initial_velocity = forward_global.normalized() * speed; + else + imu_params.initial_velocity = Eigen::Vector3d::Zero(); + } + } + } + else + { + // For methods using initial trajectory orientations (0-4): + // velocity from previous trajectory segments (original approach) + 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 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(); + 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), + method, worker_data.raw_imu_data, new_trajectory, imu_params); } else diff --git a/core/include/imu_preintegration.h b/core/include/imu_preintegration.h index 7fa27f02..281ab739 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, 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)"; + case PreintegrationMethod::euler_body_frame: return "Euler, no gravity comp., SM velocity"; + case PreintegrationMethod::trapezoidal_body_frame: return "Trapezoidal, no gravity comp., SM velocity"; + case PreintegrationMethod::euler_gravity_compensated: return "Euler, gravity comp. (initial traj. orient.), SM velocity"; + case PreintegrationMethod::trapezoidal_gravity_compensated: return "Trapezoidal, gravity comp. (initial traj. orient.), SM velocity"; + case PreintegrationMethod::kalman_filter: return "Kalman, gravity comp. (initial traj. orient.), SM velocity"; + case PreintegrationMethod::euler_gyro_gravity_compensated: return "Euler, gravity comp. (per-worker VQF orient.), VQF velocity"; + case PreintegrationMethod::trapezoidal_gyro_gravity_compensated: return "Trapezoidal, gravity comp. (per-worker VQF orient.), VQF velocity"; + case PreintegrationMethod::kalman_gyro_gravity_compensated: return "Kalman, gravity comp. (per-worker VQF orient.), VQF velocity"; default: return "unknown"; } } diff --git a/core/src/imu_preintegration.cpp b/core/src/imu_preintegration.cpp index e800efbd..f6ed3490 100644 --- a/core/src/imu_preintegration.cpp +++ b/core/src/imu_preintegration.cpp @@ -74,13 +74,12 @@ std::vector estimate_orientations( } // 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; + // RawIMUData: gyro in rad/s, accel in g 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 }; + 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, From 0670e766b7c648953f6bd6b3c9313e2ae05c1da3 Mon Sep 17 00:00:00 2001 From: Jakubach Date: Sun, 8 Mar 2026 21:20:39 +0100 Subject: [PATCH 2/5] Remove per-worker VQF, use global VQF orientations, add prediction vector visualization - Remove estimate_orientations() and per-worker VQF instance - Methods 5-7 now use orientations from global VQF (via new_trajectory) instead of spawning a fresh VQF per worker with hardcoded params - Only difference between methods 2-4 and 5-7 is velocity source (SM vs VQF) - Add imu_prediction_vector to WorkerData for visualization - Render prediction vectors as yellow arrows (menu: Show prediction vectors) - Update method names to reflect unified orientation source --- .../lidar_odometry_gui.cpp | 39 +++++++++-- .../lidar_odometry_utils_optimizers.cpp | 3 + core/include/Core/structures.h | 1 + core/include/imu_preintegration.h | 16 ++--- core/src/imu_preintegration.cpp | 68 +------------------ 5 files changed, 46 insertions(+), 81 deletions(-) diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp index 924b6dea..6bdc6cb1 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp +++ b/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; @@ -923,12 +924,12 @@ void settings_gui() const char* methods[] = { "Euler, no gravity comp., SM velocity", "Trapezoidal, no gravity comp., SM velocity", - "Euler, gravity comp. (initial traj. orient.), SM velocity", - "Trapezoidal, gravity comp. (initial traj. orient.), SM velocity", - "Kalman, gravity comp. (initial traj. orient.), SM velocity", - "Euler, gravity comp. (per-worker VQF orient.), VQF velocity", - "Trapezoidal, gravity comp. (per-worker VQF orient.), VQF velocity", - "Kalman, gravity comp. (per-worker VQF orient.), VQF 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", ¶ms.imu_preintegration_method, methods, IM_ARRAYSIZE(methods)); } ImGui::InputDouble("VQF tauAcc [s]", ¶ms.vqf_tauAcc, 0.0, 0.0, "%.3f"); @@ -1875,6 +1876,31 @@ void display() glEnd(); } + if (show_prediction_vectors) + { + glLineWidth(2.0f); + glBegin(GL_LINES); + for (const auto& wd : worker_data) + { + if (wd.intermediate_trajectory.size() > 0 && wd.imu_prediction_vector.norm() > 1e-6) + { + const auto& first_pose = wd.intermediate_trajectory[0]; + double x = first_pose(0, 3); + double y = first_pose(1, 3); + double z = first_pose(2, 3); + + // Yellow arrow: prediction vector + glColor3f(1.0f, 1.0f, 0.0f); + glVertex3f(x, y, z); + glVertex3f(x + wd.imu_prediction_vector.x(), + y + wd.imu_prediction_vector.y(), + z + wd.imu_prediction_vector.z()); + } + } + glEnd(); + glLineWidth(1.0f); + } + if (show_trajectory) { glPointSize(3); @@ -2218,6 +2244,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); 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 82ebec55..9fd62ea1 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -2482,6 +2482,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(new_trajectory.size()); + for (int tr = 0; tr < new_trajectory.size(); tr++) { new_trajectory[tr].translation() += mean_shift * tr; diff --git a/core/include/Core/structures.h b/core/include/Core/structures.h index d2d56be5..8bdfb0a6 100644 --- a/core/include/Core/structures.h +++ b/core/include/Core/structures.h @@ -426,5 +426,6 @@ struct WorkerData std::vector> intermediate_trajectory_timestamps; std::vector imu_om_fi_ka; std::vector raw_imu_data; + Eigen::Vector3d imu_prediction_vector = Eigen::Vector3d::Zero(); bool show = false; }; diff --git a/core/include/imu_preintegration.h b/core/include/imu_preintegration.h index 281ab739..a9c63ef4 100644 --- a/core/include/imu_preintegration.h +++ b/core/include/imu_preintegration.h @@ -22,10 +22,6 @@ 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 @@ -125,12 +121,12 @@ inline const char* to_string(PreintegrationMethod method) { case PreintegrationMethod::euler_body_frame: return "Euler, no gravity comp., SM velocity"; case PreintegrationMethod::trapezoidal_body_frame: return "Trapezoidal, no gravity comp., SM velocity"; - case PreintegrationMethod::euler_gravity_compensated: return "Euler, gravity comp. (initial traj. orient.), SM velocity"; - case PreintegrationMethod::trapezoidal_gravity_compensated: return "Trapezoidal, gravity comp. (initial traj. orient.), SM velocity"; - case PreintegrationMethod::kalman_filter: return "Kalman, gravity comp. (initial traj. orient.), SM velocity"; - case PreintegrationMethod::euler_gyro_gravity_compensated: return "Euler, gravity comp. (per-worker VQF orient.), VQF velocity"; - case PreintegrationMethod::trapezoidal_gyro_gravity_compensated: return "Trapezoidal, gravity comp. (per-worker VQF orient.), VQF velocity"; - case PreintegrationMethod::kalman_gyro_gravity_compensated: return "Kalman, gravity comp. (per-worker VQF orient.), VQF velocity"; + case PreintegrationMethod::euler_gravity_compensated: return "Euler, gravity comp., SM velocity"; + case PreintegrationMethod::trapezoidal_gravity_compensated: return "Trapezoidal, gravity comp., SM velocity"; + case PreintegrationMethod::kalman_filter: return "Kalman, gravity comp., SM velocity"; + case PreintegrationMethod::euler_gyro_gravity_compensated: return "Euler, gravity comp., VQF velocity"; + case PreintegrationMethod::trapezoidal_gyro_gravity_compensated: return "Trapezoidal, gravity comp., VQF velocity"; + case PreintegrationMethod::kalman_gyro_gravity_compensated: return "Kalman, gravity comp., VQF velocity"; default: return "unknown"; } } diff --git a/core/src/imu_preintegration.cpp b/core/src/imu_preintegration.cpp index f6ed3490..34ca365a 100644 --- a/core/src/imu_preintegration.cpp +++ b/core/src/imu_preintegration.cpp @@ -4,8 +4,6 @@ #include #include -#include - namespace imu_utils { @@ -41,60 +39,6 @@ bool is_accel_valid(const Eigen::Vector3d& accel_ms2, double threshold) return accel_ms2.norm() < threshold && !has_nan(accel_ms2); } -std::vector estimate_orientations( - const std::vector& raw_imu_data, - const Eigen::Matrix3d& initial_orientation, - const IntegrationParams& params) -{ - std::vector orientations; - orientations.reserve(raw_imu_data.size()); - - // Compute average dt for VQF initialization - double avg_dt = 1.0 / 200.0; // default 200 Hz - if (raw_imu_data.size() >= 2) - { - double total_time = raw_imu_data.back().timestamp - raw_imu_data.front().timestamp; - if (total_time > 0.0) - avg_dt = total_time / static_cast(raw_imu_data.size() - 1); - } - - VQFParams vqf_params; - vqf_params.tauAcc = params.vqf_tauAcc > 0.0 ? params.vqf_tauAcc : 3.0; - VQF vqf(vqf_params, avg_dt); - - orientations.push_back(initial_orientation); - - for (size_t k = 1; k < raw_imu_data.size(); k++) - { - 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( @@ -309,13 +253,8 @@ Eigen::Vector3d ImuPreintegration::create_and_preintegrate( 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]; - + // Use orientations from new_trajectory directly (global VQF from calculate_trajectory, + // anchored to previous worker's SM pose). No per-worker VQF needed. accel_model = std::make_unique(); if (method == PreintegrationMethod::euler_gyro_gravity_compensated) integration_method = std::make_unique(); @@ -323,8 +262,7 @@ Eigen::Vector3d ImuPreintegration::create_and_preintegrate( integration_method = std::make_unique(); else integration_method = std::make_unique(); - - return preint.preintegrate(raw_imu_data, imu_trajectory, *accel_model, *integration_method); + break; } default: std::cerr << "ImuPreintegration: unknown method " << static_cast(method) << std::endl; From ec9c5a74d774a4c352c95ec4d11762f6ff99ac6a Mon Sep 17 00:00:00 2001 From: Jakubach Date: Sun, 8 Mar 2026 21:25:56 +0100 Subject: [PATCH 3/5] Rename is_per_worker_vqf to uses_vqf_velocity --- .../lidar_odometry_utils_optimizers.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 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 9fd62ea1..96f7041e 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -2418,13 +2418,13 @@ bool process_worker_step_1( IntegrationParams imu_params; auto method = static_cast(params.imu_preintegration_method); - bool is_per_worker_vqf = (method == PreintegrationMethod::euler_gyro_gravity_compensated || + bool uses_vqf_velocity = (method == PreintegrationMethod::euler_gyro_gravity_compensated || method == PreintegrationMethod::trapezoidal_gyro_gravity_compensated || method == PreintegrationMethod::kalman_gyro_gravity_compensated); - if (is_per_worker_vqf) + if (uses_vqf_velocity) { - // SM-independent initial_velocity for per-worker VQF methods: + // SM-independent initial_velocity for VQF velocity methods (5-7): // - 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 = From 1227f58c53634ad934232f7203c0aef79234109f Mon Sep 17 00:00:00 2001 From: Jakubach Date: Sun, 8 Mar 2026 21:27:22 +0100 Subject: [PATCH 4/5] Refactor preintegration velocity code, fix prediction vector visibility - Extract common total_imu_time computation - Simplify velocity branches with cleaner structure - Use .back() instead of [size()-1] - Fix prediction vector rendering: magenta color, thicker line, disable depth test --- .../lidar_odometry_gui.cpp | 27 +++---- .../lidar_odometry_utils_optimizers.cpp | 79 ++++++++----------- 2 files changed, 46 insertions(+), 60 deletions(-) diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp index 6bdc6cb1..8aee676c 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp @@ -1878,27 +1878,24 @@ void display() if (show_prediction_vectors) { - glLineWidth(2.0f); + glDisable(GL_DEPTH_TEST); + glLineWidth(3.0f); glBegin(GL_LINES); for (const auto& wd : worker_data) { - if (wd.intermediate_trajectory.size() > 0 && wd.imu_prediction_vector.norm() > 1e-6) - { - const auto& first_pose = wd.intermediate_trajectory[0]; - double x = first_pose(0, 3); - double y = first_pose(1, 3); - double z = first_pose(2, 3); - - // Yellow arrow: prediction vector - glColor3f(1.0f, 1.0f, 0.0f); - glVertex3f(x, y, z); - glVertex3f(x + wd.imu_prediction_vector.x(), - y + wd.imu_prediction_vector.y(), - z + wd.imu_prediction_vector.z()); - } + 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) 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 96f7041e..6b58b42b 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -2416,64 +2416,53 @@ bool process_worker_step_1( if (params.use_imu_preintegration) { IntegrationParams imu_params; - auto method = static_cast(params.imu_preintegration_method); - bool uses_vqf_velocity = (method == PreintegrationMethod::euler_gyro_gravity_compensated || - method == PreintegrationMethod::trapezoidal_gyro_gravity_compensated || - method == PreintegrationMethod::kalman_gyro_gravity_compensated); - if (uses_vqf_velocity) + // 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) { - // SM-independent initial_velocity for VQF velocity methods (5-7): - // - 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) + bool uses_vqf_velocity = (method == PreintegrationMethod::euler_gyro_gravity_compensated || + method == PreintegrationMethod::trapezoidal_gyro_gravity_compensated || + method == PreintegrationMethod::kalman_gyro_gravity_compensated); + + if (uses_vqf_velocity) { - double total_imu_time = worker_data.raw_imu_data.back().timestamp - worker_data.raw_imu_data.front().timestamp; - if (total_imu_time > 0.0) - { - double speed = prev_mm_displacement.norm() / total_imu_time; - - 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; - if (forward_global.norm() > 1e-6) - imu_params.initial_velocity = forward_global.normalized() * speed; - else - imu_params.initial_velocity = Eigen::Vector3d::Zero(); - } + // 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; + + Eigen::Vector3d forward_global = worker_data.intermediate_trajectory[0].linear() * Eigen::Vector3d(1, 0, 0); + forward_global.z() = 0; + imu_params.initial_velocity = (forward_global.norm() > 1e-6) + ? forward_global.normalized() * speed + : Eigen::Vector3d::Zero(); } - } - else - { - // For methods using initial trajectory orientations (0-4): - // velocity from previous trajectory segments (original approach) - 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) + else { - 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; + // 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( - 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(prev_worker_data.intermediate_trajectory.size()); } if (mean_shift.norm() > 1.0) From b3e0014d8e0ff082fa82860e014563cd4ca1a0d5 Mon Sep 17 00:00:00 2001 From: Jakubach Date: Sun, 8 Mar 2026 21:30:07 +0100 Subject: [PATCH 5/5] Rename PreintegrationMethod enum to reflect actual differences - euler_body_frame -> euler_no_gravity_sm_vel - trapezoidal_body_frame -> trapezoidal_no_gravity_sm_vel - euler_gravity_compensated -> euler_gravity_sm_vel - trapezoidal_gravity_compensated -> trapezoidal_gravity_sm_vel - kalman_filter -> kalman_gravity_sm_vel - euler_gyro_gravity_compensated -> euler_gravity_vqf_vel - trapezoidal_gyro_gravity_compensated -> trapezoidal_gravity_vqf_vel - kalman_gyro_gravity_compensated -> kalman_gravity_vqf_vel Fix Eigen ternary operator compile error --- .../lidar_odometry_utils_optimizers.cpp | 13 ++++---- core/include/imu_preintegration.h | 32 +++++++++---------- core/src/imu_preintegration.cpp | 20 ++++++------ 3 files changed, 33 insertions(+), 32 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 6b58b42b..86813c99 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -2425,9 +2425,9 @@ bool process_worker_step_1( if (total_imu_time > 0.0) { - bool uses_vqf_velocity = (method == PreintegrationMethod::euler_gyro_gravity_compensated || - method == PreintegrationMethod::trapezoidal_gyro_gravity_compensated || - method == PreintegrationMethod::kalman_gyro_gravity_compensated); + 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) { @@ -2440,9 +2440,10 @@ bool process_worker_step_1( Eigen::Vector3d forward_global = worker_data.intermediate_trajectory[0].linear() * Eigen::Vector3d(1, 0, 0); forward_global.z() = 0; - imu_params.initial_velocity = (forward_global.norm() > 1e-6) - ? forward_global.normalized() * speed - : Eigen::Vector3d::Zero(); + if (forward_global.norm() > 1e-6) + imu_params.initial_velocity = forward_global.normalized() * speed; + else + imu_params.initial_velocity = Eigen::Vector3d::Zero(); } else { diff --git a/core/include/imu_preintegration.h b/core/include/imu_preintegration.h index a9c63ef4..c585935e 100644 --- a/core/include/imu_preintegration.h +++ b/core/include/imu_preintegration.h @@ -105,28 +105,28 @@ 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 comp., SM velocity"; - case PreintegrationMethod::trapezoidal_body_frame: return "Trapezoidal, no gravity comp., SM velocity"; - case PreintegrationMethod::euler_gravity_compensated: return "Euler, gravity comp., SM velocity"; - case PreintegrationMethod::trapezoidal_gravity_compensated: return "Trapezoidal, gravity comp., SM velocity"; - case PreintegrationMethod::kalman_filter: return "Kalman, gravity comp., SM velocity"; - case PreintegrationMethod::euler_gyro_gravity_compensated: return "Euler, gravity comp., VQF velocity"; - case PreintegrationMethod::trapezoidal_gyro_gravity_compensated: return "Trapezoidal, gravity comp., VQF velocity"; - case PreintegrationMethod::kalman_gyro_gravity_compensated: return "Kalman, gravity comp., VQF velocity"; + 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"; } } diff --git a/core/src/imu_preintegration.cpp b/core/src/imu_preintegration.cpp index 34ca365a..14fbbdf9 100644 --- a/core/src/imu_preintegration.cpp +++ b/core/src/imu_preintegration.cpp @@ -229,36 +229,36 @@ Eigen::Vector3d ImuPreintegration::create_and_preintegrate( switch (method) { - case PreintegrationMethod::euler_body_frame: + case PreintegrationMethod::euler_no_gravity_sm_vel: accel_model = std::make_unique(); integration_method = std::make_unique(); break; - case PreintegrationMethod::trapezoidal_body_frame: + case PreintegrationMethod::trapezoidal_no_gravity_sm_vel: accel_model = std::make_unique(); integration_method = std::make_unique(); break; - case PreintegrationMethod::euler_gravity_compensated: + case PreintegrationMethod::euler_gravity_sm_vel: accel_model = std::make_unique(); integration_method = std::make_unique(); break; - case PreintegrationMethod::trapezoidal_gravity_compensated: + case PreintegrationMethod::trapezoidal_gravity_sm_vel: accel_model = std::make_unique(); integration_method = std::make_unique(); break; - case PreintegrationMethod::kalman_filter: + case PreintegrationMethod::kalman_gravity_sm_vel: 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: + case PreintegrationMethod::euler_gravity_vqf_vel: + 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. accel_model = std::make_unique(); - if (method == PreintegrationMethod::euler_gyro_gravity_compensated) + if (method == PreintegrationMethod::euler_gravity_vqf_vel) integration_method = std::make_unique(); - else if (method == PreintegrationMethod::trapezoidal_gyro_gravity_compensated) + else if (method == PreintegrationMethod::trapezoidal_gravity_vqf_vel) integration_method = std::make_unique(); else integration_method = std::make_unique();