From e245b7751edf3f1c3b74578527c707b5aa3e630c Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Sun, 9 Nov 2025 20:44:28 -0800
Subject: [PATCH] Fixed first imu of a batch used 2x internally

---
 rtabmap_odom/src/OdometryROS.cpp | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp
index 4176fee7..bc34b042 100644
--- a/rtabmap_odom/src/OdometryROS.cpp
+++ b/rtabmap_odom/src/OdometryROS.cpp
@@ -571,9 +571,8 @@ void OdometryROS::processData()
 		for(std::map<double, sensor_msgs::msg::Imu::ConstSharedPtr>::iterator iter=iterFirst; iter!=iterEnd;)
 		{
 			// Because we always keep the last processed imu in the buffer, skip the first 
-			// one when processing again the buffer unless its time is lower/equal to image 
-			// current stamp (could happen on initialization).
-			if(iter!=iterFirst || iter->first <= rtabmap_conversions::timestampFromROS(header.stamp)) {
+			// one when processing again the buffer
+			if(iter!=iterFirst) {
 				imus.push_back(*iter);
 			}
 			if(iter!=iterLast) {