diff --git a/README.md b/README.md index dfd3856b7..c8616eba7 100644 --- a/README.md +++ b/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) diff --git a/include/kimera-vio/common/vio_types.h b/include/kimera-vio/common/vio_types.h index 3eb909d28..3a323a4fa 100644 --- a/include/kimera-vio/common/vio_types.h +++ b/include/kimera-vio/common/vio_types.h @@ -57,6 +57,8 @@ using StatusKeypointCV = std::pair; using StatusKeypointsCV = std::vector; using BearingVectors = std::vector>; + +// TODO(Toni): move make unique and to underlying to another file... // Add compatibility for c++11's lack of make_unique. template std::unique_ptr make_unique(Args&&... args) { diff --git a/include/kimera-vio/utils/ThreadsafeQueue.h b/include/kimera-vio/utils/ThreadsafeQueue.h index 8dbe8874c..f128bd401 100644 --- a/include/kimera-vio/utils/ThreadsafeQueue.h +++ b/include/kimera-vio/utils/ThreadsafeQueue.h @@ -25,7 +25,9 @@ #include +#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 { using TQB = ThreadsafeQueueBase; 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 { using TQB::data_queue_; using TQB::mutex_; using TQB::shutdown_; + + //! Stats on how full the queue gets. + std::unique_ptr queue_size_stats_; }; /** @@ -258,20 +264,27 @@ ThreadsafeQueueBase::ThreadsafeQueueBase(const std::string& queue_id) shutdown_(false) {} template -ThreadsafeQueue::ThreadsafeQueue(const std::string& queue_id) - : ThreadsafeQueueBase(queue_id) {} +ThreadsafeQueue::ThreadsafeQueue(const std::string& queue_id, + const bool& log_queue_size) + : ThreadsafeQueueBase(queue_id), + queue_size_stats_( + log_queue_size + ? VIO::make_unique(queue_id + " Size [#]") + : nullptr) {} template bool ThreadsafeQueue::push(T new_value) { if (shutdown_) return false; // atomic, no lock needed. std::shared_ptr data(std::make_shared(std::move(new_value))); std::unique_lock 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::pushBlockingIfFull(T new_value, if (shutdown_) return false; // atomic, no lock needed. std::shared_ptr data(std::make_shared(std::move(new_value))); std::unique_lock 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; } diff --git a/src/frontend/StereoVisionFrontEnd.cpp b/src/frontend/StereoVisionFrontEnd.cpp index 343d9bdde..52210f61a 100644 --- a/src/frontend/StereoVisionFrontEnd.cpp +++ b/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(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( @@ -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(false, diff --git a/src/pipeline/Pipeline.cpp b/src/pipeline/Pipeline.cpp index 5d0aa380e..509575ddb 100644 --- a/src/pipeline/Pipeline.cpp +++ b/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(); diff --git a/src/utils/Statistics.cpp b/src/utils/Statistics.cpp index ff1ad472d..2b017a2a0 100644 --- a/src/utils/Statistics.cpp +++ b/src/utils/Statistics.cpp @@ -32,6 +32,7 @@ #include #include +#include #include #include @@ -136,7 +137,7 @@ std::vector Statistics::GetAllSamples(size_t handle) { std::lock_guard lock(Instance().mutex_); return Instance().stats_collectors_[handle].GetAllValues(); } -std::vector Statistics::GetAllSamples(std::string const &tag) { +std::vector 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 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 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 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,17 +313,17 @@ 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 &samples = GetAllSamples(index); - for (const auto &sample : samples) { + const std::vector& samples = GetAllSamples(index); + for (const auto& sample : samples) { output_file << ',' << sample; } output_file << '\n'; @@ -326,7 +331,7 @@ void Statistics::WriteAllSamplesToCsvFile(const std::string &path) { } } -void Statistics::WriteToYamlFile(const std::string &path) { +void Statistics::WriteToYamlFile(const std::string& path) { std::ofstream output_file(path); if (!output_file) { @@ -334,7 +339,7 @@ void Statistics::WriteToYamlFile(const std::string &path) { return; } - const map_t &tag_map = Instance().tag_map_; + const map_t& tag_map = Instance().tag_map_; if (tag_map.empty()) { return; }