diff --git a/src/api/processor/depth_processor.cc b/src/api/processor/depth_processor.cc index 83b9d44..8d5c9d4 100644 --- a/src/api/processor/depth_processor.cc +++ b/src/api/processor/depth_processor.cc @@ -15,10 +15,13 @@ #include +#include + MYNTEYE_BEGIN_NAMESPACE -DepthProcessor::DepthProcessor() : Processor() { - VLOG(2) << __func__; +DepthProcessor::DepthProcessor(std::int32_t proc_period) + : Processor(std::move(proc_period)) { + VLOG(2) << __func__ << ": proc_period=" << proc_period; } DepthProcessor::~DepthProcessor() { diff --git a/src/api/processor/depth_processor.h b/src/api/processor/depth_processor.h index 96fe0cb..f7a94ba 100644 --- a/src/api/processor/depth_processor.h +++ b/src/api/processor/depth_processor.h @@ -25,7 +25,7 @@ class DepthProcessor : public Processor { public: static constexpr auto &&NAME = "DepthProcessor"; - DepthProcessor(); + explicit DepthProcessor(std::int32_t proc_period = 0); virtual ~DepthProcessor(); std::string Name() override; diff --git a/src/api/processor/disparity_normalized_processor.cc b/src/api/processor/disparity_normalized_processor.cc index d9c68f4..fa13844 100644 --- a/src/api/processor/disparity_normalized_processor.cc +++ b/src/api/processor/disparity_normalized_processor.cc @@ -17,10 +17,14 @@ #include +#include + MYNTEYE_BEGIN_NAMESPACE -DisparityNormalizedProcessor::DisparityNormalizedProcessor() : Processor() { - VLOG(2) << __func__; +DisparityNormalizedProcessor::DisparityNormalizedProcessor( + std::int32_t proc_period) + : Processor(std::move(proc_period)) { + VLOG(2) << __func__ << ": proc_period=" << proc_period; } DisparityNormalizedProcessor::~DisparityNormalizedProcessor() { diff --git a/src/api/processor/disparity_normalized_processor.h b/src/api/processor/disparity_normalized_processor.h index 7a15f64..a66d078 100644 --- a/src/api/processor/disparity_normalized_processor.h +++ b/src/api/processor/disparity_normalized_processor.h @@ -25,7 +25,7 @@ class DisparityNormalizedProcessor : public Processor { public: static constexpr auto &&NAME = "DisparityNormalizedProcessor"; - DisparityNormalizedProcessor(); + explicit DisparityNormalizedProcessor(std::int32_t proc_period = 0); virtual ~DisparityNormalizedProcessor(); std::string Name() override; diff --git a/src/api/processor/disparity_processor.cc b/src/api/processor/disparity_processor.cc index 3ec2305..ff3e71c 100644 --- a/src/api/processor/disparity_processor.cc +++ b/src/api/processor/disparity_processor.cc @@ -17,10 +17,13 @@ #include +#include + MYNTEYE_BEGIN_NAMESPACE -DisparityProcessor::DisparityProcessor() : Processor() { - VLOG(2) << __func__; +DisparityProcessor::DisparityProcessor(std::int32_t proc_period) + : Processor(std::move(proc_period)) { + VLOG(2) << __func__ << ": proc_period=" << proc_period; int sgbmWinSize = 3; int numberOfDisparities = 64; diff --git a/src/api/processor/disparity_processor.h b/src/api/processor/disparity_processor.h index fb29f44..c1b8671 100644 --- a/src/api/processor/disparity_processor.h +++ b/src/api/processor/disparity_processor.h @@ -31,7 +31,7 @@ class DisparityProcessor : public Processor { public: static constexpr auto &&NAME = "DisparityProcessor"; - DisparityProcessor(); + explicit DisparityProcessor(std::int32_t proc_period = 0); virtual ~DisparityProcessor(); std::string Name() override; diff --git a/src/api/processor/points_processor.cc b/src/api/processor/points_processor.cc index e348f3a..0bb82ec 100644 --- a/src/api/processor/points_processor.cc +++ b/src/api/processor/points_processor.cc @@ -21,8 +21,9 @@ MYNTEYE_BEGIN_NAMESPACE -PointsProcessor::PointsProcessor(cv::Mat Q) : Processor(), Q_(std::move(Q)) { - VLOG(2) << __func__; +PointsProcessor::PointsProcessor(cv::Mat Q, std::int32_t proc_period) + : Processor(std::move(proc_period)), Q_(std::move(Q)) { + VLOG(2) << __func__ << ": proc_period=" << proc_period; } PointsProcessor::~PointsProcessor() { diff --git a/src/api/processor/points_processor.h b/src/api/processor/points_processor.h index fcdaadd..0519c39 100644 --- a/src/api/processor/points_processor.h +++ b/src/api/processor/points_processor.h @@ -27,7 +27,7 @@ class PointsProcessor : public Processor { public: static constexpr auto &&NAME = "PointsProcessor"; - explicit PointsProcessor(cv::Mat Q); + explicit PointsProcessor(cv::Mat Q, std::int32_t proc_period = 0); virtual ~PointsProcessor(); std::string Name() override; diff --git a/src/api/processor/processor.cc b/src/api/processor/processor.cc index 9c1c50d..75e5e9e 100644 --- a/src/api/processor/processor.cc +++ b/src/api/processor/processor.cc @@ -17,10 +17,13 @@ #include +#include "internal/times.h" + MYNTEYE_BEGIN_NAMESPACE -Processor::Processor() - : activated_(false), +Processor::Processor(std::int32_t proc_period) + : proc_period_(std::move(proc_period)), + activated_(false), input_ready_(false), idle_(true), dropped_count_(0), @@ -146,10 +149,34 @@ std::uint64_t Processor::GetDroppedCount() { void Processor::Run() { VLOG(2) << Name() << " thread start"; + + auto sleep = [this](const times::system_clock::time_point &time_beg) { + if (proc_period_ > 0) { + static times::system_clock::time_point time_prev = time_beg; + auto &&time_elapsed_ms = + times::count(times::now() - time_prev); + time_prev = time_beg; + + if (time_elapsed_ms < proc_period_) { + VLOG(2) << Name() << " process cost " + << times::count(times::now() - time_beg) + << " ms, sleep " << (proc_period_ - time_elapsed_ms) << " ms"; + std::this_thread::sleep_for( + std::chrono::milliseconds(proc_period_ - time_elapsed_ms)); + return; + } + } + VLOG(2) << Name() << " process cost " + << times::count(times::now() - time_beg) + << " ms"; + }; + while (true) { std::unique_lock lk(mtx_input_ready_); cond_input_ready_.wait(lk, [this] { return input_ready_; }); + auto &&time_beg = times::now(); + if (!activated_) { SetIdle(true); input_ready_ = false; @@ -194,6 +221,8 @@ void Processor::Run() { SetIdle(true); input_ready_ = false; + + sleep(time_beg); } VLOG(2) << Name() << " thread end"; } diff --git a/src/api/processor/processor.h b/src/api/processor/processor.h index c116c6e..505b38d 100644 --- a/src/api/processor/processor.h +++ b/src/api/processor/processor.h @@ -37,7 +37,7 @@ class Processor /*: public std::enable_shared_from_this*/ { using ProcessCallback = std::function; - Processor(); + explicit Processor(std::int32_t proc_period = 0); virtual ~Processor(); virtual std::string Name(); @@ -80,6 +80,8 @@ class Processor /*: public std::enable_shared_from_this*/ { void SetIdle(bool idle); + std::int32_t proc_period_; + bool activated_; bool input_ready_; diff --git a/src/api/processor/rectify_processor.cc b/src/api/processor/rectify_processor.cc index 1034930..6197f0b 100644 --- a/src/api/processor/rectify_processor.cc +++ b/src/api/processor/rectify_processor.cc @@ -18,13 +18,16 @@ #include +#include + #include "device/device.h" MYNTEYE_BEGIN_NAMESPACE -RectifyProcessor::RectifyProcessor(std::shared_ptr device) - : Processor() { - VLOG(2) << __func__; +RectifyProcessor::RectifyProcessor( + std::shared_ptr device, std::int32_t proc_period) + : Processor(std::move(proc_period)) { + VLOG(2) << __func__ << ": proc_period=" << proc_period; InitParams( device->GetIntrinsics(Stream::LEFT), device->GetIntrinsics(Stream::RIGHT), device->GetExtrinsics(Stream::LEFT, Stream::RIGHT)); diff --git a/src/api/processor/rectify_processor.h b/src/api/processor/rectify_processor.h index 808c2c9..8f8c190 100644 --- a/src/api/processor/rectify_processor.h +++ b/src/api/processor/rectify_processor.h @@ -31,7 +31,8 @@ class RectifyProcessor : public Processor { public: static constexpr auto &&NAME = "RectifyProcessor"; - explicit RectifyProcessor(std::shared_ptr device); + RectifyProcessor( + std::shared_ptr device, std::int32_t proc_period = 0); virtual ~RectifyProcessor(); std::string Name() override; diff --git a/src/api/synthetic.cc b/src/api/synthetic.cc index 447c0a9..94bd351 100644 --- a/src/api/synthetic.cc +++ b/src/api/synthetic.cc @@ -29,6 +29,12 @@ #include "api/processor/rectify_processor.h" #include "device/device.h" +#define RECTIFY_PROC_PERIOD 0 +#define DISPARITY_PROC_PERIOD 0 +#define DISPARITY_NORM_PROC_PERIOD 0 +#define POINTS_PROC_PERIOD 0 +#define DEPTH_PROC_PERIOD 0 + MYNTEYE_BEGIN_NAMESPACE namespace { @@ -383,13 +389,16 @@ void Synthetic::DisableStreamData(const Stream &stream, std::uint32_t depth) { } void Synthetic::InitProcessors() { - auto &&rectify_processor = std::make_shared(api_->device()); - auto &&disparity_processor = std::make_shared(); + auto &&rectify_processor = + std::make_shared(api_->device(), RECTIFY_PROC_PERIOD); + auto &&disparity_processor = + std::make_shared(DISPARITY_PROC_PERIOD); auto &&disparitynormalized_processor = - std::make_shared(); - auto &&points_processor = - std::make_shared(rectify_processor->Q); - auto &&depth_processor = std::make_shared(); + std::make_shared( + DISPARITY_NORM_PROC_PERIOD); + auto &&points_processor = std::make_shared( + rectify_processor->Q, POINTS_PROC_PERIOD); + auto &&depth_processor = std::make_shared(DEPTH_PROC_PERIOD); using namespace std::placeholders; // NOLINT rectify_processor->SetProcessCallback(