From c2f052f9f86269356f3ad008195cc4807c99722a Mon Sep 17 00:00:00 2001
From: Mateo de Mayo <mateo.demayo@collabora.com>
Date: Fri, 6 May 2022 15:30:37 -0300
Subject: [PATCH] Register internal stages timestamps

---
 .../optical_flow/frame_to_frame_optical_flow.h  |  2 +-
 include/basalt/optical_flow/optical_flow.h      | 17 +++++++++++++++++
 include/basalt/vi_estimator/sqrt_keypoint_vio.h |  3 ++-
 src/vi_estimator/sqrt_keypoint_vio.cpp          | 11 ++++++++++-
 thirdparty/basalt-headers                       |  2 +-
 5 files changed, 31 insertions(+), 4 deletions(-)

diff --git a/include/basalt/optical_flow/frame_to_frame_optical_flow.h b/include/basalt/optical_flow/frame_to_frame_optical_flow.h
index 342d430..d811a5f 100644
--- a/include/basalt/optical_flow/frame_to_frame_optical_flow.h
+++ b/include/basalt/optical_flow/frame_to_frame_optical_flow.h
@@ -103,7 +103,7 @@ class FrameToFrameOpticalFlow : public OpticalFlowBase {
         if (output_queue) output_queue->push(nullptr);
         break;
       }
-      input_ptr->addTime("opticalflow_received");
+      input_ptr->addTime("frames_received");
 
       processFrame(input_ptr->t_ns, input_ptr);
     }
diff --git a/include/basalt/optical_flow/optical_flow.h b/include/basalt/optical_flow/optical_flow.h
index 279a5cb..6a8b125 100644
--- a/include/basalt/optical_flow/optical_flow.h
+++ b/include/basalt/optical_flow/optical_flow.h
@@ -34,6 +34,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 #pragma once
 
+#include <chrono>
 #include <memory>
 
 #include <Eigen/Geometry>
@@ -56,6 +57,22 @@ struct OpticalFlowInput {
 
   int64_t t_ns;
   std::vector<ImageData> img_data;
+
+  // Keep track of internal timestamps for this input
+  bool timing_enabled = false;
+  std::vector<int64_t> tss{};
+  void addTime(const char* /* name */, int64_t custom_ts = INT64_MIN) {
+    if (!timing_enabled) {
+      return;
+    }
+
+    if (custom_ts != INT64_MIN) {
+      tss.push_back(custom_ts);
+    } else {
+      auto ts = std::chrono::steady_clock::now().time_since_epoch().count();
+      tss.push_back(ts);
+    }
+  }
 };
 
 struct OpticalFlowResult {
diff --git a/include/basalt/vi_estimator/sqrt_keypoint_vio.h b/include/basalt/vi_estimator/sqrt_keypoint_vio.h
index 69494db..7e32051 100644
--- a/include/basalt/vi_estimator/sqrt_keypoint_vio.h
+++ b/include/basalt/vi_estimator/sqrt_keypoint_vio.h
@@ -120,7 +120,8 @@ class SqrtKeypointVioEstimator : public VioEstimatorBase,
   // int64_t propagate();
   // void addNewState(int64_t data_t_ns);
 
-  void optimize_and_marg(const std::map<int64_t, int>& num_points_connected,
+  void optimize_and_marg(const OpticalFlowInput::Ptr& input_images,
+                         const std::map<int64_t, int>& num_points_connected,
                          const std::unordered_set<KeypointId>& lost_landmaks);
 
   void marginalize(const std::map<int64_t, int>& num_points_connected,
diff --git a/src/vi_estimator/sqrt_keypoint_vio.cpp b/src/vi_estimator/sqrt_keypoint_vio.cpp
index e7500dd..ed7be7f 100644
--- a/src/vi_estimator/sqrt_keypoint_vio.cpp
+++ b/src/vi_estimator/sqrt_keypoint_vio.cpp
@@ -175,6 +175,7 @@ void SqrtKeypointVioEstimator<Scalar_>::initialize(const Eigen::Vector3d& bg_,
       if (!curr_frame.get()) {
         break;
       }
+      curr_frame->input_images->addTime("vio_start");
 
       // Correct camera time offset
       // curr_frame->t_ns += calib.cam_time_offset_ns;
@@ -252,6 +253,7 @@ void SqrtKeypointVioEstimator<Scalar_>::initialize(const Eigen::Vector3d& bg_,
           data->t_ns = tmp;
         }
       }
+      curr_frame->input_images->addTime("imu_preintegrated");
 
       measure(curr_frame, meas);
       prev_frame = curr_frame;
@@ -477,8 +479,10 @@ bool SqrtKeypointVioEstimator<Scalar_>::measure(
       }
     }
   }
+  opt_flow_meas->input_images->addTime("landmarks_updated");
 
-  optimize_and_marg(num_points_connected, lost_landmaks);
+  optimize_and_marg(opt_flow_meas->input_images, num_points_connected,
+                    lost_landmaks);
 
   if (out_state_queue) {
     PoseVelBiasStateWithLin p = frame_states.at(last_state_t_ns);
@@ -486,6 +490,8 @@ bool SqrtKeypointVioEstimator<Scalar_>::measure(
     typename PoseVelBiasState<double>::Ptr data(
         new PoseVelBiasState<double>(p.getState().template cast<double>()));
 
+    data->input_images = opt_flow_meas->input_images;
+    data->input_images->addTime("pose_produced");
     out_state_queue->push(data);
   }
 
@@ -1425,10 +1431,13 @@ void SqrtKeypointVioEstimator<Scalar_>::optimize() {
 
 template <class Scalar_>
 void SqrtKeypointVioEstimator<Scalar_>::optimize_and_marg(
+    const OpticalFlowInput::Ptr& input_images,
     const std::map<int64_t, int>& num_points_connected,
     const std::unordered_set<KeypointId>& lost_landmaks) {
   optimize();
+  input_images->addTime("optimized");
   marginalize(num_points_connected, lost_landmaks);
+  input_images->addTime("marginalized");
 }
 
 template <class Scalar_>
diff --git a/thirdparty/basalt-headers b/thirdparty/basalt-headers
index 8c39d10..1473e8a 160000
--- a/thirdparty/basalt-headers
+++ b/thirdparty/basalt-headers
@@ -1 +1 @@
-Subproject commit 8c39d10b3d5463bf257701a32293caee088b8836
+Subproject commit 1473e8a417313082372bd0356c732cfcd60e183e