From 533178e3a5e8f79f17d1b25ff2805940f13a7b12 Mon Sep 17 00:00:00 2001
From: Samuel Grayson <sam@samgrayson.me>
Date: Tue, 2 Mar 2021 13:53:52 -0600
Subject: [PATCH] Experiments

---
 ov_msckf/src/slam2.cpp | 9 ++++++++-
 1 file changed, 8 insertions(+), 1 deletion(-)

diff --git a/ov_msckf/src/slam2.cpp b/ov_msckf/src/slam2.cpp
index 233d516f7..277b55ec5 100644
--- a/ov_msckf/src/slam2.cpp
+++ b/ov_msckf/src/slam2.cpp
@@ -193,6 +193,9 @@ class slam2 : public plugin {
 
 	void feed_imu_cam(switchboard::ptr<const imu_cam_type> datum, std::size_t iteration_no) {
 		// Ensures that slam doesnt start before valid IMU readings come in
+		double timestamp_in_seconds;
+		{
+			CPU_TIMER_TIME_BLOCK("IMU");
 		if (datum == nullptr) {
 			std::cerr << "slam2:feed_imu_cam datum == nullptr\n";
 			abort();
@@ -200,7 +203,7 @@ class slam2 : public plugin {
 
 		// This ensures that every data point is coming in chronological order If youre failing this assert, 
 		// make sure that your data folder matches the name in offline_imu_cam/plugin.cc
-		double timestamp_in_seconds = (double(datum->dataset_time) / NANO_SEC);
+		timestamp_in_seconds = (double(datum->dataset_time) / NANO_SEC);
 		assert(timestamp_in_seconds > previous_timestamp);
 		previous_timestamp = timestamp_in_seconds;
 
@@ -219,7 +222,10 @@ class slam2 : public plugin {
 			imu_cam_buffer = datum;
 			return;
 		}
+		}
 
+		{
+			CPU_TIMER_TIME_BLOCK("cam");
 		cv::Mat img0;
 		cv::Mat img1;
 
@@ -299,6 +305,7 @@ class slam2 : public plugin {
 		// const_cast<imu_cam_type*>(imu_cam_buffer)->img0.reset();
 		// const_cast<imu_cam_type*>(imu_cam_buffer)->img1.reset();
 		imu_cam_buffer = datum;
+		}
 	}
 
 	virtual ~slam2() override {}