From f6a33469f55ca281ad603d86af3e821f6a2556b9 Mon Sep 17 00:00:00 2001
From: Vladyslav Usenko <vlad.usenko@tum.de>
Date: Fri, 9 Aug 2019 19:34:11 +0200
Subject: [PATCH] Added skipping IMU measurements before the first image.

---
 src/vi_estimator/keypoint_vio.cpp | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/src/vi_estimator/keypoint_vio.cpp b/src/vi_estimator/keypoint_vio.cpp
index f413019..e99fbf9 100644
--- a/src/vi_estimator/keypoint_vio.cpp
+++ b/src/vi_estimator/keypoint_vio.cpp
@@ -138,6 +138,14 @@ void KeypointVioEstimator::initialize(const Eigen::Vector3d& bg,
       // curr_frame->t_ns += calib.cam_time_offset_ns;
 
       if (!initialized) {
+        while (data->t_ns < curr_frame->t_ns) {
+          imu_data_queue.pop(data);
+          if (!data.get()) break;
+          data->accel = calib.calib_accel_bias.getCalibrated(data->accel);
+          data->gyro = calib.calib_gyro_bias.getCalibrated(data->gyro);
+          std::cout << "Skipping IMU data.." << std::endl;
+        }
+
         Eigen::Vector3d vel_w_i_init;
         vel_w_i_init.setZero();