From 302d64aa5e7e2f30112332479e0772b100c8b4c5 Mon Sep 17 00:00:00 2001
From: "k.koide" <k.koide@aist.go.jp>
Date: Mon, 25 Nov 2024 17:43:11 +0900
Subject: [PATCH] fix imu_cursor

---
 src/glim/odometry/loose_initial_state_estimation.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/glim/odometry/loose_initial_state_estimation.cpp b/src/glim/odometry/loose_initial_state_estimation.cpp
index 32ae2e27..f266badc 100644
--- a/src/glim/odometry/loose_initial_state_estimation.cpp
+++ b/src/glim/odometry/loose_initial_state_estimation.cpp
@@ -130,7 +130,7 @@ EstimationFrame::ConstPtr LooseInitialStateEstimation::initial_pose() {
       imu_cursor++;
     }
 
-    const Eigen::Vector3d acc_local = imu_data[i].middleRows<3>(1);
+    const Eigen::Vector3d acc_local = imu_data[imu_cursor].middleRows<3>(1);
     sum_acc_odom += (T_odom_lidar[i].second * T_lidar_imu).linear() * acc_local.normalized();
   }