Rearrage include and src

src/mynteye/device/async_callback.h

@@ -0,0 +1,62 @@
+// 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_ASYNC_CALLBACK_H_
+#define MYNTEYE_DEVICE_ASYNC_CALLBACK_H_
+#pragma once
+
+#include <condition_variable>
+#include <functional>
+#include <mutex>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include "mynteye/mynteye.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+template <class Data>
+class AsyncCallback {
+ public:
+  using callback_t = std::function<void(Data data)>;
+
+  AsyncCallback(
+      std::string name, callback_t callback, std::size_t max_data_size = 0);
+  ~AsyncCallback();
+
+  void PushData(Data data);
+
+ private:
+  void Run();
+
+  std::string name_;
+
+  callback_t callback_;
+
+  std::mutex mtx_;
+  std::condition_variable cv_;
+
+  bool running_;
+  std::thread thread_;
+
+  std::uint32_t count_;
+  std::vector<Data> datas_;
+  std::size_t max_data_size_;
+};
+
+MYNTEYE_END_NAMESPACE
+
+#include "mynteye/device/async_callback_impl.h"
+
+#endif  // MYNTEYE_DEVICE_ASYNC_CALLBACK_H_

src/mynteye/device/async_callback_impl.h

@@ -0,0 +1,92 @@
+// 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_ASYNC_CALLBACK_IMPL_H_
+#define MYNTEYE_DEVICE_ASYNC_CALLBACK_IMPL_H_
+#pragma once
+
+#include <string>
+#include <utility>
+
+#include "mynteye/logger.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+template <class Data>
+AsyncCallback<Data>::AsyncCallback(
+    std::string name, callback_t callback, std::size_t max_data_size)
+    : name_(std::move(name)),
+      callback_(std::move(callback)),
+      count_(0),
+      max_data_size_(max_data_size) {
+  VLOG(2) << __func__;
+  running_ = true;
+  thread_ = std::thread(&AsyncCallback<Data>::Run, this);
+}
+
+template <class Data>
+AsyncCallback<Data>::~AsyncCallback() {
+  VLOG(2) << __func__;
+  {
+    std::lock_guard<std::mutex> _(mtx_);
+    running_ = false;
+    ++count_;
+  }
+  cv_.notify_one();
+  if (thread_.joinable()) {
+    thread_.join();
+  }
+}
+
+template <class Data>
+void AsyncCallback<Data>::PushData(Data data) {
+  std::lock_guard<std::mutex> _(mtx_);
+  if (max_data_size_ <= 0) {
+    datas_.clear();
+  } else if (max_data_size_ == datas_.size()) {  // >= 1
+    datas_.erase(datas_.begin());
+  }
+  datas_.push_back(data);
+  ++count_;
+  cv_.notify_one();
+}
+
+template <class Data>
+void AsyncCallback<Data>::Run() {
+  VLOG(2) << "AsyncCallback(" << name_ << ") thread start";
+  while (true) {
+    std::unique_lock<std::mutex> lock(mtx_);
+    cv_.wait(lock, [this] { return count_ > 0; });
+
+    if (!running_)
+      break;
+
+    if (callback_) {
+      for (auto &&data : datas_) {
+        callback_(data);
+      }
+    }
+
+    if (VLOG_IS_ON(2) && count_ > datas_.size()) {
+      VLOG(2) << "AsyncCallback(" << name_ << ") dropped "
+              << (count_ - datas_.size());
+    }
+    count_ = 0;
+    datas_.clear();
+  }
+  VLOG(2) << "AsyncCallback(" << name_ << ") thread end";
+}
+
+MYNTEYE_END_NAMESPACE
+
+#endif  // MYNTEYE_DEVICE_ASYNC_CALLBACK_IMPL_H_

src/mynteye/device/channels.h

@@ -0,0 +1,156 @@
+// 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_CHANNELS_H_
+#define MYNTEYE_DEVICE_CHANNELS_H_
+#pragma once
+
+#include <map>
+#include <memory>
+#include <thread>
+
+#include "mynteye/mynteye.h"
+#include "mynteye/types.h"
+#include "mynteye/device/types.h"
+#include "mynteye/uvc/uvc.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+namespace uvc {
+
+struct device;
+struct xu;
+
+}  // namespace uvc
+
+class MYNTEYE_API Channels {
+ public:
+  typedef enum Channel {
+    CHANNEL_CAM_CTRL = 1,
+    CHANNEL_HALF_DUPLEX = 2,
+    CHANNEL_IMU_WRITE = 3,
+    CHANNEL_IMU_READ = 4,
+    CHANNEL_FILE = 5,
+    CHANNEL_LAST
+  } channel_t;
+
+  typedef struct ControlInfo {
+    std::int32_t min;
+    std::int32_t max;
+    std::int32_t def;
+  } control_info_t;
+
+  typedef enum XuCmd {
+    XU_CMD_ZDC = 0xE6,    // zero drift calibration
+    XU_CMD_ERASE = 0xDE,  // erase chip
+    XU_CMD_LAST
+  } xu_cmd_t;
+
+  typedef enum FileId {
+    FID_DEVICE_INFO = 1,  // device info
+    FID_IMG_PARAMS = 2,   // image intrinsics & extrinsics
+    FID_IMU_PARAMS = 4,   // imu intrinsics & extrinsics
+    FID_LAST,
+  } file_id_t;
+
+  using imu_callback_t = std::function<void(const ImuPacket &packet)>;
+
+  using device_info_t = DeviceInfo;
+
+  typedef struct ImgParams {
+    bool ok;
+    Intrinsics in_left;
+    Intrinsics in_right;
+    Extrinsics ex_right_to_left;
+  } img_params_t;
+
+  typedef struct ImuParams {
+    bool ok;
+    ImuIntrinsics in_accel;
+    ImuIntrinsics in_gyro;
+    Extrinsics ex_left_to_imu;
+  } imu_params_t;
+
+  explicit Channels(std::shared_ptr<uvc::device> device);
+  ~Channels();
+
+  void LogControlInfos() const;
+  void UpdateControlInfos();
+  control_info_t GetControlInfo(const Option &option) const;
+
+  std::int32_t GetControlValue(const Option &option) const;
+  void SetControlValue(const Option &option, std::int32_t value);
+
+  bool RunControlAction(const Option &option) const;
+
+  void SetImuCallback(imu_callback_t callback);
+  void DoImuTrack();
+
+  void StartImuTracking(imu_callback_t callback = nullptr);
+  void StopImuTracking();
+
+  bool GetFiles(
+      device_info_t *info, img_params_t *img_params, imu_params_t *imu_params,
+      Version *spec_version = nullptr) const;
+  bool SetFiles(
+      device_info_t *info, img_params_t *img_params, imu_params_t *imu_params,
+      Version *spec_version = nullptr);
+
+ private:
+  bool PuControlRange(
+      Option option, int32_t *min, int32_t *max, int32_t *def) const;
+  bool PuControlQuery(Option option, uvc::pu_query query, int32_t *value) const;
+
+  bool XuControlRange(
+      channel_t channel, uint8_t id, int32_t *min, int32_t *max,
+      int32_t *def) const;
+  bool XuControlRange(
+      const uvc::xu &xu, uint8_t selector, uint8_t id, int32_t *min,
+      int32_t *max, int32_t *def) const;
+
+  bool XuControlQuery(
+      channel_t channel, uvc::xu_query query, uint16_t size,
+      uint8_t *data) const;
+  bool XuControlQuery(
+      const uvc::xu &xu, uint8_t selector, uvc::xu_query query, uint16_t size,
+      uint8_t *data) const;
+
+  bool XuCamCtrlQuery(uvc::xu_query query, uint16_t size, uint8_t *data) const;
+  std::int32_t XuCamCtrlGet(Option option) const;
+  void XuCamCtrlSet(Option option, std::int32_t value) const;
+
+  bool XuHalfDuplexSet(Option option, xu_cmd_t cmd) const;
+
+  bool XuImuWrite(const ImuReqPacket &req) const;
+  bool XuImuRead(ImuResPacket *res) const;
+
+  bool XuFileQuery(uvc::xu_query query, uint16_t size, uint8_t *data) const;
+
+  control_info_t PuControlInfo(Option option) const;
+  control_info_t XuControlInfo(Option option) const;
+
+  std::shared_ptr<uvc::device> device_;
+
+  std::map<Option, control_info_t> control_infos_;
+
+  bool is_imu_tracking_;
+  std::thread imu_track_thread_;
+  volatile bool imu_track_stop_;
+
+  std::uint32_t imu_sn_;
+  imu_callback_t imu_callback_;
+};
+
+MYNTEYE_END_NAMESPACE
+
+#endif  // MYNTEYE_DEVICE_CHANNELS_H_

src/mynteye/device/config.cc

@@ -0,0 +1,36 @@
+// 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/config.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+const std::map<Model, StreamSupports> stream_supports_map = {
+    {Model::STANDARD, {Stream::LEFT, Stream::RIGHT}}};
+
+const std::map<Model, CapabilitiesSupports> capabilities_supports_map = {
+    {Model::STANDARD, {Capabilities::STEREO, Capabilities::IMU}}};
+
+const std::map<Model, OptionSupports> option_supports_map = {
+    {Model::STANDARD,
+     {Option::GAIN, Option::BRIGHTNESS, Option::CONTRAST, Option::FRAME_RATE,
+      Option::IMU_FREQUENCY, Option::EXPOSURE_MODE, Option::MAX_GAIN,
+      Option::MAX_EXPOSURE_TIME, Option::DESIRED_BRIGHTNESS, Option::IR_CONTROL,
+      Option::HDR_MODE, Option::ZERO_DRIFT_CALIBRATION, Option::ERASE_CHIP}}};
+
+const std::map<Model, std::map<Capabilities, StreamRequests>>
+    stream_requests_map = {
+        {Model::STANDARD,
+         {{Capabilities::STEREO, {{752, 480, Format::YUYV, 25}}}}}};
+
+MYNTEYE_END_NAMESPACE

src/mynteye/device/config.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_CONFIG_H_
+#define MYNTEYE_DEVICE_CONFIG_H_
+#pragma once
+
+#include <map>
+#include <set>
+#include <vector>
+
+#include "mynteye/mynteye.h"
+#include "mynteye/types.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+using StreamSupports = std::set<Stream>;
+using CapabilitiesSupports = std::set<Capabilities>;
+using OptionSupports = std::set<Option>;
+
+extern const std::map<Model, StreamSupports> stream_supports_map;
+extern const std::map<Model, CapabilitiesSupports> capabilities_supports_map;
+extern const std::map<Model, OptionSupports> option_supports_map;
+
+using StreamRequests = std::vector<StreamRequest>;
+
+extern const std::map<Model, std::map<Capabilities, StreamRequests>>
+    stream_requests_map;
+
+MYNTEYE_END_NAMESPACE
+
+#endif  // MYNTEYE_DEVICE_CONFIG_H_

src/mynteye/device/context.cc

@@ -0,0 +1,48 @@
+// 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/context.h"
+
+#include "mynteye/logger.h"
+
+#include "mynteye/device/device.h"
+#include "mynteye/uvc/uvc.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+Context::Context() : context_(uvc::create_context()) {
+  VLOG(2) << __func__;
+
+  for (auto &&device : uvc::query_devices(context_)) {
+    auto name = uvc::get_name(*device);
+    auto vid = uvc::get_vendor_id(*device);
+    auto pid = uvc::get_product_id(*device);
+    // auto video_name = uvc::get_video_name(*device);
+    VLOG(2) << "UVC device detected, name: " << name << ", vid: 0x" << std::hex
+            << vid << ", pid: 0x" << std::hex << pid;
+    if (vid == MYNTEYE_VID) {
+      auto d = Device::Create(name, device);
+      if (d) {
+        devices_.push_back(d);
+      } else {
+        LOG(ERROR) << "Device is not supported by MYNT EYE.";
+      }
+    }
+  }
+}
+
+Context::~Context() {
+  VLOG(2) << __func__;
+}
+
+MYNTEYE_END_NAMESPACE

src/mynteye/device/device.cc

@@ -0,0 +1,609 @@
+// 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/device.h"
+
+#include <algorithm>
+#include <iterator>
+#include <stdexcept>
+#include <utility>
+
+#include "mynteye/logger.h"
+#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/streams.h"
+#include "mynteye/device/types.h"
+#include "mynteye/util/strings.h"
+#include "mynteye/util/times.h"
+#include "mynteye/uvc/uvc.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+namespace {
+
+struct DeviceModel {
+  char type;
+  std::uint8_t generation;
+  std::uint8_t baseline_code;
+  std::uint8_t hardware_code;
+  std::uint8_t custom_code;
+  bool ir_fixed;
+
+  DeviceModel() = default;
+  explicit DeviceModel(std::string model) {
+    CHECK_GE(model.size(), 5);
+    type = model[0];
+    generation = model[1];
+    baseline_code = model[2];
+    hardware_code = model[3];
+    custom_code = model[4];
+    ir_fixed = (model.size() == 8) && model.substr(5) == "-IR";
+  }
+};
+
+bool CheckSupports(
+    const Device *const device, const Stream &stream, bool fatal = true) {
+  if (device->Supports(stream)) {
+    return true;
+  } else {
+    auto &&supports = stream_supports_map.at(device->GetModel());
+    std::ostringstream ss;
+    std::copy(
+        supports.begin(), supports.end(),
+        std::ostream_iterator<Stream>(ss, ", "));
+    if (fatal) {
+      LOG(FATAL) << "Unsupported stream: " << stream
+                 << ". Please use these: " << ss.str();
+    } else {
+      LOG(WARNING) << "Unsupported stream: " << stream
+                   << ". Please use these: " << ss.str();
+    }
+    return false;
+  }
+}
+
+}  // namespace
+
+Device::Device(const Model &model, std::shared_ptr<uvc::device> device)
+    : video_streaming_(false),
+      motion_tracking_(false),
+      model_(model),
+      device_(device),
+      streams_(nullptr),
+      channels_(std::make_shared<Channels>(device)),
+      motions_(std::make_shared<Motions>(channels_)) {
+  VLOG(2) << __func__;
+  ReadAllInfos();
+}
+
+Device::~Device() {
+  VLOG(2) << __func__;
+}
+
+std::shared_ptr<Device> Device::Create(
+    const std::string &name, std::shared_ptr<uvc::device> device) {
+  if (name == "MYNTEYE") {
+    return std::make_shared<StandardDevice>(device);
+  } else if (strings::starts_with(name, "MYNT-EYE-")) {
+    // TODO(JohnZhao): Create different device by name, such as MYNT-EYE-S1000
+    std::string model_s = name.substr(9);
+    VLOG(2) << "MYNE EYE Model: " << model_s;
+    DeviceModel model(model_s);
+    switch (model.type) {
+      case 'S':
+        return std::make_shared<StandardDevice>(device);
+      default:
+        LOG(FATAL) << "MYNT EYE model is not supported now";
+    }
+  }
+  return nullptr;
+}
+
+bool Device::Supports(const Stream &stream) const {
+  auto &&supports = stream_supports_map.at(model_);
+  return supports.find(stream) != supports.end();
+}
+
+bool Device::Supports(const Capabilities &capability) const {
+  auto &&supports = capabilities_supports_map.at(model_);
+  return supports.find(capability) != supports.end();
+}
+
+bool Device::Supports(const Option &option) const {
+  auto &&supports = option_supports_map.at(model_);
+  return supports.find(option) != supports.end();
+}
+
+bool Device::Supports(const AddOns &addon) const {
+  CHECK_NOTNULL(device_info_);
+  auto &&hw_flag = device_info_->hardware_version.flag();
+  switch (addon) {
+    case AddOns::INFRARED:
+      return hw_flag[0];
+    case AddOns::INFRARED2:
+      return hw_flag[1];
+    default:
+      LOG(WARNING) << "Unknown add-on";
+      return false;
+  }
+}
+
+const std::vector<StreamRequest> &Device::GetStreamRequests(
+    const Capabilities &capability) const {
+  if (!Supports(capability)) {
+    LOG(FATAL) << "Unsupported capability: " << capability;
+  }
+  try {
+    auto &&cap_requests = stream_requests_map.at(model_);
+    return cap_requests.at(capability);
+  } catch (const std::out_of_range &e) {
+    LOG(FATAL) << "Stream request of " << capability << " of " << model_
+               << " not found";
+  }
+}
+
+void Device::ConfigStreamRequest(
+    const Capabilities &capability, const StreamRequest &request) {
+  auto &&requests = GetStreamRequests(capability);
+  if (std::find(requests.cbegin(), requests.cend(), request) ==
+      requests.cend()) {
+    LOG(WARNING) << "Config stream request of " << capability
+                 << " is not accpected";
+    return;
+  }
+  stream_config_requests_[capability] = request;
+}
+
+std::shared_ptr<DeviceInfo> Device::GetInfo() const {
+  return device_info_;
+}
+
+std::string Device::GetInfo(const Info &info) const {
+  CHECK_NOTNULL(device_info_);
+  switch (info) {
+    case Info::DEVICE_NAME:
+      return device_info_->name;
+    case Info::SERIAL_NUMBER:
+      return device_info_->serial_number;
+    case Info::FIRMWARE_VERSION:
+      return device_info_->firmware_version.to_string();
+    case Info::HARDWARE_VERSION:
+      return device_info_->hardware_version.to_string();
+    case Info::SPEC_VERSION:
+      return device_info_->spec_version.to_string();
+    case Info::LENS_TYPE:
+      return device_info_->lens_type.to_string();
+    case Info::IMU_TYPE:
+      return device_info_->imu_type.to_string();
+    case Info::NOMINAL_BASELINE:
+      return std::to_string(device_info_->nominal_baseline);
+    default:
+      LOG(WARNING) << "Unknown device info";
+      return "";
+  }
+}
+
+Intrinsics Device::GetIntrinsics(const Stream &stream) const {
+  bool ok;
+  return GetIntrinsics(stream, &ok);
+}
+
+Extrinsics Device::GetExtrinsics(const Stream &from, const Stream &to) const {
+  bool ok;
+  return GetExtrinsics(from, to, &ok);
+}
+
+MotionIntrinsics Device::GetMotionIntrinsics() const {
+  bool ok;
+  return GetMotionIntrinsics(&ok);
+}
+
+Extrinsics Device::GetMotionExtrinsics(const Stream &from) const {
+  bool ok;
+  return GetMotionExtrinsics(from, &ok);
+}
+
+Intrinsics Device::GetIntrinsics(const Stream &stream, bool *ok) const {
+  try {
+    *ok = true;
+    return stream_intrinsics_.at(stream);
+  } catch (const std::out_of_range &e) {
+    *ok = false;
+    LOG(WARNING) << "Intrinsics of " << stream << " not found";
+    return {};
+  }
+}
+
+Extrinsics Device::GetExtrinsics(
+    const Stream &from, const Stream &to, bool *ok) const {
+  try {
+    *ok = true;
+    return stream_from_extrinsics_.at(from).at(to);
+  } catch (const std::out_of_range &e) {
+    try {
+      *ok = true;
+      return stream_from_extrinsics_.at(to).at(from).Inverse();
+    } catch (const std::out_of_range &e) {
+      *ok = false;
+      LOG(WARNING) << "Extrinsics from " << from << " to " << to
+                   << " not found";
+      return {};
+    }
+  }
+}
+
+MotionIntrinsics Device::GetMotionIntrinsics(bool *ok) const {
+  if (motion_intrinsics_) {
+    *ok = true;
+    return *motion_intrinsics_;
+  } else {
+    *ok = false;
+    VLOG(2) << "Motion intrinsics not found";
+    return {};
+  }
+}
+
+Extrinsics Device::GetMotionExtrinsics(const Stream &from, bool *ok) const {
+  try {
+    *ok = true;
+    return motion_from_extrinsics_.at(from);
+  } catch (const std::out_of_range &e) {
+    *ok = false;
+    VLOG(2) << "Motion extrinsics from " << from << " not found";
+    return {};
+  }
+}
+
+void Device::SetIntrinsics(const Stream &stream, const Intrinsics &in) {
+  stream_intrinsics_[stream] = in;
+}
+
+void Device::SetExtrinsics(
+    const Stream &from, const Stream &to, const Extrinsics &ex) {
+  stream_from_extrinsics_[from][to] = ex;
+}
+
+void Device::SetMotionIntrinsics(const MotionIntrinsics &in) {
+  if (motion_intrinsics_ == nullptr) {
+    motion_intrinsics_ = std::make_shared<MotionIntrinsics>();
+  }
+  *motion_intrinsics_ = in;
+}
+
+void Device::SetMotionExtrinsics(const Stream &from, const Extrinsics &ex) {
+  motion_from_extrinsics_[from] = ex;
+}
+
+void Device::LogOptionInfos() const {
+  channels_->LogControlInfos();
+}
+
+OptionInfo Device::GetOptionInfo(const Option &option) const {
+  if (!Supports(option)) {
+    LOG(WARNING) << "Unsupported option: " << option;
+    return {0, 0, 0};
+  }
+  auto &&info = channels_->GetControlInfo(option);
+  return {info.min, info.max, info.def};
+}
+
+std::int32_t Device::GetOptionValue(const Option &option) const {
+  if (!Supports(option)) {
+    LOG(WARNING) << "Unsupported option: " << option;
+    return -1;
+  }
+  return channels_->GetControlValue(option);
+}
+
+void Device::SetOptionValue(const Option &option, std::int32_t value) {
+  if (!Supports(option)) {
+    LOG(WARNING) << "Unsupported option: " << option;
+    return;
+  }
+  channels_->SetControlValue(option, value);
+}
+
+bool Device::RunOptionAction(const Option &option) const {
+  if (!Supports(option)) {
+    LOG(WARNING) << "Unsupported option: " << option;
+    return false;
+  }
+  return channels_->RunControlAction(option);
+}
+
+void Device::SetStreamCallback(
+    const Stream &stream, stream_callback_t callback, bool async) {
+  if (!CheckSupports(this, stream, false)) {
+    return;
+  }
+  if (callback) {
+    stream_callbacks_[stream] = callback;
+    if (async)
+      stream_async_callbacks_[stream] =
+          std::make_shared<stream_async_callback_t>(
+              to_string(stream), callback);  // max_data_size = 1
+  } else {
+    stream_callbacks_.erase(stream);
+    stream_async_callbacks_.erase(stream);
+  }
+}
+
+void Device::SetMotionCallback(motion_callback_t callback, bool async) {
+  motion_callback_ = callback;
+  if (callback) {
+    if (async)
+      motion_async_callback_ =
+          std::make_shared<motion_async_callback_t>("motion", callback, 1000);
+    // will drop old motion datas after callback cost > 2 s (1000 / 500 Hz)
+  } else {
+    motion_async_callback_ = nullptr;
+  }
+}
+
+bool Device::HasStreamCallback(const Stream &stream) const {
+  try {
+    return stream_callbacks_.at(stream) != nullptr;
+  } catch (const std::out_of_range &e) {
+    return false;
+  }
+}
+
+bool Device::HasMotionCallback() const {
+  return motion_callback_ != nullptr;
+}
+
+void Device::Start(const Source &source) {
+  if (source == Source::VIDEO_STREAMING) {
+    StartVideoStreaming();
+  } else if (source == Source::MOTION_TRACKING) {
+    StartMotionTracking();
+  } else if (source == Source::ALL) {
+    Start(Source::VIDEO_STREAMING);
+    Start(Source::MOTION_TRACKING);
+  } else {
+    LOG(ERROR) << "Unsupported source :(";
+  }
+}
+
+void Device::Stop(const Source &source) {
+  if (source == Source::VIDEO_STREAMING) {
+    StopVideoStreaming();
+  } else if (source == Source::MOTION_TRACKING) {
+    StopMotionTracking();
+  } else if (source == Source::ALL) {
+    Stop(Source::MOTION_TRACKING);
+    // Must stop motion tracking before video streaming and sleep a moment here
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    Stop(Source::VIDEO_STREAMING);
+  } else {
+    LOG(ERROR) << "Unsupported source :(";
+  }
+}
+
+void Device::WaitForStreams() {
+  CHECK(video_streaming_);
+  CHECK_NOTNULL(streams_);
+  streams_->WaitForStreams();
+}
+
+std::vector<device::StreamData> Device::GetStreamDatas(const Stream &stream) {
+  CHECK(video_streaming_);
+  CHECK_NOTNULL(streams_);
+  CheckSupports(this, stream);
+  std::lock_guard<std::mutex> _(mtx_streams_);
+  return streams_->GetStreamDatas(stream);
+}
+
+device::StreamData Device::GetLatestStreamData(const Stream &stream) {
+  CHECK(video_streaming_);
+  CHECK_NOTNULL(streams_);
+  CheckSupports(this, stream);
+  std::lock_guard<std::mutex> _(mtx_streams_);
+  return streams_->GetLatestStreamData(stream);
+}
+
+void Device::EnableMotionDatas(std::size_t max_size) {
+  CHECK_NOTNULL(motions_);
+  motions_->EnableMotionDatas(max_size);
+}
+
+std::vector<device::MotionData> Device::GetMotionDatas() {
+  CHECK(motion_tracking_);
+  CHECK_NOTNULL(motions_);
+  return motions_->GetMotionDatas();
+}
+
+const StreamRequest &Device::GetStreamRequest(const Capabilities &capability) {
+  try {
+    return stream_config_requests_.at(capability);
+  } catch (const std::out_of_range &e) {
+    auto &&requests = GetStreamRequests(capability);
+    if (requests.size() >= 1) {
+      VLOG(2) << "Select the first one stream request of " << capability;
+      return requests[0];
+    } else {
+      LOG(FATAL) << "Please config the stream request of " << capability;
+    }
+  }
+}
+
+void Device::StartVideoStreaming() {
+  if (video_streaming_) {
+    LOG(WARNING) << "Cannot start video streaming without first stopping it";
+    return;
+  }
+
+  streams_ = std::make_shared<Streams>(GetKeyStreams());
+
+  // if stream capabilities are supported with subdevices of device_
+  /*
+  Capabilities stream_capabilities[] = {
+    Capabilities::STEREO,
+    Capabilities::COLOR,
+    Capabilities::DEPTH,
+    Capabilities::POINTS,
+    Capabilities::FISHEYE,
+    Capabilities::INFRARED,
+    Capabilities::INFRARED2
+  };
+  for (auto &&capability : stream_capabilities) {
+  }
+  */
+  if (Supports(Capabilities::STEREO)) {
+    // do stream request selection if more than one request of each stream
+    auto &&stream_request = GetStreamRequest(Capabilities::STEREO);
+
+    streams_->ConfigStream(Capabilities::STEREO, stream_request);
+    uvc::set_device_mode(
+        *device_, stream_request.width, stream_request.height,
+        static_cast<int>(stream_request.format), stream_request.fps,
+        [this](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(Capabilities::STEREO, 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;
+}
+
+void Device::StopVideoStreaming() {
+  if (!video_streaming_) {
+    LOG(WARNING) << "Cannot stop video streaming without first starting it";
+    return;
+  }
+  stop_streaming(*device_);
+  video_streaming_ = false;
+}
+
+void Device::StartMotionTracking() {
+  if (!Supports(Capabilities::IMU)) {
+    LOG(FATAL) << "IMU capability is not supported by this device";
+  }
+  if (motion_tracking_) {
+    LOG(WARNING) << "Cannot start motion tracking without first stopping it";
+    return;
+  }
+  motions_->SetMotionCallback(
+      std::bind(&Device::CallbackMotionData, this, std::placeholders::_1));
+  // motions_->StartMotionTracking();
+  motion_tracking_ = true;
+}
+
+void Device::StopMotionTracking() {
+  if (!motion_tracking_) {
+    LOG(WARNING) << "Cannot stop motion tracking without first starting it";
+    return;
+  }
+  // motions_->StopMotionTracking();
+  motion_tracking_ = false;
+}
+
+void Device::OnStereoStreamUpdate() {}
+
+void Device::ReadAllInfos() {
+  device_info_ = std::make_shared<DeviceInfo>();
+
+  CHECK_NOTNULL(channels_);
+  Channels::img_params_t img_params;
+  Channels::imu_params_t imu_params;
+  if (!channels_->GetFiles(device_info_.get(), &img_params, &imu_params)) {
+#if defined(WITH_DEVICE_INFO_REQUIRED)
+    LOG(FATAL)
+#else
+    LOG(WARNING)
+#endif
+        << "Read device infos failed. Please upgrade your firmware to the "
+           "latest version.";
+  }
+  VLOG(2) << "Device info: {name: " << device_info_->name
+          << ", serial_number: " << device_info_->serial_number
+          << ", firmware_version: "
+          << device_info_->firmware_version.to_string()
+          << ", hardware_version: "
+          << device_info_->hardware_version.to_string()
+          << ", spec_version: " << device_info_->spec_version.to_string()
+          << ", lens_type: " << device_info_->lens_type.to_string()
+          << ", imu_type: " << device_info_->imu_type.to_string()
+          << ", nominal_baseline: " << device_info_->nominal_baseline << "}";
+
+  device_info_->name = uvc::get_name(*device_);
+  if (img_params.ok) {
+    SetIntrinsics(Stream::LEFT, img_params.in_left);
+    SetIntrinsics(Stream::RIGHT, img_params.in_right);
+    SetExtrinsics(Stream::RIGHT, Stream::LEFT, img_params.ex_right_to_left);
+    VLOG(2) << "Intrinsics left: {" << GetIntrinsics(Stream::LEFT) << "}";
+    VLOG(2) << "Intrinsics right: {" << GetIntrinsics(Stream::RIGHT) << "}";
+    VLOG(2) << "Extrinsics right to left: {"
+            << GetExtrinsics(Stream::RIGHT, Stream::LEFT) << "}";
+  } else {
+    LOG(WARNING) << "Intrinsics & extrinsics not exist";
+  }
+  if (imu_params.ok) {
+    SetMotionIntrinsics({imu_params.in_accel, imu_params.in_gyro});
+    SetMotionExtrinsics(Stream::LEFT, imu_params.ex_left_to_imu);
+    VLOG(2) << "Motion intrinsics: {" << GetMotionIntrinsics() << "}";
+    VLOG(2) << "Motion extrinsics left to imu: {"
+            << GetMotionExtrinsics(Stream::LEFT) << "}";
+  } else {
+    VLOG(2) << "Motion intrinsics & extrinsics not exist";
+  }
+}
+
+void Device::CallbackPushedStreamData(const Stream &stream) {
+  if (HasStreamCallback(stream)) {
+    auto &&datas = streams_->stream_datas(stream);
+    // if (datas.size() > 0) {}
+    auto &&data = datas.back();
+    if (stream_async_callbacks_.find(stream) != stream_async_callbacks_.end()) {
+      stream_async_callbacks_.at(stream)->PushData(data);
+    } else {
+      stream_callbacks_.at(stream)(data);
+    }
+  }
+}
+
+void Device::CallbackMotionData(const device::MotionData &data) {
+  if (HasMotionCallback()) {
+    if (motion_async_callback_) {
+      motion_async_callback_->PushData(data);
+    } else {
+      motion_callback_(data);
+    }
+  }
+}
+
+MYNTEYE_END_NAMESPACE

src/mynteye/device/device_s.cc

@@ -0,0 +1,41 @@
+// 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/device_s.h"
+
+#include "mynteye/logger.h"
+#include "mynteye/device/motions.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+StandardDevice::StandardDevice(std::shared_ptr<uvc::device> device)
+    : Device(Model::STANDARD, device) {
+  VLOG(2) << __func__;
+}
+
+StandardDevice::~StandardDevice() {
+  VLOG(2) << __func__;
+}
+
+std::vector<Stream> StandardDevice::GetKeyStreams() const {
+  return {Stream::LEFT, Stream::RIGHT};
+}
+
+void StandardDevice::OnStereoStreamUpdate() {
+  if (motion_tracking_) {
+    auto &&motions = this->motions();
+    motions->DoMotionTrack();
+  }
+}
+
+MYNTEYE_END_NAMESPACE

src/mynteye/device/device_s.h

@@ -0,0 +1,37 @@
+// 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_DEVICE_S_H_
+#define MYNTEYE_DEVICE_DEVICE_S_H_
+#pragma once
+
+#include <memory>
+#include <vector>
+
+#include "mynteye/device/device.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+class StandardDevice : public Device {
+ public:
+  explicit StandardDevice(std::shared_ptr<uvc::device> device);
+  virtual ~StandardDevice();
+
+  std::vector<Stream> GetKeyStreams() const override;
+
+  void OnStereoStreamUpdate() override;
+};
+
+MYNTEYE_END_NAMESPACE
+
+#endif  // MYNTEYE_DEVICE_DEVICE_S_H_

src/mynteye/device/motions.cc

@@ -0,0 +1,110 @@
+// 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/motions.h"
+
+#include "mynteye/logger.h"
+#include "mynteye/device/channels.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+Motions::Motions(std::shared_ptr<Channels> channels)
+    : channels_(channels),
+      motion_callback_(nullptr),
+      motion_datas_enabled_(false),
+      is_imu_tracking(false) {
+  CHECK_NOTNULL(channels_);
+  VLOG(2) << __func__;
+}
+
+Motions::~Motions() {
+  VLOG(2) << __func__;
+}
+
+void Motions::SetMotionCallback(motion_callback_t callback) {
+  motion_callback_ = callback;
+  if (motion_callback_) {
+    channels_->SetImuCallback([this](const ImuPacket &packet) {
+      if (!motion_callback_ && !motion_datas_enabled_) {
+        LOG(WARNING) << "";
+        return;
+      }
+      for (auto &&seg : packet.segments) {
+        auto &&imu = std::make_shared<ImuData>();
+        imu->frame_id = seg.frame_id;
+        // if (seg.offset < 0 &&
+        //     static_cast<uint32_t>(-seg.offset) > packet.timestamp) {
+        //   LOG(WARNING) << "Imu timestamp offset is incorrect";
+        // }
+        imu->timestamp = packet.timestamp + seg.offset;
+        imu->accel[0] = seg.accel[0] * 8.f / 0x10000;
+        imu->accel[1] = seg.accel[1] * 8.f / 0x10000;
+        imu->accel[2] = seg.accel[2] * 8.f / 0x10000;
+        imu->gyro[0] = seg.gyro[0] * 1000.f / 0x10000;
+        imu->gyro[1] = seg.gyro[1] * 1000.f / 0x10000;
+        imu->gyro[2] = seg.gyro[2] * 1000.f / 0x10000;
+        imu->temperature = seg.temperature / 326.8f + 25;
+
+        std::lock_guard<std::mutex> _(mtx_datas_);
+        motion_data_t data = {imu};
+        motion_datas_.push_back(data);
+
+        motion_callback_(data);
+      }
+    });
+  } else {
+    channels_->SetImuCallback(nullptr);
+  }
+}
+
+void Motions::DoMotionTrack() {
+  channels_->DoImuTrack();
+}
+
+void Motions::StartMotionTracking() {
+  if (!is_imu_tracking) {
+    channels_->StartImuTracking();
+    is_imu_tracking = true;
+  } else {
+    LOG(WARNING) << "Imu is tracking already";
+  }
+}
+
+void Motions::StopMotionTracking() {
+  if (is_imu_tracking) {
+    channels_->StopImuTracking();
+    is_imu_tracking = false;
+  }
+}
+
+void Motions::EnableMotionDatas(std::size_t max_size) {
+  if (max_size <= 0) {
+    LOG(WARNING) << "Could not enable motion datas with max_size <= 0";
+    return;
+  }
+  motion_datas_enabled_ = true;
+  motion_datas_max_size = max_size;
+}
+
+Motions::motion_datas_t Motions::GetMotionDatas() {
+  if (!motion_datas_enabled_) {
+    LOG(FATAL) << "Must enable motion datas before getting them, or you set "
+                  "motion callback instead";
+  }
+  std::lock_guard<std::mutex> _(mtx_datas_);
+  motion_datas_t datas = motion_datas_;
+  motion_datas_.clear();
+  return datas;
+}
+
+MYNTEYE_END_NAMESPACE

src/mynteye/device/motions.h

@@ -0,0 +1,64 @@
+// 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_MOTIONS_H_
+#define MYNTEYE_DEVICE_MOTIONS_H_
+#pragma once
+
+#include <memory>
+#include <mutex>
+#include <vector>
+
+#include "mynteye/mynteye.h"
+#include "mynteye/device/callbacks.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+class Channels;
+
+class Motions {
+ public:
+  using motion_data_t = device::MotionData;
+  using motion_datas_t = std::vector<motion_data_t>;
+
+  using motion_callback_t = device::MotionCallback;
+
+  explicit Motions(std::shared_ptr<Channels> channels);
+  ~Motions();
+
+  void SetMotionCallback(motion_callback_t callback);
+  void DoMotionTrack();
+
+  void StartMotionTracking();
+  void StopMotionTracking();
+
+  void EnableMotionDatas(std::size_t max_size);
+  motion_datas_t GetMotionDatas();
+
+ private:
+  std::shared_ptr<Channels> channels_;
+
+  motion_callback_t motion_callback_;
+
+  motion_datas_t motion_datas_;
+  bool motion_datas_enabled_;
+  std::size_t motion_datas_max_size;
+
+  bool is_imu_tracking;
+
+  std::mutex mtx_datas_;
+};
+
+MYNTEYE_END_NAMESPACE
+
+#endif  // MYNTEYE_DEVICE_MOTIONS_H_

src/mynteye/device/streams.cc

@@ -0,0 +1,310 @@
+// 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/streams.h"
+
+#include <algorithm>
+#include <chrono>
+#include <iomanip>
+#include <stdexcept>
+
+#include "mynteye/logger.h"
+#include "mynteye/device/types.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+namespace {
+
+bool unpack_stereo_img_data(
+    const void *data, const StreamRequest &request, ImgData *img) {
+  CHECK_NOTNULL(img);
+  CHECK_EQ(request.format, Format::YUYV);
+
+  auto data_new = reinterpret_cast<const std::uint8_t *>(data);
+  std::size_t data_n =
+      request.width * request.height * bytes_per_pixel(request.format);
+  auto data_end = data_new + data_n;
+
+  std::size_t packet_n = sizeof(ImagePacket);
+  std::vector<std::uint8_t> packet(packet_n);
+  std::reverse_copy(data_end - packet_n, data_end, packet.begin());
+
+  ImagePacket img_packet(packet.data());
+  // LOG(INFO) << "ImagePacket: header=0x" << std::hex <<
+  // static_cast<int>(img_packet.header)
+  //   << ", size=0x" << std::hex << static_cast<int>(img_packet.size)
+  //   << ", frame_id="<< std::dec << img_packet.frame_id
+  //   << ", timestamp="<< std::dec << img_packet.timestamp
+  //   << ", exposure_time="<< std::dec << img_packet.exposure_time
+  //   << ", checksum=0x" << std::hex << static_cast<int>(img_packet.checksum);
+
+  if (img_packet.header != 0x3B) {
+    VLOG(2) << "Image packet header must be 0x3B, but 0x" << std::hex
+            << std::uppercase << std::setw(2) << std::setfill('0')
+            << static_cast<int>(img_packet.header) << " now";
+    return false;
+  }
+
+  std::uint8_t checksum = 0;
+  for (std::size_t i = 2, n = packet_n - 2; i <= n; i++) {  // content: [2,9]
+    checksum = (checksum ^ packet[i]);
+  }
+  if (img_packet.checksum != checksum) {
+    VLOG(2) << "Image packet checksum should be 0x" << std::hex
+            << std::uppercase << std::setw(2) << std::setfill('0')
+            << static_cast<int>(img_packet.checksum) << ", but 0x"
+            << std::setw(2) << std::setfill('0') << static_cast<int>(checksum)
+            << " now";
+    return false;
+  }
+
+  img->frame_id = img_packet.frame_id;
+  img->timestamp = img_packet.timestamp;
+  img->exposure_time = img_packet.exposure_time;
+  return true;
+}
+
+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
+
+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}),
+      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__;
+}
+
+Streams::~Streams() {
+  VLOG(2) << __func__;
+}
+
+void Streams::ConfigStream(
+    const Capabilities &capability, const StreamRequest &request) {
+  if (!IsStreamCapability(capability)) {
+    LOG(ERROR) << "Cannot config stream without stream capability";
+    return;
+  }
+  VLOG(2) << "Config stream request of " << capability << ", " << request;
+  stream_config_requests_[capability] = request;
+}
+
+bool Streams::PushStream(const Capabilities &capability, const void *data) {
+  if (!HasStreamConfigRequest(capability)) {
+    LOG(FATAL) << "Cannot push stream without stream config request";
+  }
+  std::unique_lock<std::mutex> lock(mtx_);
+  auto &&request = GetStreamConfigRequest(capability);
+  bool pushed = false;
+  switch (capability) {
+    case Capabilities::STEREO: {
+      // alloc left
+      AllocStreamData(Stream::LEFT, request, Format::GREY);
+      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, Format::GREY);
+        auto &&right_data = stream_datas_map_[Stream::RIGHT].back();
+        *right_data.img = *left_data.img;
+        right_data.frame_id = left_data.img->frame_id;
+        // 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());
+        pushed = true;
+      } else {
+        // discard left
+        DiscardStreamData(Stream::LEFT);
+        VLOG(2) << "Image packet is unaccepted, frame dropped";
+        pushed = false;
+      }
+    } break;
+    default:
+      LOG(FATAL) << "Not supported " << capability << " now";
+  }
+  if (HasKeyStreamDatas())
+    cv_.notify_one();
+  return pushed;
+}
+
+void Streams::WaitForStreams() {
+  std::unique_lock<std::mutex> lock(mtx_);
+  auto ready = std::bind(&Streams::HasKeyStreamDatas, this);
+  if (!ready() && !cv_.wait_for(lock, std::chrono::seconds(2), ready)) {
+    LOG(FATAL) << "Timeout waiting for key frames. Please use USB 3.0, and not "
+                  "in virtual machine.";
+  }
+}
+
+void Streams::ConfigStreamLimits(
+    const Stream &stream, std::size_t max_data_size) {
+  CHECK_GT(max_data_size, 0);
+  stream_limits_map_[stream] = max_data_size;
+}
+
+std::size_t Streams::GetStreamDataMaxSize(const Stream &stream) const {
+  try {
+    return stream_limits_map_.at(stream);
+  } catch (const std::out_of_range &e) {
+    return 4;  // default stream data max size
+  }
+}
+
+Streams::stream_datas_t Streams::GetStreamDatas(const Stream &stream) {
+  std::unique_lock<std::mutex> lock(mtx_);
+  if (!HasStreamDatas(stream)) {
+    LOG(WARNING) << "There are no stream datas of " << stream
+                 << ". Did you call WaitForStreams() before this?";
+    return {};
+  }
+  auto datas = stream_datas_map_.at(stream);
+  stream_datas_map_[stream].clear();
+  return datas;
+}
+
+Streams::stream_data_t Streams::GetLatestStreamData(const Stream &stream) {
+  std::unique_lock<std::mutex> lock(mtx_);
+  if (!HasStreamDatas(stream)) {
+    LOG(WARNING) << "There are no stream datas of " << stream
+                 << ". Did you call WaitForStreams() before this?";
+    return {};
+  }
+  auto data = stream_datas_map_.at(stream).back();
+  stream_datas_map_[stream].clear();
+  return data;
+}
+
+const Streams::stream_datas_t &Streams::stream_datas(const Stream &stream) {
+  std::unique_lock<std::mutex> lock(mtx_);
+  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 {
+  return std::find(
+             stream_capabilities_.begin(), stream_capabilities_.end(),
+             capability) != stream_capabilities_.end();
+}
+
+bool Streams::HasStreamConfigRequest(const Capabilities &capability) const {
+  return stream_config_requests_.find(capability) !=
+         stream_config_requests_.end();
+}
+
+const StreamRequest &Streams::GetStreamConfigRequest(
+    const Capabilities &capability) const {
+  return stream_config_requests_.at(capability);
+}
+
+bool Streams::HasStreamDatas(const Stream &stream) const {
+  return stream_datas_map_.find(stream) != stream_datas_map_.end() &&
+         !stream_datas_map_.at(stream).empty();
+}
+
+void Streams::AllocStreamData(
+    const Stream &stream, const StreamRequest &request) {
+  AllocStreamData(stream, request, request.format);
+}
+
+void Streams::AllocStreamData(
+    const Stream &stream, const StreamRequest &request, const Format &format) {
+  stream_data_t data;
+
+  if (HasStreamDatas(stream)) {
+    // If cached equal to limits_max, drop the oldest one.
+    if (stream_datas_map_.at(stream).size() == GetStreamDataMaxSize(stream)) {
+      auto &&datas = stream_datas_map_[stream];
+      // reuse the dropped data
+      data.img = datas.front().img;
+      data.frame = datas.front().frame;
+      data.frame_id = 0;
+      datas.erase(datas.begin());
+      VLOG(2) << "Stream data of " << stream << " is dropped as out of limits";
+    }
+  }
+
+  if (stream == Stream::LEFT || stream == Stream::RIGHT) {
+    if (!data.img) {
+      data.img = std::make_shared<ImgData>();
+    }
+  } else {
+    data.img = nullptr;
+  }
+  if (!data.frame) {
+    data.frame = std::make_shared<frame_t>(
+        request.width, request.height, format, nullptr);
+  }
+  data.frame_id = 0;
+  stream_datas_map_[stream].push_back(data);
+}
+
+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.pop_back();
+  } else {
+    VLOG(2) << "Stream data of " << stream << " is empty, could not discard";
+  }
+}
+
+bool Streams::HasKeyStreamDatas() const {
+  for (auto &&s : key_streams_) {
+    if (!HasStreamDatas(s))
+      return false;
+  }
+  return true;
+}
+
+MYNTEYE_END_NAMESPACE

src/mynteye/device/streams.h

@@ -0,0 +1,93 @@
+// 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_STREAMS_H_
+#define MYNTEYE_DEVICE_STREAMS_H_
+#pragma once
+
+#include <condition_variable>
+#include <functional>
+#include <map>
+#include <mutex>
+#include <vector>
+
+#include "mynteye/mynteye.h"
+#include "mynteye/types.h"
+#include "mynteye/device/callbacks.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+class Streams {
+ public:
+  using frame_t = device::Frame;
+  using stream_data_t = device::StreamData;
+  using stream_datas_t = std::vector<stream_data_t>;
+
+  using unpack_img_data_t = std::function<bool(
+      const void *data, const StreamRequest &request, ImgData *img)>;
+  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);
+  ~Streams();
+
+  void ConfigStream(
+      const Capabilities &capability, const StreamRequest &request);
+
+  bool PushStream(const Capabilities &capability, const void *data);
+
+  void WaitForStreams();
+
+  void ConfigStreamLimits(const Stream &stream, std::size_t max_data_size);
+  std::size_t GetStreamDataMaxSize(const Stream &stream) const;
+
+  stream_datas_t GetStreamDatas(const Stream &stream);
+  stream_data_t GetLatestStreamData(const Stream &stream);
+
+  const stream_datas_t &stream_datas(const Stream &stream);
+
+ private:
+  bool IsStreamCapability(const Capabilities &capability) const;
+  bool HasStreamConfigRequest(const Capabilities &capability) const;
+
+  const StreamRequest &GetStreamConfigRequest(
+      const Capabilities &capability) const;
+
+  bool HasStreamDatas(const Stream &stream) const;
+
+  void AllocStreamData(const Stream &stream, const StreamRequest &request);
+  void AllocStreamData(
+      const Stream &stream, const StreamRequest &request, const Format &format);
+
+  void DiscardStreamData(const Stream &stream);
+
+  bool HasKeyStreamDatas() const;
+
+  std::vector<Stream> key_streams_;
+
+  std::vector<Capabilities> stream_capabilities_;
+  std::map<Capabilities, StreamRequest> stream_config_requests_;
+
+  std::map<Stream, unpack_img_data_t> unpack_img_data_map_;
+  std::map<Stream, unpack_img_pixels_t> unpack_img_pixels_map_;
+
+  std::map<Stream, std::size_t> stream_limits_map_;
+  std::map<Stream, stream_datas_t> stream_datas_map_;
+
+  std::mutex mtx_;
+  std::condition_variable cv_;
+};
+
+MYNTEYE_END_NAMESPACE
+
+#endif  // MYNTEYE_DEVICE_STREAMS_H_

src/mynteye/device/types.cc

@@ -0,0 +1,61 @@
+// 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/types.h"
+
+#include <algorithm>
+#include <iomanip>
+#include <sstream>
+
+#include "mynteye/logger.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+std::string Version::to_string() const {
+  std::stringstream s;
+  s << static_cast<int>(major_) << "." << static_cast<int>(minor_);
+  return s.str();
+}
+
+std::vector<std::string> Version::split(const std::string &s) {
+  std::vector<std::string> result;
+  auto e = s.end();
+  auto i = s.begin();
+  while (i != e) {
+    i = std::find_if_not(i, e, [](char c) { return c == '.'; });
+    if (i == e)
+      break;
+    auto j = std::find(i, e, '.');
+    result.emplace_back(i, j);
+    i = j;
+  }
+  return result;
+}
+
+Version::value_t Version::parse_part(const std::string &name, size_t part) {
+  return std::stoi(split(name)[part]);
+}
+
+std::string Type::to_string() const {
+  std::stringstream s;
+  s << std::hex << std::uppercase << std::setfill('0') << std::setw(2)
+    << vendor_ << std::setfill('0') << std::setw(2) << product_;
+  return s.str();
+}
+
+Type::value_t Type::parse_part(
+    const std::string &name, size_t pos, size_t count) {
+  return std::stoi(name.substr(pos, count), 0, 16);
+}
+
+MYNTEYE_END_NAMESPACE

src/mynteye/device/types.h

@@ -0,0 +1,296 @@
+// 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_TYPES_H_
+#define MYNTEYE_DEVICE_TYPES_H_
+#pragma once
+
+#include <cstdint>
+
+#include <array>
+#include <bitset>
+#include <string>
+#include <vector>
+
+#include "mynteye/mynteye.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+#define MYNTEYE_PROPERTY(TYPE, NAME) \
+ public:                             \
+  void set_##NAME(TYPE NAME) {       \
+    NAME##_ = NAME;                  \
+  }                                  \
+  TYPE NAME() const {                \
+    return NAME##_;                  \
+  }                                  \
+                                     \
+ private:                            \
+  TYPE NAME##_;
+
+/**
+ * Version.
+ */
+class MYNTEYE_API Version {
+ public:
+  using size_t = std::size_t;
+  using value_t = std::uint8_t;
+
+  Version() = default;
+  Version(value_t major, value_t minor) : major_(major), minor_(minor) {}
+  explicit Version(const std::string &name)
+      : major_(parse_part(name, 0)), minor_(parse_part(name, 1)) {}
+  virtual ~Version() {}
+
+  bool operator==(const Version &other) const {
+    return major_ == other.major_ && minor_ == other.minor_;
+  }
+  bool operator<=(const Version &other) const {
+    if (major_ < other.major_)
+      return true;
+    if (major_ > other.major_)
+      return false;
+    return minor_ <= other.minor_;
+  }
+  bool operator!=(const Version &other) const {
+    return !(*this == other);
+  }
+  bool operator<(const Version &other) const {
+    return !(*this == other) && (*this <= other);
+  }
+  bool operator>(const Version &other) const {
+    return !(*this <= other);
+  }
+  bool operator>=(const Version &other) const {
+    return (*this == other) || (*this > other);
+  }
+  bool is_between(const Version &from, const Version &until) {
+    return (from <= *this) && (*this <= until);
+  }
+
+  std::string to_string() const;
+
+  static std::vector<std::string> split(const std::string &s);
+  static value_t parse_part(const std::string &name, size_t part);
+
+  MYNTEYE_PROPERTY(value_t, major)
+  MYNTEYE_PROPERTY(value_t, minor)
+};
+
+/**
+ * Hardware version.
+ */
+class MYNTEYE_API HardwareVersion : public Version {
+ public:
+  using flag_t = std::bitset<8>;
+
+  HardwareVersion() = default;
+  HardwareVersion(value_t major, value_t minor, value_t flag = 0)
+      : Version(major, minor), flag_(flag) {}
+  explicit HardwareVersion(const std::string &name, value_t flag = 0)
+      : Version(parse_part(name, 0), parse_part(name, 1)), flag_(flag) {}
+
+  MYNTEYE_PROPERTY(flag_t, flag)
+};
+
+/**
+ * Type.
+ */
+class MYNTEYE_API Type {
+ public:
+  using size_t = std::size_t;
+  using value_t = std::uint16_t;
+
+  Type() = default;
+  Type(value_t vendor, value_t product) : vendor_(vendor), product_(product) {}
+  explicit Type(const std::string &name)
+      : vendor_(parse_part(name, 0, 2)), product_(parse_part(name, 2, 2)) {}
+  virtual ~Type() {}
+
+  std::string to_string() const;
+  static value_t parse_part(const std::string &name, size_t pos, size_t count);
+
+  MYNTEYE_PROPERTY(value_t, vendor)
+  MYNTEYE_PROPERTY(value_t, product)
+};
+
+/**
+ * @ingroup datatypes
+ * Device infomation.
+ */
+struct MYNTEYE_API DeviceInfo {
+  std::string name;
+  std::string serial_number;
+  Version firmware_version;
+  HardwareVersion hardware_version;
+  Version spec_version;
+  Type lens_type;
+  Type imu_type;
+  std::uint16_t nominal_baseline;
+};
+
+/**
+ * @ingroup datatypes
+ * Image packet.
+ */
+#pragma pack(push, 1)
+struct ImagePacket {
+  std::uint8_t header;
+  std::uint8_t size;
+  std::uint16_t frame_id;
+  std::uint32_t timestamp;
+  std::uint16_t exposure_time;
+  std::uint8_t checksum;
+
+  ImagePacket() = default;
+  explicit ImagePacket(std::uint8_t *data) {
+    from_data(data);
+  }
+
+  void from_data(std::uint8_t *data) {
+    header = *data;
+    size = *(data + 1);
+    frame_id = (*(data + 2) << 8) | *(data + 3);
+    timestamp = (*(data + 4) << 24) | (*(data + 5) << 16) | (*(data + 6) << 8) |
+                *(data + 7);
+    exposure_time = (*(data + 8) << 8) | *(data + 9);
+    checksum = *(data + 10);
+  }
+};
+#pragma pack(pop)
+
+/**
+ * @ingroup datatypes
+ * Imu request packet.
+ */
+#pragma pack(push, 1)
+struct ImuReqPacket {
+  std::uint8_t header;
+  std::uint32_t serial_number;
+
+  ImuReqPacket() = default;
+  explicit ImuReqPacket(std::uint32_t serial_number)
+      : ImuReqPacket(0x5A, serial_number) {}
+  ImuReqPacket(std::uint8_t header, std::uint32_t serial_number)
+      : header(header), serial_number(serial_number) {}
+
+  std::array<std::uint8_t, 5> to_data() const {
+    return {{header, static_cast<std::uint8_t>((serial_number >> 24) & 0xFF),
+             static_cast<std::uint8_t>((serial_number >> 16) & 0xFF),
+             static_cast<std::uint8_t>((serial_number >> 8) & 0xFF),
+             static_cast<std::uint8_t>(serial_number & 0xFF)}};
+  }
+};
+#pragma pack(pop)
+
+/**
+ * @ingroup datatypes
+ * Imu segment.
+ */
+#pragma pack(push, 1)
+struct ImuSegment {
+  std::int16_t offset;
+  std::uint16_t frame_id;
+  std::int16_t accel[3];
+  std::int16_t temperature;
+  std::int16_t gyro[3];
+
+  ImuSegment() = default;
+  explicit ImuSegment(std::uint8_t *data) {
+    from_data(data);
+  }
+
+  void from_data(std::uint8_t *data) {
+    offset = (*(data) << 8) | *(data + 1);
+    frame_id = (*(data + 2) << 8) | *(data + 3);
+    accel[0] = (*(data + 4) << 8) | *(data + 5);
+    accel[1] = (*(data + 6) << 8) | *(data + 7);
+    accel[2] = (*(data + 8) << 8) | *(data + 9);
+    temperature = (*(data + 10) << 8) | *(data + 11);
+    gyro[0] = (*(data + 12) << 8) | *(data + 13);
+    gyro[1] = (*(data + 14) << 8) | *(data + 15);
+    gyro[2] = (*(data + 16) << 8) | *(data + 17);
+  }
+};
+#pragma pack(pop)
+
+/**
+ * @ingroup datatypes
+ * Imu packet.
+ */
+#pragma pack(push, 1)
+struct ImuPacket {
+  std::uint32_t serial_number;
+  std::uint32_t timestamp;
+  std::uint8_t count;
+  std::vector<ImuSegment> segments;
+
+  ImuPacket() = default;
+  explicit ImuPacket(std::uint8_t *data) {
+    from_data(data);
+  }
+
+  void from_data(std::uint8_t *data) {
+    serial_number = (*(data) << 24) | (*(data + 1) << 16) | (*(data + 2) << 8) |
+                    *(data + 3);
+    timestamp = (*(data + 4) << 24) | (*(data + 5) << 16) | (*(data + 6) << 8) |
+                *(data + 7);
+    count = *(data + 8);
+
+    std::size_t seg_n = sizeof(ImuSegment);  // 18
+    for (std::size_t i = 0; i < count; i++) {
+      segments.push_back(ImuSegment(data + 9 + (seg_n * i)));
+    }
+  }
+};
+#pragma pack(pop)
+
+/**
+ * @ingroup datatypes
+ * Imu response packet.
+ */
+#pragma pack(push, 1)
+struct ImuResPacket {
+  std::uint8_t header;
+  std::uint8_t state;
+  std::uint16_t size;
+  std::vector<ImuPacket> packets;
+  std::uint8_t checksum;
+
+  ImuResPacket() = default;
+  explicit ImuResPacket(std::uint8_t *data) {
+    from_data(data);
+  }
+
+  void from_data(std::uint8_t *data) {
+    header = *data;
+    state = *(data + 1);
+    size = (*(data + 2) << 8) | *(data + 3);
+
+    std::size_t seg_n = sizeof(ImuSegment);  // 18
+    for (std::size_t i = 4; i < size;) {
+      ImuPacket packet(data + i);
+      packets.push_back(packet);
+      i += 9 + (packet.count * seg_n);
+    }
+
+    checksum = *(data + 4 + size);
+  }
+};
+#pragma pack(pop)
+
+#undef MYNTEYE_PROPERTY
+
+MYNTEYE_END_NAMESPACE
+
+#endif  // MYNTEYE_DEVICE_TYPES_H_

src/mynteye/device/utils.cc

@@ -0,0 +1,115 @@
+// 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/utils.h"
+
+#include "mynteye/logger.h"
+
+#include "mynteye/device/context.h"
+#include "mynteye/device/device.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+namespace device {
+
+std::shared_ptr<Device> select() {
+  LOG(INFO) << "Detecting MYNT EYE devices";
+  Context context;
+  auto &&devices = context.devices();
+
+  size_t n = devices.size();
+  if (n <= 0) {
+    LOG(ERROR) << "No MYNT EYE devices :(";
+    return nullptr;
+  }
+
+  LOG(INFO) << "MYNT EYE devices:";
+  for (size_t i = 0; i < n; i++) {
+    auto &&device = devices[i];
+    LOG(INFO) << "  index: " << i
+              << ", name: " << device->GetInfo(Info::DEVICE_NAME)
+              << ", sn: " << device->GetInfo(Info::SERIAL_NUMBER);
+  }
+
+  std::shared_ptr<Device> device = nullptr;
+  if (n <= 1) {
+    device = devices[0];
+    LOG(INFO) << "Only one MYNT EYE device, select index: 0";
+  } else {
+    while (true) {
+      size_t i;
+      LOG(INFO) << "There are " << n << " MYNT EYE devices, select index: ";
+      std::cin >> i;
+      if (i >= n) {
+        LOG(WARNING) << "Index out of range :(";
+        continue;
+      }
+      device = devices[i];
+      break;
+    }
+  }
+
+  return device;
+}
+
+}  // namespace device
+
+namespace utils {
+
+float get_real_exposure_time(
+    std::int32_t frame_rate, std::uint16_t exposure_time) {
+  float real_max = 0;
+  switch (frame_rate) {
+    case 10:
+      real_max = 18;
+      break;
+    case 15:
+      real_max = 18;
+      break;
+    case 20:
+      real_max = 18;
+      break;
+    case 25:
+      real_max = 18;
+      break;
+    case 30:
+      real_max = 18;
+      break;
+    case 35:
+      real_max = 18;
+      break;
+    case 40:
+      real_max = 18;
+      break;
+    case 45:
+      real_max = 18;
+      break;
+    case 50:
+      real_max = 17;
+      break;
+    case 55:
+      real_max = 16.325;
+      break;
+    case 60:
+      real_max = 15;
+      break;
+    default:
+      LOG(ERROR) << "Invalid frame rate: " << frame_rate;
+      return exposure_time;
+  }
+  return exposure_time * real_max / 480.f;
+}
+
+}  // namespace utils
+
+MYNTEYE_END_NAMESPACE