From 12451cff17ce3058c7ae1bbf6a0b66fae2116e54 Mon Sep 17 00:00:00 2001
From: Christian Forster <ch.forster@gmail.com>
Date: Thu, 27 Mar 2014 11:17:47 +0100
Subject: [PATCH] trace timestamp with logs for plotting

---
 svo/include/svo/frame_handler_base.h | 8 ++++----
 svo/src/frame_handler_base.cpp       | 9 +++++++--
 svo/src/frame_handler_mono.cpp       | 4 ++--
 3 files changed, 13 insertions(+), 8 deletions(-)

diff --git a/svo/include/svo/frame_handler_base.h b/svo/include/svo/frame_handler_base.h
index 51286c7..d8d8109 100644
--- a/svo/include/svo/frame_handler_base.h
+++ b/svo/include/svo/frame_handler_base.h
@@ -90,13 +90,13 @@ protected:
   DepthFilter* depth_filter_;                   //!< Depth estimation algorithm runs in a parallel thread and is used to initialize new 3D points.
 
   /// Before a frame is processed, this function is called.
-  bool startFrameProcessingCommon();
+  bool startFrameProcessingCommon(const double timestamp);
 
   /// When a frame is finished processing, this function is called.
   int finishFrameProcessingCommon(
-      size_t update_id,
-      UpdateResult dropout,
-      size_t num_observations);
+      const size_t update_id,
+      const UpdateResult dropout,
+      const size_t num_observations);
 
   /// Reset the map and frame handler to start from scratch.
   void resetCommon();
diff --git a/svo/src/frame_handler_base.cpp b/svo/src/frame_handler_base.cpp
index 991268c..04227ec 100644
--- a/svo/src/frame_handler_base.cpp
+++ b/svo/src/frame_handler_base.cpp
@@ -56,6 +56,7 @@ FrameHandlerBase() :
   g_permon->addTimer("point_optimizer");
   g_permon->addTimer("local_ba");
   g_permon->addTimer("tot_time");
+  g_permon->addLog("timestamp");
   g_permon->addLog("img_align_n_tracked");
   g_permon->addLog("repr_n_mps");
   g_permon->addLog("repr_n_new_references");
@@ -90,7 +91,7 @@ FrameHandlerBase::
 }
 
 bool FrameHandlerBase::
-startFrameProcessingCommon()
+startFrameProcessingCommon(const double timestamp)
 {
   if(set_start_)
   {
@@ -101,6 +102,7 @@ startFrameProcessingCommon()
   if(stage_ == PAUSED)
     return false;
 
+  SVO_LOG(timestamp);
   SVO_DEBUG_STREAM("New Frame");
   SVO_START_TIMER("tot_time");
   timer_.start();
@@ -111,7 +113,10 @@ startFrameProcessingCommon()
 }
 
 int FrameHandlerBase::
-finishFrameProcessingCommon(size_t update_id, UpdateResult dropout, size_t num_observations)
+finishFrameProcessingCommon(
+    const size_t update_id,
+    const UpdateResult dropout,
+    const size_t num_observations)
 {
   SVO_DEBUG_STREAM("Frame: "<<update_id<<"\t fps-avg = "<< 1.0/acc_frame_timings_.getMean()<<"\t nObs = "<<acc_num_obs_.getMean());
   SVO_LOG(dropout);
diff --git a/svo/src/frame_handler_mono.cpp b/svo/src/frame_handler_mono.cpp
index 1c4c814..105d546 100644
--- a/svo/src/frame_handler_mono.cpp
+++ b/svo/src/frame_handler_mono.cpp
@@ -43,9 +43,9 @@ FrameHandlerMono::
 {}
 
 void FrameHandlerMono::
-addImage(const cv::Mat& img, double timestamp)
+addImage(const cv::Mat& img, const double timestamp)
 {
-  if(!startFrameProcessingCommon())
+  if(!startFrameProcessingCommon(timestamp))
     return;
 
   // some cleanup from last iteration, can't do before because of visualization