From a38e6a782a2521cfc2e2801912cbc723bc5402a8 Mon Sep 17 00:00:00 2001
From: John Zhao <johnzhao@slightech.com>
Date: Thu, 20 Dec 2018 22:08:29 +0800
Subject: [PATCH] fix(api): update rectify params after config stream request

---
 src/mynteye/api/api.cc                        |  4 ++-
 .../api/processor/rectify_processor.cc        | 13 ++++++---
 src/mynteye/api/processor/rectify_processor.h |  4 +++
 src/mynteye/api/synthetic.cc                  | 29 +++++++++----------
 src/mynteye/api/synthetic.h                   |  2 ++
 5 files changed, 31 insertions(+), 21 deletions(-)

diff --git a/src/mynteye/api/api.cc b/src/mynteye/api/api.cc
index 4780a4b..41c201f 100644
--- a/src/mynteye/api/api.cc
+++ b/src/mynteye/api/api.cc
@@ -210,7 +210,7 @@ std::vector<std::string> get_plugin_paths() {
 
 API::API(std::shared_ptr<Device> device) : device_(device) {
   VLOG(2) << __func__;
-  std::dynamic_pointer_cast<StandardDevice>(device_);
+  // std::dynamic_pointer_cast<StandardDevice>(device_);
   synthetic_.reset(new Synthetic(this));
 }
 
@@ -266,6 +266,7 @@ const std::vector<StreamRequest> &API::GetStreamRequests(
 void API::ConfigStreamRequest(
     const Capabilities &capability, const StreamRequest &request) {
   device_->ConfigStreamRequest(capability, request);
+  synthetic_->NotifyImageParamsChanged();
 }
 
 const StreamRequest &API::GetStreamRequest(
@@ -279,6 +280,7 @@ const std::vector<StreamRequest> &API::GetStreamRequests() const {
 
 void API::ConfigStreamRequest(const StreamRequest &request) {
   device_->ConfigStreamRequest(request);
+  synthetic_->NotifyImageParamsChanged();
 }
 
 const StreamRequest &API::GetStreamRequest() const {
diff --git a/src/mynteye/api/processor/rectify_processor.cc b/src/mynteye/api/processor/rectify_processor.cc
index 9c570f0..86b0988 100644
--- a/src/mynteye/api/processor/rectify_processor.cc
+++ b/src/mynteye/api/processor/rectify_processor.cc
@@ -27,11 +27,9 @@ const char RectifyProcessor::NAME[] = "RectifyProcessor";
 
 RectifyProcessor::RectifyProcessor(
     std::shared_ptr<Device> device, std::int32_t proc_period)
-    : Processor(std::move(proc_period)) {
+    : Processor(std::move(proc_period)), device_(device) {
   VLOG(2) << __func__ << ": proc_period=" << proc_period;
-  InitParams(
-      device->GetIntrinsics(Stream::LEFT), device->GetIntrinsics(Stream::RIGHT),
-      device->GetExtrinsics(Stream::RIGHT, Stream::LEFT));
+  NotifyImageParamsChanged();
 }
 
 RectifyProcessor::~RectifyProcessor() {
@@ -42,6 +40,13 @@ std::string RectifyProcessor::Name() {
   return NAME;
 }
 
+void RectifyProcessor::NotifyImageParamsChanged() {
+  InitParams(
+      device_->GetIntrinsics(Stream::LEFT),
+      device_->GetIntrinsics(Stream::RIGHT),
+      device_->GetExtrinsics(Stream::RIGHT, Stream::LEFT));
+}
+
 Object *RectifyProcessor::OnCreateOutput() {
   return new ObjMat2();
 }
diff --git a/src/mynteye/api/processor/rectify_processor.h b/src/mynteye/api/processor/rectify_processor.h
index 0cb33a0..30b615b 100644
--- a/src/mynteye/api/processor/rectify_processor.h
+++ b/src/mynteye/api/processor/rectify_processor.h
@@ -37,6 +37,8 @@ class RectifyProcessor : public Processor {
 
   std::string Name() override;
 
+  void NotifyImageParamsChanged();
+
   cv::Mat R1, P1, R2, P2, Q;
   cv::Mat map11, map12, map21, map22;
 
@@ -48,6 +50,8 @@ class RectifyProcessor : public Processor {
  private:
   void InitParams(
       Intrinsics in_left, Intrinsics in_right, Extrinsics ex_right_to_left);
+
+  std::shared_ptr<Device> device_;
 };
 
 MYNTEYE_END_NAMESPACE
diff --git a/src/mynteye/api/synthetic.cc b/src/mynteye/api/synthetic.cc
index 04f1ca9..44fe48b 100644
--- a/src/mynteye/api/synthetic.cc
+++ b/src/mynteye/api/synthetic.cc
@@ -83,6 +83,11 @@ Synthetic::~Synthetic() {
   }
 }
 
+void Synthetic::NotifyImageParamsChanged() {
+  auto &&processor = find_processor<RectifyProcessor>(processor_);
+  if (processor) processor->NotifyImageParamsChanged();
+}
+
 bool Synthetic::Supports(const Stream &stream) const {
   return stream_supports_mode_.find(stream) != stream_supports_mode_.end();
 }
@@ -311,42 +316,35 @@ void Synthetic::EnableStreamData(const Stream &stream, std::uint32_t depth) {
         break;
       stream_enabled_mode_[stream] = MODE_SYNTHETIC;
       CHECK(ActivateProcessor<RectifyProcessor>());
-    }
-      return;
+    } return;
     case Stream::RIGHT_RECTIFIED: {
       if (!IsStreamDataEnabled(Stream::RIGHT))
         break;
       stream_enabled_mode_[stream] = MODE_SYNTHETIC;
       CHECK(ActivateProcessor<RectifyProcessor>());
-    }
-      return;
+    } return;
     case Stream::DISPARITY: {
       stream_enabled_mode_[stream] = MODE_SYNTHETIC;
       EnableStreamData(Stream::LEFT_RECTIFIED, depth + 1);
       EnableStreamData(Stream::RIGHT_RECTIFIED, depth + 1);
       CHECK(ActivateProcessor<DisparityProcessor>());
-    }
-      return;
+    } return;
     case Stream::DISPARITY_NORMALIZED: {
       stream_enabled_mode_[stream] = MODE_SYNTHETIC;
       EnableStreamData(Stream::DISPARITY, depth + 1);
       CHECK(ActivateProcessor<DisparityNormalizedProcessor>());
-    }
-      return;
+    } return;
     case Stream::POINTS: {
       stream_enabled_mode_[stream] = MODE_SYNTHETIC;
       EnableStreamData(Stream::DISPARITY, depth + 1);
       CHECK(ActivateProcessor<PointsProcessor>());
-    }
-      return;
+    } return;
     case Stream::DEPTH: {
       stream_enabled_mode_[stream] = MODE_SYNTHETIC;
       EnableStreamData(Stream::POINTS, depth + 1);
       CHECK(ActivateProcessor<DepthProcessor>());
-    }
-      return;
-    default:
-      break;
+    } return;
+    default: break;
   }
   if (depth == 0) {
     LOG(WARNING) << "Enable stream data of " << stream << " failed";
@@ -399,8 +397,7 @@ void Synthetic::DisableStreamData(const Stream &stream, std::uint32_t depth) {
       case Stream::DEPTH: {
         DeactivateProcessor<DepthProcessor>();
       } break;
-      default:
-        return;
+      default: return;
     }
     if (depth > 0) {
       LOG(WARNING) << "Disable synthetic stream data of " << stream << " too";
diff --git a/src/mynteye/api/synthetic.h b/src/mynteye/api/synthetic.h
index 441ddfb..27c9d44 100644
--- a/src/mynteye/api/synthetic.h
+++ b/src/mynteye/api/synthetic.h
@@ -43,6 +43,8 @@ class Synthetic {
   explicit Synthetic(API *api);
   ~Synthetic();
 
+  void NotifyImageParamsChanged();
+
   bool Supports(const Stream &stream) const;
   mode_t SupportsMode(const Stream &stream) const;