diff --git a/include/basalt/io/dataset_io_uzh.h b/include/basalt/io/dataset_io_uzh.h
index 80bd48f..658e1d6 100644
--- a/include/basalt/io/dataset_io_uzh.h
+++ b/include/basalt/io/dataset_io_uzh.h
@@ -175,19 +175,17 @@ class UzhIO : public DatasetIoInterface {
               << " left images and " << data->right_image_path.size()
               << std::endl;
 
-    {
-      int64_t t_ns = data->get_image_timestamps()[0];
-      std::cout << t_ns << " " << data->left_image_path.at(t_ns) << " "
-                << data->right_image_path.at(t_ns) << std::endl;
-    }
+    //    {
+    //      int64_t t_ns = data->get_image_timestamps()[0];
+    //      std::cout << t_ns << " " << data->left_image_path.at(t_ns) << " "
+    //                << data->right_image_path.at(t_ns) << std::endl;
+    //    }
 
     read_imu_data(path + "/imu.txt");
 
     std::cout << "Loaded " << data->get_gyro_data().size() << " imu msgs."
               << std::endl;
 
-    {}
-
     if (file_exists(path + "/groundtruth.txt")) {
       read_gt_data_pose(path + "/groundtruth.txt");
     }
@@ -307,7 +305,10 @@ class UzhIO : public DatasetIoInterface {
       ss >> tmp >> timestamp >> pos[0] >> pos[1] >> pos[2] >> q.x() >> q.y() >>
           q.z() >> q.w();
 
-      data->gt_timestamps.emplace_back(timestamp * 1e9);
+      int64_t t_ns = timestamp * 1e9;
+      t_ns += -99902802;
+
+      data->gt_timestamps.emplace_back(t_ns);
       data->gt_pose_data.emplace_back(q, pos);
     }
   }
diff --git a/src/vi_estimator/keypoint_vio.cpp b/src/vi_estimator/keypoint_vio.cpp
index 9daadc3..95b5a70 100644
--- a/src/vi_estimator/keypoint_vio.cpp
+++ b/src/vi_estimator/keypoint_vio.cpp
@@ -125,6 +125,13 @@ void KeypointVioEstimator::initialize(const Eigen::Vector3d& bg,
     while (true) {
       vision_data_queue.pop(curr_frame);
 
+      if (!curr_frame.get()) {
+        break;
+      }
+
+      // Correct camera time offset
+      // curr_frame->t_ns += calib.cam_time_offset_ns;
+
       if (!initialized) {
         Eigen::Vector3d vel_w_i_init;
         vel_w_i_init.setZero();
@@ -150,10 +157,6 @@ void KeypointVioEstimator::initialize(const Eigen::Vector3d& bg,
         initialized = true;
       }
 
-      if (!curr_frame.get()) {
-        break;
-      }
-
       if (prev_frame) {
         // preintegrate measurements
 
@@ -179,6 +182,7 @@ void KeypointVioEstimator::initialize(const Eigen::Vector3d& bg,
         }
 
         if (meas->get_start_t_ns() + meas->get_dt_ns() < curr_frame->t_ns) {
+          if (!data.get()) break;
           int64_t tmp = data->t_ns;
           data->t_ns = curr_frame->t_ns;
           meas->integrate(*data, accel_cov, gyro_cov);