Drop wrong image and the first one

src/device/device.cc

@@ -321,15 +321,25 @@ void Device::StartVideoStreaming() {
         *device_, stream_request.width, stream_request.height,
         static_cast<int>(stream_request.format), stream_request.fps,
         [this](const void *data) {
+          // drop the first stereo stream data
+          static std::uint8_t drop_count = 1;
+          if (drop_count > 0) {
+            --drop_count;
+            return;
+          }
           std::lock_guard<std::mutex> _(mtx_streams_);
           streams_->PushStream(Capabilities::STEREO, data);
           if (HasStreamCallback(Stream::LEFT)) {
-            auto &&stream_data = streams_->stream_datas(Stream::LEFT).back();
-            stream_callbacks_.at(Stream::LEFT)(stream_data);
+            auto &&stream_datas = streams_->stream_datas(Stream::LEFT);
+            if (stream_datas.size() > 0) {
+              stream_callbacks_.at(Stream::LEFT)(stream_datas.back());
+            }
           }
           if (HasStreamCallback(Stream::RIGHT)) {
-            auto &&stream_data = streams_->stream_datas(Stream::RIGHT).back();
-            stream_callbacks_.at(Stream::RIGHT)(stream_data);
+            auto &&stream_datas = streams_->stream_datas(Stream::RIGHT);
+            if (stream_datas.size() > 0) {
+              stream_callbacks_.at(Stream::RIGHT)(stream_datas.back());
+            }
           }
         });
   } else {

src/internal/streams.cc

@@ -5,6 +5,7 @@
 #include <algorithm>
 #include <chrono>
 #include <iomanip>
+#include <stdexcept>
 
 #include "internal/types.h"
 
@@ -136,24 +137,26 @@ void Streams::PushStream(const Capabilities &capability, const void *data) {
   auto &&request = GetStreamConfigRequest(capability);
   switch (capability) {
     case Capabilities::STEREO: {
-      // alloc
+      // alloc left
       AllocStreamData(Stream::LEFT, request, Format::GREY);
-      AllocStreamData(Stream::RIGHT, request, Format::GREY);
       auto &&left_data = stream_datas_map_[Stream::LEFT].back();
-      auto &&right_data = stream_datas_map_[Stream::RIGHT].back();
       // unpack img data
       if (unpack_img_data_map_[Stream::LEFT](
               data, request, left_data.img.get())) {
-        // TODO(JohnZhao)
+        // alloc right
+        AllocStreamData(Stream::RIGHT, request, Format::GREY);
+        auto &&right_data = stream_datas_map_[Stream::RIGHT].back();
+        *right_data.img = *left_data.img;
+        // unpack frame
+        unpack_img_pixels_map_[Stream::LEFT](
+            data, request, left_data.frame.get());
+        unpack_img_pixels_map_[Stream::RIGHT](
+            data, request, right_data.frame.get());
       } else {
+        // discard left
+        DiscardStreamData(Stream::LEFT);
         LOG(WARNING) << "Image packet is unaccepted, frame dropped";
       }
-      *right_data.img = *left_data.img;
-      // unpack frame
-      unpack_img_pixels_map_[Stream::LEFT](
-          data, request, left_data.frame.get());
-      unpack_img_pixels_map_[Stream::RIGHT](
-          data, request, right_data.frame.get());
     } break;
     default:
       LOG(FATAL) << "Not supported " << capability << " now";
@@ -186,9 +189,14 @@ Streams::stream_data_t Streams::GetLatestStreamData(const Stream &stream) {
   return GetStreamDatas(stream).back();
 }
 
-const Streams::stream_datas_t &Streams::stream_datas(
-    const Stream &stream) const {
-  return stream_datas_map_.at(stream);
+const Streams::stream_datas_t &Streams::stream_datas(const Stream &stream) {
+  try {
+    return stream_datas_map_.at(stream);
+  } catch (const std::out_of_range &e) {
+    // Add empty vector of this stream key
+    stream_datas_map_[stream] = {};
+    return stream_datas_map_.at(stream);
+  }
 }
 
 bool Streams::IsStreamCapability(const Capabilities &capability) const {
@@ -236,6 +244,16 @@ void Streams::AllocStreamData(
   }
 }
 
+void Streams::DiscardStreamData(const Stream &stream) {
+  // Must discard after alloc, otherwise at will out of range when no this key.
+  if (stream_datas_map_.at(stream).size() > 0) {
+    auto &&datas = stream_datas_map_[stream];
+    datas.erase(datas.end() - 1);
+  } else {
+    VLOG(2) << "Stream data of " << stream << " is empty, could not discard";
+  }
+}
+
 bool Streams::HasKeyStreamDatas() const {
   for (auto &&s : key_streams_) {
     if (!HasStreamDatas(s))

src/internal/streams.h

@@ -39,7 +39,7 @@ class Streams {
   stream_datas_t GetStreamDatas(const Stream &stream);
   stream_data_t GetLatestStreamData(const Stream &stream);
 
-  const stream_datas_t &stream_datas(const Stream &stream) const;
+  const stream_datas_t &stream_datas(const Stream &stream);
 
  private:
   bool IsStreamCapability(const Capabilities &capability) const;
@@ -54,6 +54,8 @@ class Streams {
   void AllocStreamData(
       const Stream &stream, const StreamRequest &request, const Format &format);
 
+  void DiscardStreamData(const Stream &stream);
+
   bool HasKeyStreamDatas() const;
 
   std::vector<Stream> key_streams_;