Merge pull request #112 from rsasaki0109/agent/koide-imu-yaw-prediction

Validate Koide IMU rotation prediction

Author: rsasaki0109

CMakeLists.txt

@@ -749,6 +749,10 @@ if(BUILD_TESTING)
     NAME analyze_koide_imu_consistency
     COMMAND ${Python3_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/test/test_analyze_koide_imu_consistency.py
   )
+  add_test(
+    NAME koide_imu_yaw_prediction_analysis
+    COMMAND ${Python3_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/test/test_koide_imu_yaw_prediction_analysis.py
+  )
   add_test(
     NAME benchmark_compare_runs
     COMMAND ${Python3_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/test/test_benchmark_compare_runs.py

experiments/imu_yaw_prediction/README.md

@@ -0,0 +1,33 @@
+# Koide IMU rotation prediction experiment
+
+This experiment checks whether gyro-only relative rotation can safely seed local NDT tracking.
+It does not add global localization and does not integrate acceleration or position.
+
+`run_dataset_analysis.py` analyzes all 11 Koide sequences. It compares raw identity axes,
+signed-axis permutations, and a fitted Wahba rotation; searches LiDAR/IMU time offset; reports
+gravity convention, apparent bias, static-TF agreement, and timestamp coverage. The standalone
+`imu_rotation_prediction.hpp` candidate uses bounded interpolation, trapezoidal SO(3)
+integration, and coverage/duration/sample-gap rejection. Its test can be run with:
+
+```bash
+g++ -std=c++17 -I/usr/include/eigen3 -Iexperiments/imu_yaw_prediction \
+  experiments/imu_yaw_prediction/test_imu_rotation_prediction.cpp -o /tmp/test_imu_rotation_prediction
+/tmp/test_imu_rotation_prediction
+```
+
+The full offline result is stored outside the repository at
+`/media/sasaki/aiueo/datasets/koide_hard_localization/generated/imu_yaw_validation_20260714`.
+The compact result and runtime A/B gates are in `results.json`.
+
+## Decision
+
+Do not promote this candidate to runtime. Indoor `indoor_easy_01` regressed from 0.053 m to
+1.314 m translation RMSE and from 1.68 to 2.42 degrees rotation RMSE. Outdoor runs were close,
+but `outdoor_kidnap_a` also slightly worsened final rotation error. The full-loop indoor runs
+were therefore stopped by the early regression gate rather than spending additional runtime on
+an already rejected method. The production LiDAR-only path and global-localization behavior are
+unchanged.
+
+One contaminated run is intentionally excluded: an interrupted benchmark left a rosbag process
+on the same ROS domain, producing alternating scan timestamps about 64 seconds apart. Clean runs
+used isolated domains and no surviving replay processes.

experiments/imu_yaw_prediction/__init__.py

@@ -0,0 +1 @@
+"""Koide local yaw-prediction calibration experiments."""

experiments/imu_yaw_prediction/imu_rotation_prediction.hpp

@@ -0,0 +1,197 @@
+#ifndef LIDAR_LOCALIZATION_EXPERIMENT_IMU_ROTATION_PREDICTION_HPP_
+#define LIDAR_LOCALIZATION_EXPERIMENT_IMU_ROTATION_PREDICTION_HPP_
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <algorithm>
+#include <cmath>
+#include <cstddef>
+#include <deque>
+#include <iterator>
+#include <vector>
+
+namespace lidar_localization
+{
+
+enum class ImuRotationPredictionStatus
+{
+  kReady = 0,
+  kDisabled,
+  kInvalidInterval,
+  kDurationTooLarge,
+  kInsufficientCoverage,
+  kSampleGapTooLarge,
+  kNonFiniteSample,
+};
+
+inline const char * imuRotationPredictionStatusName(ImuRotationPredictionStatus status)
+{
+  switch (status) {
+    case ImuRotationPredictionStatus::kReady: return "imu_rotation_prediction_ready";
+    case ImuRotationPredictionStatus::kDisabled: return "imu_rotation_prediction_disabled";
+    case ImuRotationPredictionStatus::kInvalidInterval:
+      return "imu_rotation_prediction_invalid_interval";
+    case ImuRotationPredictionStatus::kDurationTooLarge:
+      return "imu_rotation_prediction_duration_too_large";
+    case ImuRotationPredictionStatus::kInsufficientCoverage:
+      return "imu_rotation_prediction_insufficient_coverage";
+    case ImuRotationPredictionStatus::kSampleGapTooLarge:
+      return "imu_rotation_prediction_sample_gap_too_large";
+    case ImuRotationPredictionStatus::kNonFiniteSample:
+      return "imu_rotation_prediction_non_finite_sample";
+  }
+  return "imu_rotation_prediction_unknown";
+}
+
+struct ImuRotationSample
+{
+  double stamp_sec{0.0};
+  Eigen::Vector3d angular_velocity_base_rad_s{Eigen::Vector3d::Zero()};
+};
+
+struct ImuRotationPredictionResult
+{
+  ImuRotationPredictionStatus status{ImuRotationPredictionStatus::kInsufficientCoverage};
+  Eigen::Matrix3f relative_rotation{Eigen::Matrix3f::Identity()};
+  double duration_sec{0.0};
+  double max_sample_gap_sec{0.0};
+  std::size_t integrated_sample_count{0};
+
+  bool ready() const {return status == ImuRotationPredictionStatus::kReady;}
+};
+
+class ImuRotationPredictionBuffer
+{
+public:
+  explicit ImuRotationPredictionBuffer(double history_duration_sec = 2.0)
+  : history_duration_sec_(history_duration_sec) {}
+
+  bool addSample(double stamp_sec, const Eigen::Vector3d & angular_velocity_base_rad_s)
+  {
+    if (!std::isfinite(stamp_sec) || !angular_velocity_base_rad_s.allFinite()) {
+      return false;
+    }
+    if (!samples_.empty() && stamp_sec <= samples_.back().stamp_sec) {
+      return false;
+    }
+    samples_.push_back({stamp_sec, angular_velocity_base_rad_s});
+    const double oldest_stamp = stamp_sec - history_duration_sec_;
+    while (samples_.size() > 2 && samples_[1].stamp_sec < oldest_stamp) {
+      samples_.pop_front();
+    }
+    return true;
+  }
+
+  void clear() {samples_.clear();}
+  std::size_t size() const {return samples_.size();}
+
+  ImuRotationPredictionResult predict(
+    double start_sec, double end_sec, double max_duration_sec,
+    double max_sample_gap_sec) const
+  {
+    ImuRotationPredictionResult result;
+    result.duration_sec = end_sec - start_sec;
+    if (!std::isfinite(start_sec) || !std::isfinite(end_sec) || end_sec <= start_sec) {
+      result.status = ImuRotationPredictionStatus::kInvalidInterval;
+      return result;
+    }
+    if (max_duration_sec <= 0.0 || result.duration_sec > max_duration_sec) {
+      result.status = ImuRotationPredictionStatus::kDurationTooLarge;
+      return result;
+    }
+    if (
+      samples_.size() < 2 || samples_.front().stamp_sec > start_sec ||
+      samples_.back().stamp_sec < end_sec)
+    {
+      result.status = ImuRotationPredictionStatus::kInsufficientCoverage;
+      return result;
+    }
+
+    std::vector<ImuRotationSample> window;
+    window.reserve(samples_.size() + 2);
+    window.push_back({start_sec, interpolate(start_sec)});
+    for (const auto & sample : samples_) {
+      if (sample.stamp_sec > start_sec && sample.stamp_sec < end_sec) {
+        window.push_back(sample);
+      }
+    }
+    window.push_back({end_sec, interpolate(end_sec)});
+
+    Eigen::Quaterniond rotation = Eigen::Quaterniond::Identity();
+    for (std::size_t index = 1; index < window.size(); ++index) {
+      const double dt = window[index].stamp_sec - window[index - 1].stamp_sec;
+      result.max_sample_gap_sec = std::max(result.max_sample_gap_sec, dt);
+      if (!std::isfinite(dt) || dt <= 0.0 ||
+        max_sample_gap_sec <= 0.0 || dt > max_sample_gap_sec)
+      {
+        result.status = ImuRotationPredictionStatus::kSampleGapTooLarge;
+        return result;
+      }
+      const Eigen::Vector3d mean_rate = 0.5 * (
+        window[index - 1].angular_velocity_base_rad_s +
+        window[index].angular_velocity_base_rad_s);
+      if (!mean_rate.allFinite()) {
+        result.status = ImuRotationPredictionStatus::kNonFiniteSample;
+        return result;
+      }
+      const Eigen::Vector3d rotation_vector = mean_rate * dt;
+      const double angle = rotation_vector.norm();
+      if (angle > 1e-12) {
+        rotation = (rotation * Eigen::Quaterniond(
+          Eigen::AngleAxisd(angle, rotation_vector / angle))).normalized();
+      }
+      ++result.integrated_sample_count;
+    }
+    result.relative_rotation = rotation.toRotationMatrix().cast<float>();
+    if (!result.relative_rotation.allFinite()) {
+      result.status = ImuRotationPredictionStatus::kNonFiniteSample;
+      result.relative_rotation = Eigen::Matrix3f::Identity();
+      return result;
+    }
+    result.status = ImuRotationPredictionStatus::kReady;
+    return result;
+  }
+
+private:
+  Eigen::Vector3d interpolate(double stamp_sec) const
+  {
+    auto right = std::lower_bound(
+      samples_.begin(), samples_.end(), stamp_sec,
+      [](const ImuRotationSample & sample, double stamp) {
+        return sample.stamp_sec < stamp;
+      });
+    if (right == samples_.begin()) {
+      return right->angular_velocity_base_rad_s;
+    }
+    if (right == samples_.end()) {
+      return samples_.back().angular_velocity_base_rad_s;
+    }
+    if (right->stamp_sec == stamp_sec) {
+      return right->angular_velocity_base_rad_s;
+    }
+    const auto left = std::prev(right);
+    const double fraction =
+      (stamp_sec - left->stamp_sec) / (right->stamp_sec - left->stamp_sec);
+    return left->angular_velocity_base_rad_s + fraction * (
+      right->angular_velocity_base_rad_s - left->angular_velocity_base_rad_s);
+  }
+
+  double history_duration_sec_{2.0};
+  std::deque<ImuRotationSample> samples_;
+};
+
+inline Eigen::Matrix4f applyImuRotationPrediction(
+  const Eigen::Matrix4f & accepted_pose,
+  const Eigen::Matrix4f & translation_seed,
+  const Eigen::Matrix3f & relative_rotation)
+{
+  Eigen::Matrix4f result = translation_seed;
+  result.block<3, 3>(0, 0) =
+    accepted_pose.block<3, 3>(0, 0) * relative_rotation;
+  return result;
+}
+
+}  // namespace lidar_localization
+
+#endif  // LIDAR_LOCALIZATION_EXPERIMENT_IMU_ROTATION_PREDICTION_HPP_

experiments/imu_yaw_prediction/results.json

@@ -0,0 +1,52 @@
+{
+  "schema_version": 1,
+  "date": "2026-07-14",
+  "dataset_sequence_count": 11,
+  "offline_analysis": {
+    "full_results": "/media/sasaki/aiueo/datasets/koide_hard_localization/generated/imu_yaw_validation_20260714/summary.json",
+    "indoor_rate_rmse_rad_s": {
+      "identity_median": 1.0536,
+      "identity_worst": 2.49,
+      "signed_axis_median": 0.0775,
+      "signed_axis_worst": 0.356,
+      "wahba_median": 0.0361,
+      "wahba_worst": 0.35
+    },
+    "outdoor_rate_rmse_rad_s": {
+      "identity_median": 0.0651,
+      "identity_worst": 0.0813,
+      "wahba_median": 0.0628,
+      "wahba_worst": 0.0799
+    },
+    "maximum_absolute_time_offset_sec": {
+      "indoor": 0.005,
+      "outdoor_hard": 0.005,
+      "outdoor_kidnap": 0.045
+    }
+  },
+  "runtime_ab": [
+    {
+      "sequence": "indoor_easy_01",
+      "duration_sec": 8.0,
+      "lidar_only": {"translation_rmse_m": 0.05284, "rotation_rmse_deg": 1.67677},
+      "imu_rotation": {"translation_rmse_m": 1.31443, "rotation_rmse_deg": 2.41686},
+      "decision": "reject"
+    },
+    {
+      "sequence": "outdoor_hard_01a",
+      "duration_sec": 8.0,
+      "lidar_only": {"translation_rmse_m": 0.17624, "rotation_rmse_deg": 0.60179},
+      "imu_rotation": {"translation_rmse_m": 0.17416, "rotation_rmse_deg": 0.53217},
+      "decision": "insufficient_for_promotion"
+    },
+    {
+      "sequence": "outdoor_kidnap_a",
+      "duration_sec": 30.0,
+      "lidar_only": {"translation_rmse_m": 0.22, "rotation_rmse_deg": 0.60276, "rotation_error_last_deg": 3.87328},
+      "imu_rotation": {"translation_rmse_m": 0.20934, "rotation_rmse_deg": 0.58145, "rotation_error_last_deg": 3.98249},
+      "decision": "reject_last_rotation_regression"
+    }
+  ],
+  "promotion_decision": "rejected",
+  "reason": "The candidate regressed indoor translation and rotation, and slightly regressed final rotation on the outdoor kidnap run. It remains experiment-only and is not connected to runtime parameters."
+}