diff --git a/include/basalt/device/rs_t265.h b/include/basalt/device/rs_t265.h
index f5ef6d0..a57b6de 100644
--- a/include/basalt/device/rs_t265.h
+++ b/include/basalt/device/rs_t265.h
@@ -47,8 +47,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <pangolin/image/typed_image.h>
 #include <pangolin/pangolin.h>
-#include <opencv2/core.hpp>
-#include <opencv2/highgui.hpp>
 
 #include <tbb/concurrent_queue.h>
 
diff --git a/src/rs_t265_vio.cpp b/src/rs_t265_vio.cpp
index 0249ab8..511c9d4 100644
--- a/src/rs_t265_vio.cpp
+++ b/src/rs_t265_vio.cpp
@@ -178,12 +178,12 @@ int main(int argc, char** argv) {
   if (show_gui) vio->out_vis_queue = &out_vis_queue;
   vio->out_state_queue = &out_state_queue;
 
-  //  basalt::MargDataSaver::Ptr marg_data_saver;
-  //
-  //  if (!marg_data_path.empty()) {
-  //    marg_data_saver.reset(new basalt::MargDataSaver(marg_data_path));
-  //    vio->out_marg_queue = &marg_data_saver->in_marg_queue;
-  //  }
+  basalt::MargDataSaver::Ptr marg_data_saver;
+
+  if (!marg_data_path.empty()) {
+    marg_data_saver.reset(new basalt::MargDataSaver(marg_data_path));
+    vio->out_marg_queue = &marg_data_saver->in_marg_queue;
+  }
 
   vio_data_log.Clear();
 
@@ -282,8 +282,9 @@ int main(int argc, char** argv) {
           std::bind(&draw_image_overlay, std::placeholders::_1, idx);
     }
 
-    Eigen::Vector3d cam_p(-0.5, -3, -5);
+    Eigen::Vector3d cam_p(0.5, -2, -2);
     cam_p = vio->getT_w_i_init().so3() * calib.T_i_c[0].so3() * cam_p;
+    cam_p[2] = 1;
 
     pangolin::OpenGlRenderState camera(
         pangolin::ProjectionMatrix(640, 480, 400, 400, 320, 240, 0.001, 10000),
diff --git a/src/vi_estimator/keypoint_vio.cpp b/src/vi_estimator/keypoint_vio.cpp
index 07e2bca..cd4120e 100644
--- a/src/vi_estimator/keypoint_vio.cpp
+++ b/src/vi_estimator/keypoint_vio.cpp
@@ -94,6 +94,7 @@ void KeypointVioEstimator::initialize(int64_t t_ns, const Sophus::SE3d& T_w_i,
   initialized = true;
   T_w_i_init = T_w_i;
 
+  last_state_t_ns = t_ns;
   imu_meas[t_ns] = IntegratedImuMeasurement(t_ns, bg, ba);
   frame_states[t_ns] =
       PoseVelBiasStateWithLin(t_ns, T_w_i, vel_w_i, bg, ba, true);