From 8495fecc0122fbf98da8f0239ef4ea90aacdc4b1 Mon Sep 17 00:00:00 2001
From: Jacob Lambert <jacob.lambert.2@tier4.jp>
Date: Mon, 8 Jun 2026 22:54:50 +0900
Subject: [PATCH] expose and set IMU gravity param

---
 config/config_sensors.json              | 4 +++-
 include/glim/common/imu_integration.hpp | 3 ++-
 src/glim/common/imu_integration.cpp     | 5 +++--
 3 files changed, 8 insertions(+), 4 deletions(-)

diff --git a/config/config_sensors.json b/config/config_sensors.json
index 61212f20..67b2fae4 100644
--- a/config/config_sensors.json
+++ b/config/config_sensors.json
@@ -1,6 +1,7 @@
 {
   /*** Sensor configurations ***
   // --- IMU config ---
+  // imu_gravity_magnitude       : Gravity magnitude used for IMU preintegration
   // imu_acc_noise               : Accelerometer noise
   // imu_gyro_noise              : Gyroscope noise
   // imu_int_noise               : Integration noise
@@ -44,6 +45,7 @@
   */
   "sensors": {
     // IMU config
+    "imu_gravity_magnitude": 9.80665,
     "imu_acc_noise": 0.05,
     "imu_gyro_noise": 0.02,
     "imu_int_noise": 0.001,
@@ -96,4 +98,4 @@
       -0.03279627881588615
     ]
   }
-}
\ No newline at end of file
+}
diff --git a/include/glim/common/imu_integration.hpp b/include/glim/common/imu_integration.hpp
index e6a2a421..9abe5cc3 100644
--- a/include/glim/common/imu_integration.hpp
+++ b/include/glim/common/imu_integration.hpp
@@ -15,6 +15,7 @@ struct IMUIntegrationParams {
   ~IMUIntegrationParams();
 
   bool upright;       // If true, +Z = up
+  double gravity_magnitude;  // Gravity magnitude used by GTSAM IMU preintegration
   double acc_noise;   // Linear acceleration noise
   double gyro_noise;  // Angular velocity noise
   double int_noise;   // Integration noise
@@ -94,4 +95,4 @@ class IMUIntegration {
   std::shared_ptr<gtsam::PreintegratedImuMeasurements> imu_measurements;
   std::deque<Eigen::Matrix<double, 7, 1>> imu_queue;
 };
-}  // namespace glim
\ No newline at end of file
+}  // namespace glim
diff --git a/src/glim/common/imu_integration.cpp b/src/glim/common/imu_integration.cpp
index ab02e79a..1af15bba 100644
--- a/src/glim/common/imu_integration.cpp
+++ b/src/glim/common/imu_integration.cpp
@@ -8,6 +8,7 @@ IMUIntegrationParams::IMUIntegrationParams(const bool upright) {
   glim::Config config_sensors(glim::GlobalConfig::get_config_path("config_sensors"));
 
   this->upright = upright;
+  this->gravity_magnitude = config_sensors.param<double>("sensors", "imu_gravity_magnitude", 9.80665);
   this->acc_noise = config_sensors.param<double>("sensors", "imu_acc_noise", 0.01);
   this->gyro_noise = config_sensors.param<double>("sensors", "imu_gyro_noise", 0.001);
   this->int_noise = config_sensors.param<double>("sensors", "imu_int_noise", 0.001);
@@ -16,9 +17,9 @@ IMUIntegrationParams::IMUIntegrationParams(const bool upright) {
 IMUIntegrationParams::~IMUIntegrationParams() {}
 
 IMUIntegration::IMUIntegration(const IMUIntegrationParams& params) {
-  auto imu_params = gtsam::PreintegrationParams::MakeSharedU();
+  auto imu_params = gtsam::PreintegrationParams::MakeSharedU(params.gravity_magnitude);
   if (!params.upright) {
-    imu_params = gtsam::PreintegrationParams::MakeSharedD();
+    imu_params = gtsam::PreintegrationParams::MakeSharedD(params.gravity_magnitude);
   }
 
   imu_params->accelerometerCovariance = gtsam::Matrix3::Identity() * std::pow(params.acc_noise, 2);