From 5b5ded25c9207c5e3bc1d46535771adcbc578503 Mon Sep 17 00:00:00 2001
From: John Zhao <johnzhao@slightech.com>
Date: Mon, 25 Feb 2019 12:09:48 +0800
Subject: [PATCH] fix(correspondence): also wait stream matched ready

---
 src/mynteye/api/correspondence.cc | 15 ++++++++++++++-
 src/mynteye/api/correspondence.h  |  3 ++-
 2 files changed, 16 insertions(+), 2 deletions(-)

diff --git a/src/mynteye/api/correspondence.cc b/src/mynteye/api/correspondence.cc
index 6ebc7da..d4af6cd 100644
--- a/src/mynteye/api/correspondence.cc
+++ b/src/mynteye/api/correspondence.cc
@@ -171,9 +171,13 @@ void Correspondence::NotifyStreamDataReady() {
 
 bool Correspondence::IsStreamDataReady() {
   if (stream_datas_.empty()) return false;
+  if (stream_match_enabled_) {
+    if (stream_datas_match_.empty()) return false;
+  }
   if (motion_datas_.empty()) return false;
 
   std::uint64_t img_stamp = 0;
+  std::uint64_t img_macth_stamp = 0;
   {
     std::lock_guard<std::recursive_mutex> _(mtx_stream_datas_);
     auto data = stream_datas_.front();
@@ -181,6 +185,10 @@ bool Correspondence::IsStreamDataReady() {
       LOG(FATAL) << "stream data image info is empty!";
     }
     img_stamp = data.img->timestamp;
+
+    if (stream_match_enabled_) {
+      img_macth_stamp = stream_datas_match_.front().img->timestamp;
+    }
   }
   std::uint64_t imu_stamp = 0;
   {
@@ -192,7 +200,12 @@ bool Correspondence::IsStreamDataReady() {
     imu_stamp = data.imu->timestamp;
   }
 
-  return img_stamp + stream_interval_us_half_ < imu_stamp;
+  if (stream_match_enabled_) {
+    return img_stamp + stream_interval_us_half_ < imu_stamp
+        && img_macth_stamp + stream_interval_us_half_ < imu_stamp;
+  } else {
+    return img_stamp + stream_interval_us_half_ < imu_stamp;
+  }
 }
 
 std::vector<api::StreamData> Correspondence::GetReadyStreamData(bool matched) {
diff --git a/src/mynteye/api/correspondence.h b/src/mynteye/api/correspondence.h
index e2cbd98..df822e0 100644
--- a/src/mynteye/api/correspondence.h
+++ b/src/mynteye/api/correspondence.h
@@ -15,6 +15,7 @@
 #define MYNTEYE_API_CONFIG_H_
 #pragma once
 
+#include <atomic>
 #include <condition_variable>
 #include <memory>
 #include <mutex>
@@ -57,7 +58,7 @@ class Correspondence {
   std::shared_ptr<Device> device_;
   Stream stream_;
   Stream stream_match_;
-  bool stream_match_enabled_;
+  std::atomic_bool stream_match_enabled_;
 
   float stream_interval_us_;
   float stream_interval_us_half_;