refactor(*): adapt stereo stream to different device

CMakeLists.txt

@@ -171,8 +171,11 @@ set(MYNTEYE_SRCS
   src/mynteye/device/config.cc
   src/mynteye/device/context.cc
   src/mynteye/device/device.cc
-  src/mynteye/device/device_s.cc
   src/mynteye/device/motions.cc
+  src/mynteye/device/standard/device_s.cc
+  src/mynteye/device/standard/streams_adapter_s.cc
+  src/mynteye/device/standard2/device_s2.cc
+  src/mynteye/device/standard2/streams_adapter_s2.cc
   src/mynteye/device/streams.cc
   src/mynteye/device/types.cc
   src/mynteye/device/utils.cc

include/mynteye/device/device.h

@@ -45,6 +45,7 @@ class API;
 class Channels;
 class Motions;
 class Streams;
+class StreamsAdapter;
 
 template <class Data>
 class AsyncCallback;
@@ -300,7 +301,7 @@ class MYNTEYE_API Device {
   virtual void OnStereoStreamUpdate();
 
   virtual Capabilities GetKeyStreamCapability() const = 0;
-  virtual std::vector<Stream> GetKeyStreams() const = 0;
+  virtual std::shared_ptr<StreamsAdapter> CreateStreamsAdapter() const = 0;
 
   std::map<Resolution, device::img_params_t> GetImgParams() const {
     return all_img_params_;

include/mynteye/types.h

@@ -37,7 +37,7 @@ MYNTEYE_BEGIN_NAMESPACE
 enum class Model : std::uint8_t {
   /** Standard */
   STANDARD,
-  /** Standard 2 */
+  /** Standard 2 generation */
   STANDARD2,
   /** Last guard */
   LAST

src/mynteye/api/api.cc

@@ -26,7 +26,6 @@
 #include "mynteye/api/plugin.h"
 #include "mynteye/api/synthetic.h"
 #include "mynteye/device/device.h"
-#include "mynteye/device/device_s.h"
 #include "mynteye/device/utils.h"
 
 #if defined(WITH_FILESYSTEM) && defined(WITH_NATIVE_FILESYSTEM)

src/mynteye/api/synthetic.cc

@@ -48,7 +48,7 @@ cv::Mat frame2mat(const std::shared_ptr<device::Frame> &frame) {
   } else if (frame->format() == Format::BGR888) {
     cv::Mat img(frame->height(), frame->width(), CV_8UC3, frame->data());
     return img;
-  } else {
+  } else {  // Format::GRAY
     return cv::Mat(frame->height(), frame->width(), CV_8UC1, frame->data());
   }
 }

src/mynteye/device/device.cc

@@ -23,8 +23,9 @@
 #include "mynteye/device/async_callback.h"
 #include "mynteye/device/channels.h"
 #include "mynteye/device/config.h"
-#include "mynteye/device/device_s.h"
 #include "mynteye/device/motions.h"
+#include "mynteye/device/standard/device_s.h"
+#include "mynteye/device/standard2/device_s2.h"
 #include "mynteye/device/streams.h"
 #include "mynteye/device/types.h"
 #include "mynteye/util/strings.h"
@@ -108,7 +109,7 @@ std::shared_ptr<Device> Device::Create(
         case '1':
           return std::make_shared<StandardDevice>(device);
         case '2':
-          return std::make_shared<StandardDevice>(device);
+          return std::make_shared<Standard2Device>(device);
         default:
           LOG(FATAL) << "No such generation now";
       }
@@ -470,51 +471,54 @@ void Device::StartVideoStreaming() {
     return;
   }
 
-  streams_ = std::make_shared<Streams>(GetKeyStreams());
+  streams_ = std::make_shared<Streams>(CreateStreamsAdapter());
 
   // if stream capabilities are supported with subdevices of device_
+  /*
   Capabilities stream_capabilities[] = {
-      Capabilities::STEREO,       Capabilities::COLOR,
+      Capabilities::STEREO,       Capabilities::STEREO_COLOR,
-      Capabilities::STEREO_COLOR, Capabilities::DEPTH,
+      Capabilities::COLOR,        Capabilities::DEPTH,
       Capabilities::POINTS,       Capabilities::FISHEYE,
       Capabilities::INFRARED,     Capabilities::INFRARED2};
   for (auto &&capability : stream_capabilities) {
-    if (Supports(capability)) {
-      // do stream request selection if more than one request of each stream
-      auto &&stream_request = GetStreamRequest(capability);
-
-      streams_->ConfigStream(capability, stream_request);
-      uvc::set_device_mode(
-          *device_, stream_request.width, stream_request.height,
-          static_cast<int>(stream_request.format), stream_request.fps,
-          [this, capability](
-              const void *data, std::function<void()> continuation) {
-            // drop the first stereo stream data
-            static std::uint8_t drop_count = 1;
-            if (drop_count > 0) {
-              --drop_count;
-              continuation();
-              return;
-            }
-            // auto &&time_beg = times::now();
-            {
-              std::lock_guard<std::mutex> _(mtx_streams_);
-              if (streams_->PushStream(capability, data)) {
-                CallbackPushedStreamData(Stream::LEFT);
-                CallbackPushedStreamData(Stream::RIGHT);
-              }
-            }
-            continuation();
-            OnStereoStreamUpdate();
-            // VLOG(2) << "Stereo video callback cost "
-            //     << times::count<times::milliseconds>(times::now() - time_beg)
-            //     << " ms";
-          });
-    } else {
-      // LOG(FATAL) << "Not any stream capabilities are supported by this
-      // device";
-    }
   }
+  */
+  auto &&stream_cap = GetKeyStreamCapability();
+  if (Supports(stream_cap)) {
+    // do stream request selection if more than one request of each stream
+    auto &&stream_request = GetStreamRequest(stream_cap);
+    streams_->ConfigStream(stream_cap, stream_request);
+
+    uvc::set_device_mode(
+        *device_, stream_request.width, stream_request.height,
+        static_cast<int>(stream_request.format), stream_request.fps,
+        [this, stream_cap](
+            const void *data, std::function<void()> continuation) {
+          // drop the first stereo stream data
+          static std::uint8_t drop_count = 1;
+          if (drop_count > 0) {
+            --drop_count;
+            continuation();
+            return;
+          }
+          // auto &&time_beg = times::now();
+          {
+            std::lock_guard<std::mutex> _(mtx_streams_);
+            if (streams_->PushStream(stream_cap, data)) {
+              CallbackPushedStreamData(Stream::LEFT);
+              CallbackPushedStreamData(Stream::RIGHT);
+            }
+          }
+          continuation();
+          OnStereoStreamUpdate();
+          // VLOG(2) << "Stereo video callback cost "
+          //     << times::count<times::milliseconds>(times::now() - time_beg)
+          //     << " ms";
+        });
+  } else {
+    LOG(FATAL) << "Not any stream capabilities are supported by this device";
+  }
+
   uvc::start_streaming(*device_, 0);
   video_streaming_ = true;
 }

src/mynteye/device/standard/device_s.cc

@@ -11,15 +11,16 @@
 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 // See the License for the specific language governing permissions and
 // limitations under the License.
-#include "mynteye/device/device_s.h"
+#include "mynteye/device/standard/device_s.h"
 
 #include "mynteye/logger.h"
 #include "mynteye/device/motions.h"
+#include "mynteye/device/standard/streams_adapter_s.h"
 
 MYNTEYE_BEGIN_NAMESPACE
 
 StandardDevice::StandardDevice(std::shared_ptr<uvc::device> device)
-    : Device(Model::STANDARD2, device) {
+    : Device(Model::STANDARD, device) {
   VLOG(2) << __func__;
 }
 
@@ -28,11 +29,11 @@ StandardDevice::~StandardDevice() {
 }
 
 Capabilities StandardDevice::GetKeyStreamCapability() const {
-  return Capabilities::STEREO_COLOR;
+  return Capabilities::STEREO;
 }
 
-std::vector<Stream> StandardDevice::GetKeyStreams() const {
+std::shared_ptr<StreamsAdapter> StandardDevice::CreateStreamsAdapter() const {
-  return {Stream::LEFT, Stream::RIGHT};
+  return std::make_shared<StandardStreamsAdapter>();
 }
 
 void StandardDevice::OnStereoStreamUpdate() {

src/mynteye/device/standard/device_s.h

@@ -11,8 +11,8 @@
 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 // See the License for the specific language governing permissions and
 // limitations under the License.
-#ifndef MYNTEYE_DEVICE_DEVICE_S_H_
+#ifndef MYNTEYE_DEVICE_STANDARD_DEVICE_S_H_
-#define MYNTEYE_DEVICE_DEVICE_S_H_
+#define MYNTEYE_DEVICE_STANDARD_DEVICE_S_H_
 #pragma once
 
 #include <memory>
@@ -28,11 +28,11 @@ class StandardDevice : public Device {
   virtual ~StandardDevice();
 
   Capabilities GetKeyStreamCapability() const override;
-  std::vector<Stream> GetKeyStreams() const override;
+  std::shared_ptr<StreamsAdapter> CreateStreamsAdapter() const override;
 
   void OnStereoStreamUpdate() override;
 };
 
 MYNTEYE_END_NAMESPACE
 
-#endif  // MYNTEYE_DEVICE_DEVICE_S_H_
+#endif  // MYNTEYE_DEVICE_STANDARD_DEVICE_S_H_

src/mynteye/device/standard/streams_adapter_s.cc

@@ -0,0 +1,80 @@
+// Copyright 2018 Slightech Co., Ltd. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#include "mynteye/device/standard/streams_adapter_s.h"
+
+#include "mynteye/logger.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+namespace {
+
+bool unpack_left_img_pixels(
+    const void *data, const StreamRequest &request, Streams::frame_t *frame) {
+  CHECK_NOTNULL(frame);
+  CHECK_EQ(request.format, Format::YUYV);
+  CHECK_EQ(frame->format(), Format::GREY);
+  auto data_new = reinterpret_cast<const std::uint8_t *>(data);
+  std::size_t n = frame->width() * frame->height();
+  for (std::size_t i = 0; i < n; i++) {
+    frame->data()[i] = *(data_new + (i * 2));
+  }
+  return true;
+}
+
+bool unpack_right_img_pixels(
+    const void *data, const StreamRequest &request, Streams::frame_t *frame) {
+  CHECK_NOTNULL(frame);
+  CHECK_EQ(request.format, Format::YUYV);
+  CHECK_EQ(frame->format(), Format::GREY);
+  auto data_new = reinterpret_cast<const std::uint8_t *>(data);
+  std::size_t n = frame->width() * frame->height();
+  for (std::size_t i = 0; i < n; i++) {
+    frame->data()[i] = *(data_new + (i * 2 + 1));
+  }
+  return true;
+}
+
+}  // namespace
+
+StandardStreamsAdapter::StandardStreamsAdapter() {
+}
+
+StandardStreamsAdapter::~StandardStreamsAdapter() {
+}
+
+std::vector<Stream> StandardStreamsAdapter::GetKeyStreams() {
+  return {Stream::LEFT, Stream::RIGHT};
+}
+
+std::vector<Capabilities> StandardStreamsAdapter::GetStreamCapabilities() {
+  return {Capabilities::STEREO};
+}
+
+std::map<Stream, Streams::unpack_img_data_t>
+StandardStreamsAdapter::GetUnpackImgDataMap() {
+  return {
+    {Stream::LEFT, unpack_stereo_img_data},
+    {Stream::RIGHT, unpack_stereo_img_data}
+  };
+}
+
+std::map<Stream, Streams::unpack_img_pixels_t>
+StandardStreamsAdapter::GetUnpackImgPixelsMap() {
+  return {
+    {Stream::LEFT, unpack_left_img_pixels},
+    {Stream::RIGHT, unpack_right_img_pixels}
+  };
+}
+
+MYNTEYE_END_NAMESPACE

src/mynteye/device/standard/streams_adapter_s.h

@@ -0,0 +1,42 @@
+// Copyright 2018 Slightech Co., Ltd. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#ifndef MYNTEYE_DEVICE_STANDARD_STREAMS_ADAPTER_S_H_
+#define MYNTEYE_DEVICE_STANDARD_STREAMS_ADAPTER_S_H_
+#pragma once
+
+#include <map>
+#include <memory>
+#include <vector>
+
+#include "mynteye/device/streams.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+class StandardStreamsAdapter : public StreamsAdapter {
+ public:
+  StandardStreamsAdapter();
+  virtual ~StandardStreamsAdapter();
+
+  std::vector<Stream> GetKeyStreams() override;
+  std::vector<Capabilities> GetStreamCapabilities() override;
+
+  std::map<Stream, Streams::unpack_img_data_t>
+  GetUnpackImgDataMap() override;
+  std::map<Stream, Streams::unpack_img_pixels_t>
+  GetUnpackImgPixelsMap() override;
+};
+
+MYNTEYE_END_NAMESPACE
+
+#endif  // MYNTEYE_DEVICE_STANDARD_STREAMS_ADAPTER_S_H_

src/mynteye/device/standard2/device_s2.cc

@@ -0,0 +1,46 @@
+// Copyright 2018 Slightech Co., Ltd. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#include "mynteye/device/standard2/device_s2.h"
+
+#include "mynteye/logger.h"
+#include "mynteye/device/motions.h"
+#include "mynteye/device/standard2/streams_adapter_s2.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+Standard2Device::Standard2Device(std::shared_ptr<uvc::device> device)
+    : Device(Model::STANDARD2, device) {
+  VLOG(2) << __func__;
+}
+
+Standard2Device::~Standard2Device() {
+  VLOG(2) << __func__;
+}
+
+Capabilities Standard2Device::GetKeyStreamCapability() const {
+  return Capabilities::STEREO_COLOR;
+}
+
+std::shared_ptr<StreamsAdapter> Standard2Device::CreateStreamsAdapter() const {
+  return std::make_shared<Standard2StreamsAdapter>();
+}
+
+void Standard2Device::OnStereoStreamUpdate() {
+  if (motion_tracking_) {
+    auto &&motions = this->motions();
+    motions->DoMotionTrack();
+  }
+}
+
+MYNTEYE_END_NAMESPACE

src/mynteye/device/standard2/device_s2.h

@@ -0,0 +1,38 @@
+// Copyright 2018 Slightech Co., Ltd. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#ifndef MYNTEYE_DEVICE_STANDARD2_DEVICE_S2_H_
+#define MYNTEYE_DEVICE_STANDARD2_DEVICE_S2_H_
+#pragma once
+
+#include <memory>
+#include <vector>
+
+#include "mynteye/device/device.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+class Standard2Device : public Device {
+ public:
+  explicit Standard2Device(std::shared_ptr<uvc::device> device);
+  virtual ~Standard2Device();
+
+  Capabilities GetKeyStreamCapability() const override;
+  std::shared_ptr<StreamsAdapter> CreateStreamsAdapter() const override;
+
+  void OnStereoStreamUpdate() override;
+};
+
+MYNTEYE_END_NAMESPACE
+
+#endif  // MYNTEYE_DEVICE_STANDARD2_DEVICE_S2_H_

src/mynteye/device/standard2/streams_adapter_s2.cc

@@ -0,0 +1,98 @@
+// Copyright 2018 Slightech Co., Ltd. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#include "mynteye/device/standard2/streams_adapter_s2.h"
+
+#include "mynteye/logger.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+namespace {
+
+bool unpack_left_img_pixels(
+    const void *data, const StreamRequest &request, Streams::frame_t *frame) {
+  CHECK_NOTNULL(frame);
+  CHECK_EQ(request.format, Format::BGR888);
+  CHECK_EQ(frame->format(), Format::BGR888);
+  auto data_new = reinterpret_cast<const std::uint8_t *>(data);
+  std::size_t n = 3;
+  std::size_t w = frame->width();
+  std::size_t h = frame->height();
+  for (std::size_t i = 0; i < h; i++) {
+    for (std::size_t j = 0; j < w; j++) {
+      frame->data()[(i * w + j) * n] =
+        *(data_new + (2 * i * w + j) * n + 2);
+      frame->data()[(i * w + j) * n + 1] =
+        *(data_new + (2 * i * w + j) * n + 1);
+      frame->data()[(i * w + j) * n + 2] =
+        *(data_new + (2 * i * w + j) * n);
+    }
+  }
+  return true;
+}
+
+bool unpack_right_img_pixels(
+    const void *data, const StreamRequest &request, Streams::frame_t *frame) {
+  CHECK_NOTNULL(frame);
+  CHECK_EQ(request.format, Format::BGR888);
+  CHECK_EQ(frame->format(), Format::BGR888);
+  auto data_new = reinterpret_cast<const std::uint8_t *>(data);
+  std::size_t n = 3;
+  std::size_t w = frame->width();
+  std::size_t h = frame->height();
+  for (std::size_t i = 0; i < h; i++) {
+    for (std::size_t j = 0; j < w; j++) {
+      frame->data()[(i * w + j) * n] =
+        *(data_new + ((2 * i + 1) * w + j) * n + 2);
+      frame->data()[(i * w + j) * n + 1] =
+        *(data_new + ((2 * i + 1) * w + j) * n + 1);
+      frame->data()[(i * w + j) * n + 2] =
+        *(data_new + ((2 * i + 1) * w + j) * n);
+    }
+  }
+  return true;
+}
+
+}  // namespace
+
+Standard2StreamsAdapter::Standard2StreamsAdapter() {
+}
+
+Standard2StreamsAdapter::~Standard2StreamsAdapter() {
+}
+
+std::vector<Stream> Standard2StreamsAdapter::GetKeyStreams() {
+  return {Stream::LEFT, Stream::RIGHT};
+}
+
+std::vector<Capabilities> Standard2StreamsAdapter::GetStreamCapabilities() {
+  return {Capabilities::STEREO_COLOR};
+}
+
+std::map<Stream, Streams::unpack_img_data_t>
+Standard2StreamsAdapter::GetUnpackImgDataMap() {
+  return {
+    {Stream::LEFT, unpack_stereo_img_data},
+    {Stream::RIGHT, unpack_stereo_img_data}
+  };
+}
+
+std::map<Stream, Streams::unpack_img_pixels_t>
+Standard2StreamsAdapter::GetUnpackImgPixelsMap() {
+  return {
+    {Stream::LEFT, unpack_left_img_pixels},
+    {Stream::RIGHT, unpack_right_img_pixels}
+  };
+}
+
+MYNTEYE_END_NAMESPACE

src/mynteye/device/standard2/streams_adapter_s2.h

@@ -0,0 +1,42 @@
+// Copyright 2018 Slightech Co., Ltd. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#ifndef MYNTEYE_DEVICE_STANDARD2_STREAMS_ADAPTER_S2_H_
+#define MYNTEYE_DEVICE_STANDARD2_STREAMS_ADAPTER_S2_H_
+#pragma once
+
+#include <map>
+#include <memory>
+#include <vector>
+
+#include "mynteye/device/streams.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+class Standard2StreamsAdapter : public StreamsAdapter {
+ public:
+  Standard2StreamsAdapter();
+  virtual ~Standard2StreamsAdapter();
+
+  std::vector<Stream> GetKeyStreams() override;
+  std::vector<Capabilities> GetStreamCapabilities() override;
+
+  std::map<Stream, Streams::unpack_img_data_t>
+  GetUnpackImgDataMap() override;
+  std::map<Stream, Streams::unpack_img_pixels_t>
+  GetUnpackImgPixelsMap() override;
+};
+
+MYNTEYE_END_NAMESPACE
+
+#endif  // MYNTEYE_DEVICE_STANDARD2_STREAMS_ADAPTER_S2_H_

src/mynteye/device/streams.cc

@@ -23,11 +23,10 @@
 
 MYNTEYE_BEGIN_NAMESPACE
 
-namespace {
-
 bool unpack_stereo_img_data(
     const void *data, const StreamRequest &request, ImgData *img) {
   CHECK_NOTNULL(img);
+
   auto data_new = reinterpret_cast<const std::uint8_t *>(data);
   std::size_t data_n =
       request.width * request.height * bytes_per_pixel(request.format);
@@ -73,101 +72,11 @@ bool unpack_stereo_img_data(
   return true;
 }
 
-bool unpack_left_img_pixels(
+Streams::Streams(const std::shared_ptr<StreamsAdapter> &adapter)
-    const void *data, const StreamRequest &request, Streams::frame_t *frame) {
+    : key_streams_(std::move(adapter->GetKeyStreams())),
-  CHECK_NOTNULL(frame);
+      stream_capabilities_(std::move(adapter->GetStreamCapabilities())),
-  CHECK_EQ(request.format, frame->format());
+      unpack_img_data_map_(std::move(adapter->GetUnpackImgDataMap())),
-  auto data_new = reinterpret_cast<const std::uint8_t *>(data);
+      unpack_img_pixels_map_(std::move(adapter->GetUnpackImgPixelsMap())) {
-  if (request.format == Format::YUYV) {
-    std::size_t n = 2;
-    std::size_t w = frame->width() * n;
-    std::size_t h = frame->height();
-    for (std::size_t i = 0; i < h; i++) {
-      for (std::size_t j = 0; j < w; j++) {
-        frame->data()[i * w + j] = *(data_new + 2 * i * w + j);
-      }
-    }
-  } else if (request.format == Format::BGR888) {
-    std::size_t n = 3;
-    std::size_t w = frame->width();
-    std::size_t h = frame->height();
-    for (std::size_t i = 0; i < h; i++) {
-      for (std::size_t j = 0; j < w; j++) {
-        frame->data()[(i * w + j) * n] =
-          *(data_new + (2 * i * w + j) * n + 2);
-        frame->data()[(i * w + j) * n + 1] =
-          *(data_new + (2 * i * w + j) * n + 1);
-        frame->data()[(i * w + j) * n + 2] =
-          *(data_new + (2 * i * w + j) * n);
-      }
-    }
-  } else if (request.format == Format::GREY) {
-    std::size_t n = frame->width() * frame->height();
-    for (std::size_t i = 0; i < n; i++) {
-      frame->data()[i] = *(data_new + (i * 2));
-    }
-  } else {
-    return false;
-  }
-
-  return true;
-}
-
-// TODO(Kalman): Too similar to 'unpack_left_img_pixels'
-bool unpack_right_img_pixels(
-    const void *data, const StreamRequest &request, Streams::frame_t *frame) {
-  CHECK_NOTNULL(frame);
-  CHECK_EQ(request.format, frame->format());
-  auto data_new = reinterpret_cast<const std::uint8_t *>(data);
-  if (request.format == Format::YUYV) {
-    std::size_t n = 2;
-    std::size_t w = frame->width() * n;
-    std::size_t h = frame->height();
-    for (std::size_t i = 0; i < h; i++) {
-      for (std::size_t j = 0; j < w; j++) {
-        frame->data()[i * w + j] = *(data_new + (2 * i + 1) * w + j);
-      }
-    }
-  } else if (request.format == Format::BGR888) {
-    std::size_t n = 3;
-    std::size_t w = frame->width();
-    std::size_t h = frame->height();
-    for (std::size_t i = 0; i < h; i++) {
-      for (std::size_t j = 0; j < w; j++) {
-        frame->data()[(i * w + j) * n] =
-          *(data_new + ((2 * i + 1) * w + j) * n + 2);
-        frame->data()[(i * w + j) * n + 1] =
-          *(data_new + ((2 * i + 1) * w + j) * n + 1);
-        frame->data()[(i * w + j) * n + 2] =
-          *(data_new + ((2 * i + 1) * w + j) * n);
-      }
-    }
-  } else if (request.format == Format::GREY) {
-    std::size_t n = frame->width() * frame->height();
-    for (std::size_t i = 0; i < n; i++) {
-      frame->data()[i] = *(data_new + (i * 2 + 1));
-    }
-  } else {
-    return false;
-  }
-
-  return true;
-}
-
-}  // namespace
-
-Streams::Streams(const std::vector<Stream> key_streams)
-    : key_streams_(key_streams),
-      stream_capabilities_(
-          {Capabilities::STEREO, Capabilities::COLOR, Capabilities::DEPTH,
-           Capabilities::POINTS, Capabilities::FISHEYE, Capabilities::INFRARED,
-           Capabilities::INFRARED2, Capabilities::STEREO_COLOR}),
-      unpack_img_data_map_(
-          {{Stream::LEFT, unpack_stereo_img_data},
-           {Stream::RIGHT, unpack_stereo_img_data}}),
-      unpack_img_pixels_map_(
-          {{Stream::LEFT, unpack_left_img_pixels},
-           {Stream::RIGHT, unpack_right_img_pixels}}) {
   VLOG(2) << __func__;
 }
 
@@ -193,16 +102,17 @@ bool Streams::PushStream(const Capabilities &capability, const void *data) {
   auto &&request = GetStreamConfigRequest(capability);
   bool pushed = false;
   switch (capability) {
+    case Capabilities::STEREO:
     case Capabilities::STEREO_COLOR: {
       // alloc left
-      AllocStreamData(Stream::LEFT, request);
+      AllocStreamData(capability, Stream::LEFT, request);
       auto &&left_data = stream_datas_map_[Stream::LEFT].back();
       // unpack img data
       if (unpack_img_data_map_[Stream::LEFT](
               data, request, left_data.img.get())) {
         left_data.frame_id = left_data.img->frame_id;
         // alloc right
-        AllocStreamData(Stream::RIGHT, request);
+        AllocStreamData(capability, Stream::RIGHT, request);
         auto &&right_data = stream_datas_map_[Stream::RIGHT].back();
         *right_data.img = *left_data.img;
         right_data.frame_id = left_data.img->frame_id;
@@ -306,12 +216,16 @@ bool Streams::HasStreamDatas(const Stream &stream) const {
          !stream_datas_map_.at(stream).empty();
 }
 
-void Streams::AllocStreamData(
+void Streams::AllocStreamData(const Capabilities &capability,
     const Stream &stream, const StreamRequest &request) {
-  AllocStreamData(stream, request, request.format);
+  auto format = request.format;
+  if (capability == Capabilities::STEREO) {
+    format = Format::GREY;
+  }
+  AllocStreamData(capability, stream, request, format);
 }
 
-void Streams::AllocStreamData(
+void Streams::AllocStreamData(const Capabilities &capability,
     const Stream &stream, const StreamRequest &request, const Format &format) {
   stream_data_t data;
 
@@ -336,11 +250,12 @@ void Streams::AllocStreamData(
     data.img = nullptr;
   }
   if (!data.frame) {
-    int width = request.width;
+    auto width = request.width;
-    if (format != Format::GREY)
+    if (capability == Capabilities::STEREO_COLOR) {
-      width /= 2;
+      width /= 2;  // split to half
+    }
     data.frame =
-        std::make_shared<frame_t>(width, request.height, format, nullptr);
+      std::make_shared<frame_t>(width, request.height, format, nullptr);
   }
   data.frame_id = 0;
   stream_datas_map_[stream].push_back(data);

src/mynteye/device/streams.h

@@ -18,6 +18,7 @@
 #include <condition_variable>
 #include <functional>
 #include <map>
+#include <memory>
 #include <mutex>
 #include <vector>
 
@@ -27,6 +28,11 @@
 
 MYNTEYE_BEGIN_NAMESPACE
 
+extern bool unpack_stereo_img_data(
+    const void *data, const StreamRequest &request, ImgData *img);
+
+class StreamsAdapter;
+
 class Streams {
  public:
   using frame_t = device::Frame;
@@ -38,7 +44,7 @@ class Streams {
   using unpack_img_pixels_t = std::function<bool(
       const void *data, const StreamRequest &request, frame_t *frame)>;
 
-  explicit Streams(const std::vector<Stream> key_streams);
+  explicit Streams(const std::shared_ptr<StreamsAdapter> &adapter);
   ~Streams();
 
   void ConfigStream(
@@ -65,8 +71,9 @@ class Streams {
 
   bool HasStreamDatas(const Stream &stream) const;
 
-  void AllocStreamData(const Stream &stream, const StreamRequest &request);
+  void AllocStreamData(const Capabilities &capability,
-  void AllocStreamData(
+      const Stream &stream, const StreamRequest &request);
+  void AllocStreamData(const Capabilities &capability,
       const Stream &stream, const StreamRequest &request, const Format &format);
 
   void DiscardStreamData(const Stream &stream);
@@ -88,6 +95,19 @@ class Streams {
   std::condition_variable cv_;
 };
 
+class StreamsAdapter {
+ public:
+  virtual ~StreamsAdapter() {}
+
+  virtual std::vector<Stream> GetKeyStreams() = 0;
+  virtual std::vector<Capabilities> GetStreamCapabilities() = 0;
+
+  virtual std::map<Stream, Streams::unpack_img_data_t>
+  GetUnpackImgDataMap() = 0;
+  virtual std::map<Stream, Streams::unpack_img_pixels_t>
+  GetUnpackImgPixelsMap() = 0;
+};
+
 MYNTEYE_END_NAMESPACE
 
 #endif  // MYNTEYE_DEVICE_STREAMS_H_