Merge pull request MIT-SPARK#70 from MIT-SPARK/fix/stats_log_for_timing

Fix/stats log for timing

README.md

@@ -172,8 +172,53 @@ We also provide a `.clang-format` file with the style rules that the repo uses,
 Also, check [tips for development](./docs/tips_development.md) and our **[developer guide](./docs/developer_guide.md)**.
 
 # 5. FAQ
+
+### Issues
   If you have problems building or running the pipeline and/or issues with dependencies, you might find useful information in our [FAQ](./docs/faq.md) or in the issue tracker.
 
+### How to interpret console output
+
+```
+I0512 21:05:55.136549 21233 Pipeline.cpp:449] Statistics
+-----------                                  #	Log Hz	{avg     +- std    }	[min,max]
+Data Provider [ms]                      	    0	
+Display [ms]                            	  146	36.5421	{8.28082 +- 2.40370}	[3,213]
+VioBackEnd [ms]                         	   73	19.4868	{15.2192 +- 9.75712}	[0,39]
+VioFrontEnd Frame Rate [ms]             	  222	59.3276	{5.77027 +- 1.51571}	[3,12]
+VioFrontEnd Keyframe Rate [ms]          	   73	19.6235	{31.4110 +- 7.29504}	[24,62]
+VioFrontEnd [ms]                        	  295	77.9727	{12.1593 +- 10.7279}	[3,62]
+Visualizer [ms]                         	   73	19.4639	{3.82192 +- 0.805234}	[2,7]
+backend_input_queue Size [#]            	   73	18.3878	{1.00000 +- 0.00000}	[1,1]
+data_provider_left_frame_queue Size (#) 	  663	165.202	{182.265 +- 14.5110}	[1,359]
+data_provider_right_frame_queue Size (#)	  663	165.084	{182.029 +- 14.5150}	[1,359]
+display_input_queue Size [#]            	  146	36.5428	{1.68493 +- 0.00000}	[1,12]
+stereo_frontend_input_queue Size [#]    	  301	75.3519	{4.84718 +- 0.219043}	[1,5]
+visualizer_backend_queue Size [#]       	   73	18.3208	{1.00000 +- 0.00000}	[1,1]
+visualizer_frontend_queue Size [#]      	  295	73.9984	{4.21695 +- 1.24381}	[1,7]
+```
+
+- `#` number of samples taken.
+- `Log Hz` average number of samples taken per second in Hz.
+- `avg` average of the actual value logged. Same unit as the logged quantity.
+- `std` standard deviation of the value logged.
+- `[min,max]` minimum and maximum values that the logged value took.
+
+There are two main things logged: the time it takes for the pipeline modules to run (i.e. `VioBackEnd`, `Visualizer` etc), and the size of the queues between pipeline modules (i.e. `backend_input_queue`).
+
+For example:
+```
+VioBackEnd [ms]                         	   73	19.4868	{15.2192 +- 9.75712}	[0,39]
+```
+
+Shows that the backend runtime got sampled `73` times, at a rate of `19.48`Hz (which accounts for both the time the backend waits for input to consume and the time it takes to process it). That it takes `15.21`ms to consume its input with a standard deviation of `9.75`ms and that the least it took to run for one input was `0`ms and the most it took so far is `39`ms.
+
+For the queues, for example:
+```
+stereo_frontend_input_queue Size [#]    	  301	75.3519	{4.84718 +- 0.219043}	[1,5]
+```
+
+Shows that the frontend input queue got sampled `301` times, at a rate of `75.38`Hz. That it stores an average of `4.84` elements, with a standard deviation of `0.21` elements, and that the min size it had was `1` element, and the max size it stored was of `5` elements.
+
 # 6. Chart
 
 ![vio_chart](./docs/media/kimeravio_chart.png)

include/kimera-vio/common/vio_types.h

@@ -57,6 +57,8 @@ using StatusKeypointCV = std::pair<KeypointStatus, KeypointCV>;
 using StatusKeypointsCV = std::vector<StatusKeypointCV>;
 using BearingVectors = std::vector<Vector3, Eigen::aligned_allocator<Vector3>>;
 
+
+// TODO(Toni): move make unique and  to underlying to another file...
 // Add compatibility for c++11's lack of make_unique.
 template <typename T, typename... Args>
 std::unique_ptr<T> make_unique(Args&&... args) {

include/kimera-vio/utils/ThreadsafeQueue.h

@@ -25,7 +25,9 @@
 
 #include <glog/logging.h>
 
+#include "kimera-vio/common/vio_types.h"
 #include "kimera-vio/utils/Macros.h"
+#include "kimera-vio/utils/Statistics.h"
 
 namespace VIO {
 
@@ -149,7 +151,8 @@ class ThreadsafeQueue : public ThreadsafeQueueBase<T> {
   using TQB = ThreadsafeQueueBase<T>;
   KIMERA_POINTER_TYPEDEFS(ThreadsafeQueue);
   KIMERA_DELETE_COPY_CONSTRUCTORS(ThreadsafeQueue);
-  explicit ThreadsafeQueue(const std::string& queue_id);
+  explicit ThreadsafeQueue(const std::string& queue_id,
+                           const bool& log_queue_size = true);
   virtual ~ThreadsafeQueue() = default;
 
   /** \brief Push by value. Returns false if the queue has been shutdown.
@@ -224,6 +227,9 @@ class ThreadsafeQueue : public ThreadsafeQueueBase<T> {
   using TQB::data_queue_;
   using TQB::mutex_;
   using TQB::shutdown_;
+
+  //! Stats on how full the queue gets.
+  std::unique_ptr<utils::StatsCollector> queue_size_stats_;
 };
 
 /**
@@ -258,20 +264,27 @@ ThreadsafeQueueBase<T>::ThreadsafeQueueBase(const std::string& queue_id)
       shutdown_(false) {}
 
 template <typename T>
-ThreadsafeQueue<T>::ThreadsafeQueue(const std::string& queue_id)
-    : ThreadsafeQueueBase<T>(queue_id) {}
+ThreadsafeQueue<T>::ThreadsafeQueue(const std::string& queue_id,
+                                    const bool& log_queue_size)
+    : ThreadsafeQueueBase<T>(queue_id),
+      queue_size_stats_(
+          log_queue_size
+              ? VIO::make_unique<utils::StatsCollector>(queue_id + " Size [#]")
+              : nullptr) {}
 
 template <typename T>
 bool ThreadsafeQueue<T>::push(T new_value) {
   if (shutdown_) return false;  // atomic, no lock needed.
   std::shared_ptr<T> data(std::make_shared<T>(std::move(new_value)));
   std::unique_lock<std::mutex> lk(mutex_);
-  size_t queue_size = data_queue_.size();
-  VLOG_IF(1, queue_size != 0) << "Queue with id: " << queue_id_
-                              << " is getting full, size: " << queue_size;
   data_queue_.push(data);
+  size_t queue_size = data_queue_.size();
   lk.unlock();  // Unlock before notify.
   data_cond_.notify_one();
+  // Thread-safe so doesn't need external mutex.
+  if (queue_size_stats_) queue_size_stats_->AddSample(queue_size);
+  VLOG_IF(1, queue_size > 1u) << "Queue with id: " << queue_id_
+                              << " is getting full, size: " << queue_size;
   return true;
 }
 
@@ -281,17 +294,19 @@ bool ThreadsafeQueue<T>::pushBlockingIfFull(T new_value,
   if (shutdown_) return false;  // atomic, no lock needed.
   std::shared_ptr<T> data(std::make_shared<T>(std::move(new_value)));
   std::unique_lock<std::mutex> lk(mutex_);
-  size_t queue_size = data_queue_.size();
-  VLOG_IF(1, queue_size != 0) << "Queue with id: " << queue_id_
-                              << " is getting full, size: " << queue_size;
   // Wait until the queue has space or shutdown requested.
   data_cond_.wait(lk, [this, max_queue_size] {
     return data_queue_.size() < max_queue_size || shutdown_;
   });
   if (shutdown_) return false;
   data_queue_.push(data);
+  size_t queue_size = data_queue_.size();
   lk.unlock();  // Unlock before notify.
   data_cond_.notify_one();
+  // Thread-safe so doesn't need external mutex.
+  if (queue_size_stats_) queue_size_stats_->AddSample(queue_size);
+  VLOG_IF(1, queue_size > 1u) << "Queue with id: " << queue_id_
+                              << " is getting full, size: " << queue_size;
   return true;
 }
 

src/frontend/StereoVisionFrontEnd.cpp

@@ -78,6 +78,13 @@ StereoVisionFrontEnd::StereoVisionFrontEnd(
 /* -------------------------------------------------------------------------- */
 FrontendOutput::UniquePtr StereoVisionFrontEnd::spinOnce(
     const StereoFrontEndInputPayload& input) {
+  // For timing
+  utils::StatsCollector timing_stats_frame_rate("VioFrontEnd Frame Rate [ms]");
+  utils::StatsCollector timing_stats_keyframe_rate(
+      "VioFrontEnd Keyframe Rate [ms]");
+  auto start_time = utils::Timer::tic();
+
+  // Get stereo info
   const StereoFrame& stereoFrame_k = input.getStereoFrame();
   const auto& k = stereoFrame_k.getFrameId();
   VLOG(1) << "------------------- Processing frame k = " << k
@@ -102,8 +109,9 @@ FrontendOutput::UniquePtr StereoVisionFrontEnd::spinOnce(
 
   auto full_preint_duration =
       utils::Timer::toc<std::chrono::microseconds>(tic_full_preint).count();
-  utils::StatsCollector stats_full_preint("IMU Preintegration Timing [us]");
-  stats_full_preint.AddSample(full_preint_duration);
+  // Don't add them because they are confusing
+  // utils::StatsCollector stats_full_preint("IMU Preintegration Timing [us]");
+  // stats_full_preint.AddSample(full_preint_duration);
   VLOG_IF(1, full_preint_duration != 0.0)
       << "Current IMU Preintegration frequency: " << 10e6 / full_preint_duration
       << " Hz. (" << full_preint_duration << " us).";
@@ -165,6 +173,9 @@ FrontendOutput::UniquePtr StereoVisionFrontEnd::spinOnce(
     VLOG(10) << "Reset IMU preintegration with latest IMU bias.";
     imu_frontend_->resetIntegrationWithCachedBias();
 
+    // Record keyframe rate timing
+    timing_stats_keyframe_rate.AddSample(utils::Timer::toc(start_time).count());
+
     // Return the output of the frontend for the others.
     VLOG(2) << "Frontend output is a keyframe: pushing to output callbacks.";
     return VIO::make_unique<FrontendOutput>(
@@ -177,6 +188,9 @@ FrontendOutput::UniquePtr StereoVisionFrontEnd::spinOnce(
         feature_tracks,
         getTrackerInfo());
   } else {
+    // Record frame rate timing
+    timing_stats_frame_rate.AddSample(utils::Timer::toc(start_time).count());
+
     // We don't have a keyframe.
     VLOG(2) << "Frontend output is not a keyframe. Skipping output queue push.";
     return VIO::make_unique<FrontendOutput>(false,

src/pipeline/Pipeline.cpp

@@ -118,7 +118,7 @@ Pipeline::Pipeline(const VioParams& params,
       parallel_run_(params.parallel_run_),
       stereo_frontend_input_queue_("stereo_frontend_input_queue"),
       initialization_frontend_output_queue_(
-          "initialization_frontend_output_queue"),
+          "initialization_frontend_output_queue", false),
       backend_input_queue_("backend_input_queue"),
       display_input_queue_("display_input_queue") {
   if (FLAGS_deterministic_random_number_generator) setDeterministicPipeline();

src/utils/Statistics.cpp

@@ -32,6 +32,7 @@
 
 #include <cmath>
 #include <fstream>
+#include <iomanip>
 #include <ostream>
 #include <sstream>
 
@@ -136,7 +137,7 @@ std::vector<double> Statistics::GetAllSamples(size_t handle) {
   std::lock_guard<std::mutex> lock(Instance().mutex_);
   return Instance().stats_collectors_[handle].GetAllValues();
 }
-std::vector<double> Statistics::GetAllSamples(std::string const &tag) {
+std::vector<double> Statistics::GetAllSamples(std::string const& tag) {
   return GetAllSamples(GetHandle(tag));
 }
 double Statistics::GetVariance(size_t handle) {
@@ -164,21 +165,21 @@ double Statistics::GetMedian(size_t handle) {
   std::lock_guard<std::mutex> lock(Instance().mutex_);
   return Instance().stats_collectors_[handle].Median();
 }
-double Statistics::GetMedian(std::string const &tag) {
+double Statistics::GetMedian(std::string const& tag) {
   return GetMedian(GetHandle(tag));
 }
 double Statistics::GetQ1(size_t handle) {
   std::lock_guard<std::mutex> lock(Instance().mutex_);
   return Instance().stats_collectors_[handle].Q1();
 }
-double Statistics::GetQ1(std::string const &tag) {
+double Statistics::GetQ1(std::string const& tag) {
   return GetQ1(GetHandle(tag));
 }
 double Statistics::GetQ3(size_t handle) {
   std::lock_guard<std::mutex> lock(Instance().mutex_);
   return Instance().stats_collectors_[handle].Q1();
 }
-double Statistics::GetQ3(std::string const &tag) {
+double Statistics::GetQ3(std::string const& tag) {
   return GetQ3(GetHandle(tag));
 }
 double Statistics::GetHz(size_t handle) {
@@ -233,7 +234,8 @@ std::string Statistics::SecondsToTimeString(double seconds) {
   minutes = minutes - (hours * 60);
 
   char buffer[256];
-  snprintf(buffer, sizeof(buffer),
+  snprintf(buffer,
+           sizeof(buffer),
 #ifdef SM_TIMING_SHOW_HOURS
            "%02d:"
 #endif
@@ -266,36 +268,39 @@ void Statistics::Print(std::ostream& out) {  // NOLINT
   out.width(7);
   out.setf(std::ios::right, std::ios::adjustfield);
   out << "#\t";
-  out << "Hz\t";
-  out << "(avg     +- std    )\t";
+  out << "Log Hz\t";
+  out << "{avg     +- std    }\t";
   out << "[min,max]\n";
 
   for (const typename map_t::value_type& t : tag_map) {
     size_t i = t.second;
     out.width((std::streamsize)Instance().max_tag_length_);
     out.setf(std::ios::left, std::ios::adjustfield);
+    // Print Name of tag
     out << t.first << "\t";
-    out.width(7);
 
     out.setf(std::ios::right, std::ios::adjustfield);
-    out << GetNumSamples(i) << "\t";
+    // Print #
+    out << std::setw(5) << GetNumSamples(i) << "\t";
     if (GetNumSamples(i) > 0) {
-      out << GetHz(i) << "\t";
+      out << std::showpoint << GetHz(i) << "\t";
       double mean = GetMean(i);
       double stddev = sqrt(GetVariance(i));
-      out << "(" << mean << " +- ";
-      out << stddev << ")\t";
+      out << "{" << std::showpoint << mean;
+      out << " +- ";
+      out << std::showpoint << stddev << "}\t";
 
       double min_value = GetMin(i);
       double max_value = GetMax(i);
 
-      out << "[" << min_value << "," << max_value << "]";
+      //out.width(5);
+      out << std::noshowpoint << "[" << min_value << "," << max_value << "]";
     }
     out << std::endl;
   }
 }
 
-void Statistics::WriteAllSamplesToCsvFile(const std::string &path) {
+void Statistics::WriteAllSamplesToCsvFile(const std::string& path) {
   const map_t& tag_map = Instance().tag_map_;
   if (tag_map.empty()) {
     return;
@@ -308,33 +313,33 @@ void Statistics::WriteAllSamplesToCsvFile(const std::string &path) {
   }
 
   VLOG(1) << "Writing statistics to file: " << path;
-  for (const map_t::value_type &tag : tag_map) {
-    const size_t &index = tag.second;
+  for (const map_t::value_type& tag : tag_map) {
+    const size_t& index = tag.second;
     if (GetNumSamples(index) > 0) {
-      const std::string &label = tag.first;
+      const std::string& label = tag.first;
 
       // Add header to csv file. tag.first is the stats label.
       output_file << label;
 
       // Each row has all samples.
-      const std::vector<double> &samples = GetAllSamples(index);
-      for (const auto &sample : samples) {
+      const std::vector<double>& samples = GetAllSamples(index);
+      for (const auto& sample : samples) {
         output_file << ',' << sample;
       }
       output_file << '\n';
     }
   }
 }
 
-void Statistics::WriteToYamlFile(const std::string &path) {
+void Statistics::WriteToYamlFile(const std::string& path) {
   std::ofstream output_file(path);
 
   if (!output_file) {
     LOG(ERROR) << "Could not write statistics: Unable to open file: " << path;
     return;
   }
 
-  const map_t &tag_map = Instance().tag_map_;
+  const map_t& tag_map = Instance().tag_map_;
   if (tag_map.empty()) {
     return;
   }