IMU prediction validation (#294)

koide3 · Copilot · web-flow · commit aaddee464d8f · 2026-02-15T22:33:53.000+09:00
* IMU prediction validation

* validate IMU and skip if vel is too small

* Update include/glim/common/imu_validation.hpp

Co-authored-by: Copilot &lt;175728472+Copilot@users.noreply.github.com&gt;

---------

Co-authored-by: Copilot &lt;175728472+Copilot@users.noreply.github.com&gt;
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -71,6 +71,7 @@ add_library(glim SHARED
   src/glim/preprocess/cloud_preprocessor.cpp
   # common process
   src/glim/common/imu_integration.cpp
+  src/glim/common/imu_validation.cpp
   src/glim/common/cloud_deskewing.cpp
   src/glim/common/cloud_covariance_estimation.cpp
   # odometry
diff --git a/config/config_odometry_cpu.json b/config/config_odometry_cpu.json
@@ -30,6 +30,7 @@
     "vgicp_voxelmap_levels": 1,
     "vgicp_voxelmap_scaling_factor": 2.0,
     // Optimizer params
+    "validate_imu": true,
     "save_imu_rate_trajectory": true,
     "num_threads": 2
   }
diff --git a/config/config_odometry_gpu.json b/config/config_odometry_gpu.json
@@ -32,6 +32,7 @@
   // keyframe_delta_(trans|rot)  : Keyframe displacement threshold [m / rad]
   //
   // --- MISC ---
+  // validate_imu                 : If true, perform IMU validation and log the validation results
   // save_imu_rate_trajectory     : If true, save the IMU rate trajectory
   // num_threads                  : Number of threads for odometry estimation
   */
@@ -64,6 +65,7 @@
     "keyframe_delta_rot": 0.5,
     "keyframe_entropy_thresh": 0.99,
     // MISC
+    "validate_imu": true,
     "save_imu_rate_trajectory": true,
     "num_threads": 2
   }
diff --git a/include/glim/common/imu_validation.hpp b/include/glim/common/imu_validation.hpp
@@ -0,0 +1,39 @@
+#pragma once
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <spdlog/spdlog.h>
+#include <gtsam_points/util/runnning_statistics.hpp>
+
+namespace glim {
+
+class IMUValidation {
+public:
+  IMUValidation(const std::shared_ptr<spdlog::logger>& logger, bool enabled = true);
+  ~IMUValidation();
+
+  void validate(
+    const Eigen::Isometry3d& last_T_world_imu,
+    const Eigen::Vector3d& last_v_world_imu,
+    const Eigen::Isometry3d& predicted_T_world_imu,
+    const Eigen::Vector3d& predicted_v_world_imu,
+    const Eigen::Isometry3d& corrected_T_world_imu,
+    const Eigen::Vector3d& corrected_v_world_imu,
+    double dt);
+
+  void validate(const Eigen::Matrix<double, 6, 1>& imu_bias);
+
+private:
+  const bool enabled;
+  std::shared_ptr<spdlog::logger> logger;
+
+  int num_validations;
+  gtsam_points::RunningStatistics<Eigen::Array3d> noimu_error_stats;  // rotation, translation, velocity
+  gtsam_points::RunningStatistics<Eigen::Array3d> imu_error_stats;    // rotation, translation, velocity
+  Eigen::Array3i imu_better_counts;                                   // rotation, translation, velocity
+
+  int num_bias_validations;
+  gtsam_points::RunningStatistics<Eigen::Array<double, 6, 1>> imu_bias_stats;  // Acc, Gyro bias
+};
+
+}  // namespace glim
diff --git a/include/glim/odometry/odometry_estimation_imu.hpp b/include/glim/odometry/odometry_estimation_imu.hpp
@@ -23,6 +23,7 @@ class IncrementalFixedLagSmootherExtWithFallback;
 namespace glim {
 
 class IMUIntegration;
+class IMUValidation;
 class CloudDeskewing;
 class CloudCovarianceEstimation;
 class InitialStateEstimation;
@@ -58,6 +59,7 @@ struct OdometryEstimationIMUParams {
   double isam2_relinearize_thresh;
 
   // Logging params
+  bool validate_imu;
   bool save_imu_rate_trajectory;
 
   int num_threads;                  // Number of threads for preprocessing and per-factor parallelism
@@ -103,6 +105,7 @@ class OdometryEstimationIMU : public OdometryEstimationBase {
   // Utility classes
   std::unique_ptr<InitialStateEstimation> init_estimation;
   std::unique_ptr<IMUIntegration> imu_integration;
+  std::unique_ptr<IMUValidation> imu_validation;
   std::unique_ptr<CloudDeskewing> deskewing;
   std::unique_ptr<CloudCovarianceEstimation> covariance_estimation;
 
diff --git a/src/glim/common/imu_validation.cpp b/src/glim/common/imu_validation.cpp
@@ -0,0 +1,175 @@
+#include <glim/common/imu_validation.hpp>
+
+namespace glim {
+
+IMUValidation::IMUValidation(const std::shared_ptr<spdlog::logger>& logger, bool enabled) : enabled(enabled), logger(logger) {
+  num_validations = 0;
+  num_bias_validations = 0;
+  imu_better_counts.setZero();
+}
+
+IMUValidation::~IMUValidation() {}
+
+void IMUValidation::validate(
+  const Eigen::Isometry3d& last_T_world_imu,
+  const Eigen::Vector3d& last_v_world_imu,
+  const Eigen::Isometry3d& predicted_T_world_imu,
+  const Eigen::Vector3d& predicted_v_world_imu,
+  const Eigen::Isometry3d& corrected_T_world_imu,
+  const Eigen::Vector3d& corrected_v_world_imu,
+  double dt) {
+  if (!enabled) {
+    return;
+  }
+
+  logger->debug("validating IMU prediction (|v|={:.3f} m/s)", corrected_v_world_imu.norm());
+  if (corrected_v_world_imu.norm() < 0.1) {
+    // Skip validation when the velocity is too small, as the errors can be dominated by noise.
+    logger->debug("skipping IMU validation due to small velocity");
+    return;
+  }
+
+  Eigen::Isometry3d noimu_T_world_imu = last_T_world_imu;
+  noimu_T_world_imu.translation() += last_v_world_imu * dt;
+  const auto& noimu_v_world_imu = last_v_world_imu;
+
+  const Eigen::Isometry3d noimu_error_T = corrected_T_world_imu.inverse() * noimu_T_world_imu;
+  const Eigen::Vector3d noimu_error_v = corrected_v_world_imu - noimu_v_world_imu;
+
+  const Eigen::Isometry3d imu_error_T = corrected_T_world_imu.inverse() * predicted_T_world_imu;
+  const Eigen::Vector3d imu_error_v = corrected_v_world_imu - predicted_v_world_imu;
+
+  const double noimu_error_rot = Eigen::AngleAxisd(noimu_error_T.linear()).angle();
+  const double noimu_error_trans = noimu_error_T.translation().norm();
+  const double noimu_error_vel = noimu_error_v.norm();
+  const Eigen::Array3d noimu_errors(noimu_error_rot, noimu_error_trans, noimu_error_vel);
+  noimu_error_stats.add(noimu_errors);
+
+  const double imu_error_rot = Eigen::AngleAxisd(imu_error_T.linear()).angle();
+  const double imu_error_trans = imu_error_T.translation().norm();
+  const double imu_error_vel = imu_error_v.norm();
+  const Eigen::Array3d imu_errors(imu_error_rot, imu_error_trans, imu_error_vel);
+  imu_error_stats.add(imu_errors);
+
+  imu_better_counts += (imu_errors < noimu_errors).cast<int>();
+  num_validations++;
+
+  // Print statistics every 64 validations
+  if (num_validations % 64 == 0) {
+    const Eigen::Array3d imu_better_ratios = imu_better_counts.cast<double>() / num_validations;
+
+    bool good_imu = true;
+
+    // Heuristic rules to determine if IMU prediction is good or not.
+    // Reference: os1_128_01 dataset, better ratios for rot=1.00, trans=0.55, vel=0.66
+    good_imu &= imu_better_ratios[0] > 0.7;  // Rotation
+    good_imu &= imu_better_ratios[1] > 0.4;  // Translation
+    good_imu &= imu_better_ratios[2] > 0.5;  // Velocity
+
+    auto log_level = good_imu ? spdlog::level::debug : spdlog::level::warn;
+
+    const Eigen::Array3d noimu_error_means = noimu_error_stats.mean();
+    const Eigen::Array3d noimu_error_stds = noimu_error_stats.std();
+    const Eigen::Array3d imu_error_means = imu_error_stats.mean();
+    const Eigen::Array3d imu_error_stds = imu_error_stats.std();
+
+    if (!good_imu) {
+      logger->log(log_level, "IMU prediction is not good.");
+      logger->log(log_level, "Possibly T_lidar_imu is not accurate or IMU bias is not well estimated.");
+    }
+    logger->log(log_level, "IMU validation results:");
+    logger->log(log_level, "num_validations={}", num_validations);
+    logger->log(
+      log_level,
+      "No-IMU errors rot={:.3f} +- {:.3f} deg, trans={:.3f} +- {:.3f} m, vel={:.3f} +- {:.3f} m/s",
+      noimu_error_means[0] * 180.0 / M_PI,
+      noimu_error_stds[0] * 180.0 / M_PI,
+      noimu_error_means[1],
+      noimu_error_stds[1],
+      noimu_error_means[2],
+      noimu_error_stds[2]);
+    logger->log(
+      log_level,
+      "IMU errors rot={:.3f} +- {:.3f} deg, trans={:.3f} +- {:.3f} m, vel={:.3f} +- {:.3f} m/s",
+      imu_error_means[0] * 180.0 / M_PI,
+      imu_error_stds[0] * 180.0 / M_PI,
+      imu_error_means[1],
+      imu_error_stds[1],
+      imu_error_means[2],
+      imu_error_stds[2]);
+    logger->log(log_level, "IMU better ratios rot={:.2f}, trans={:.2f}, vel={:.2f}", imu_better_ratios[0], imu_better_ratios[1], imu_better_ratios[2]);
+  }
+}
+
+void IMUValidation::validate(const Eigen::Matrix<double, 6, 1>& imu_bias) {
+  if (!enabled) {
+    return;
+  }
+
+  num_bias_validations++;
+  if (num_bias_validations % 64) {
+    return;
+  }
+
+  const Eigen::Array3d bias_acc = imu_bias.head<3>();
+  const Eigen::Array3d bias_gyro = imu_bias.tail<3>();
+
+  bool good_imu = true;
+  good_imu &= bias_acc.cwiseAbs().maxCoeff() < 0.5;   // Acceleration bias should be small (less than 0.5 m/s^2)
+  good_imu &= bias_gyro.cwiseAbs().maxCoeff() < 0.5;  // Gyro bias should be small (less than 0.5 rad/s)
+
+  imu_bias_stats.add(imu_bias);
+  const Eigen::Array<double, 6, 1> bias_mean = imu_bias_stats.mean();
+  const Eigen::Array<double, 6, 1> bias_std = imu_bias_stats.std();
+  const Eigen::Array<double, 6, 1> bias_max = imu_bias_stats.max();
+  const Eigen::Array<double, 6, 1> bias_min = imu_bias_stats.min();
+
+  auto log_level = good_imu ? spdlog::level::debug : spdlog::level::warn;
+  if (!good_imu) {
+    logger->log(log_level, "IMU bias estimation seems inaccurate.");
+    logger->log(log_level, "Possibly T_lidar_imu is not accurate or LiDAR-IMU synchronization is not correct.");
+  }
+  logger->log(log_level, "IMU bias validation results:");
+  logger->log(log_level, "num_bias_validations={}", num_bias_validations);
+  logger->log(
+    log_level,
+    "bias_acc=[{:.3f}, {:.3f}, {:.3f}] m/s^2, bias_gyro=[{:.3f}, {:.3f}, {:.3f}] rad/s",
+    bias_acc[0],
+    bias_acc[1],
+    bias_acc[2],
+    bias_gyro[0],
+    bias_gyro[1],
+    bias_gyro[2]);
+  logger->log(
+    log_level,
+    "acc_stat ax={:.3f} +- {:.3f} [min={:.3f}, max={:.3f}], ay={:.3f} +- {:.3f} [min={:.3f}, max={:.3f}], az={:.3f} +- {:.3f} [min={:.3f}, max={:.3f}] m/s^2",
+    bias_mean[0],
+    bias_std[0],
+    bias_min[0],
+    bias_max[0],
+    bias_mean[1],
+    bias_std[1],
+    bias_min[1],
+    bias_max[1],
+    bias_mean[2],
+    bias_std[2],
+    bias_min[2],
+    bias_max[2]);
+  logger->log(
+    log_level,
+    "gyro_stat wx={:.3f} +- {:.3f} [min={:.3f}, max={:.3f}], wy={:.3f} +- {:.3f} [min={:.3f}, max={:.3f}], wz={:.3f} +- {:.3f} [min={:.3f}, max={:.3f}] rad/s",
+    bias_mean[3],
+    bias_std[3],
+    bias_min[3],
+    bias_max[3],
+    bias_mean[4],
+    bias_std[4],
+    bias_min[4],
+    bias_max[4],
+    bias_mean[5],
+    bias_std[5],
+    bias_min[5],
+    bias_max[5]);
+}
+
+}  // namespace glim
diff --git a/src/glim/odometry/odometry_estimation_imu.cpp b/src/glim/odometry/odometry_estimation_imu.cpp
@@ -13,6 +13,7 @@
 #include <glim/util/config.hpp>
 #include <glim/util/convert_to_string.hpp>
 #include <glim/common/imu_integration.hpp>
+#include <glim/common/imu_validation.hpp>
 #include <glim/common/cloud_deskewing.hpp>
 #include <glim/common/cloud_covariance_estimation.hpp>
 #include <glim/odometry/initial_state_estimation.hpp>
@@ -61,6 +62,7 @@ OdometryEstimationIMUParams::OdometryEstimationIMUParams() {
   isam2_relinearize_skip = config.param<int>("odometry_estimation", "isam2_relinearize_skip", 1);
   isam2_relinearize_thresh = config.param<double>("odometry_estimation", "isam2_relinearize_thresh", 0.1);
 
+  validate_imu = config.param<bool>("odometry_estimation", "validate_imu", true);
   save_imu_rate_trajectory = config.param<bool>("odometry_estimation", "save_imu_rate_trajectory", false);
 
   num_threads = config.param<int>("odometry_estimation", "num_threads", 4);
@@ -88,6 +90,7 @@ OdometryEstimationIMU::OdometryEstimationIMU(std::unique_ptr<OdometryEstimationI
   }
 
   imu_integration.reset(new IMUIntegration);
+  imu_validation.reset(new IMUValidation(logger, params->validate_imu));
   deskewing.reset(new CloudDeskewing);
   covariance_estimation.reset(new CloudCovarianceEstimation(params->num_threads));
 
@@ -345,6 +348,17 @@ EstimationFrame::ConstPtr OdometryEstimationIMU::insert_frame(const Preprocessed
   // Update frames
   update_frames(current, new_factors);
 
+  // Check if IMU prediction is good or not
+  imu_validation->validate(
+    Eigen::Isometry3d(last_T_world_imu.matrix()),
+    last_v_world_imu,
+    Eigen::Isometry3d(predicted_T_world_imu.matrix()),
+    predicted_v_world_imu,
+    new_frame->T_world_imu,
+    new_frame->v_world_imu,
+    new_frame->stamp - last_stamp);
+  imu_validation->validate(new_frame->imu_bias);
+
   std::vector<EstimationFrame::ConstPtr> active_frames(frames.inner_begin(), frames.inner_end());
   Callbacks::on_update_new_frame(active_frames.back());
   Callbacks::on_update_frames(active_frames);

Original file line number	Diff line number	Diff line change
`@@ -30,6 +30,7 @@`
`30`	`30`	`"vgicp_voxelmap_levels": 1,`
`31`	`31`	`"vgicp_voxelmap_scaling_factor": 2.0,`
`32`	`32`	`// Optimizer params`
	`33`	`+ "validate_imu": true,`
`33`	`34`	`"save_imu_rate_trajectory": true,`
`34`	`35`	`"num_threads": 2`
`35`	`36`	`}`