Merge branch 'develop' into feature/android

* develop: (28 commits)
  feat(*): add function to querry some hardware info
  feat: forbid 2100/210A uless update sdk to 2.3.1 and above
  refactor(synthetic): remove usless logic
  chore(readme): update readme
  docs(doxyfile): update version
  fix: change cmake version to 2.3.2
  feat(api) sdk/firmware version check
  feat(api): version check
  fix(wrapper): fix camera info repeat bug
  build(makefile): ensure uninstall before install
  fix(correspondence): also wait stream matched ready
  fix(record): shield diable logic temporarily
  chore(readme): chore(readme): update readme
  chore(readme): update readme
  chore(doc): update version
  fix(samples): delete useless comment
  fix(ros): fix camera info bug
  fix(correspondence): improve warning if not start motion tracking
  fix(correspondence): fix include header
  fix(ros): record close bug
  ...

CMakeLists.txt

@@ -14,7 +14,7 @@
 
 cmake_minimum_required(VERSION 3.0)
 
-project(mynteye VERSION 2.3.0 LANGUAGES C CXX)
+project(mynteye VERSION 2.3.2 LANGUAGES C CXX)
 
 include(cmake/Common.cmake)
 
@@ -219,6 +219,8 @@ if(WITH_API)
     src/mynteye/api/processor/depth_processor_ocv.cc
     src/mynteye/api/processor/rectify_processor_ocv.cc
     src/mynteye/api/config.cc
+    src/mynteye/api/correspondence.cc
+    src/mynteye/api/version_checker.cc
   )
   if(WITH_CAM_MODELS)
     list(APPEND MYNTEYE_SRCS

Makefile

@@ -130,7 +130,7 @@ endif
 
 # install
 
-install: build
+install: uninstall build
 	@$(call echo,Make $@)
 ifeq ($(HOST_OS),Win)
 ifneq ($(HOST_NAME),MinGW)

README.md

@@ -1,6 +1,6 @@
 # MYNT® EYE S SDK
 
-[![](https://img.shields.io/badge/MYNT%20EYE%20S%20SDK-2.3.0-brightgreen.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK)
+[![](https://img.shields.io/badge/MYNT%20EYE%20S%20SDK-2.3.2-brightgreen.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK)
 
 ## Overview
 
@@ -17,11 +17,11 @@ Please follow the guide doc to install the SDK on different platforms.
 ## Documentations
 
 * [API Doc](https://github.com/slightech/MYNT-EYE-S-SDK/releases): API reference, some guides and data spec.
-  * en: [![](https://img.shields.io/badge/Download-PDF-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2764152/mynt-eye-s-sdk-apidoc-2.3.0-en.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2764156/mynt-eye-s-sdk-apidoc-2.3.0-en.zip) [![](https://img.shields.io/badge/Online-HTML-blue.svg?style=flat)](https://slightech.github.io/MYNT-EYE-S-SDK/)
+  * en: [![](https://img.shields.io/badge/Download-PDF-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2913110/mynt-eye-s-sdk-apidoc-2.3.2-en.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2913111/mynt-eye-s-sdk-apidoc-2.3.2-en.zip) [![](https://img.shields.io/badge/Online-HTML-blue.svg?style=flat)](https://slightech.github.io/MYNT-EYE-S-SDK/)
-  * zh-Hans: [![](https://img.shields.io/badge/Download-PDF-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2764160/mynt-eye-s-sdk-apidoc-2.3.0-zh-Hans.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2764173/mynt-eye-s-sdk-apidoc-2.3.0-en.zip) [![](https://img.shields.io/badge/Online-HTML-blue.svg?style=flat)](http://doc.myntai.com/resource/api/mynt-eye-s-sdk-apidoc-2.3.0-zh-Hans/mynt-eye-s-sdk-apidoc-2.3.0-zh-Hans/index.html)
+  * zh-Hans: [![](https://img.shields.io/badge/Download-PDF-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2913112/mynt-eye-s-sdk-apidoc-2.3.2-zh-Hans.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2913113/mynt-eye-s-sdk-apidoc-2.3.2-zh-Hans.zip) [![](https://img.shields.io/badge/Online-HTML-blue.svg?style=flat)](http://doc.myntai.com/resource/api/mynt-eye-s-sdk-apidoc-2.3.2-zh-Hans/mynt-eye-s-sdk-apidoc-2.3.2-zh-Hans/index.html)
 * [Guide Doc](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/releases): How to install and start using the SDK.
-  * en: [![](https://img.shields.io/badge/Download-PDF-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2764143/mynt-eye-s-sdk-guide-2.3.0-en.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2764145/mynt-eye-s-sdk-guide-2.3.0-en.zip) [![](https://img.shields.io/badge/Online-HTML-blue.svg?style=flat)](https://slightech.github.io/MYNT-EYE-S-SDK-Guide/)
+  * en: [![](https://img.shields.io/badge/Download-PDF-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2913052/mynt-eye-s-sdk-guide-2.3.2-en.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2913053/mynt-eye-s-sdk-guide-2.3.2-en.zip) [![](https://img.shields.io/badge/Online-HTML-blue.svg?style=flat)](https://slightech.github.io/MYNT-EYE-S-SDK-Guide/)
-  * zh-Hans: [![](https://img.shields.io/badge/Download-PDF-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2764150/mynt-eye-s-sdk-guide-2.3.0-zh-Hans.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2764163/mynt-eye-s-sdk-guide-2.3.0-zh-Hans.zip) [![](https://img.shields.io/badge/Online-HTML-blue.svg?style=flat)](http://doc.myntai.com/resource/sdk/mynt-eye-s-sdk-guide-2.3.0-zh-Hans/mynt-eye-s-sdk-guide-2.3.0-zh-Hans/index.html)
+  * zh-Hans: [![](https://img.shields.io/badge/Download-PDF-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2913054/mynt-eye-s-sdk-guide-2.3.2-zh-Hans.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2913056/mynt-eye-s-sdk-guide-2.3.2-zh-Hans.zip) [![](https://img.shields.io/badge/Online-HTML-blue.svg?style=flat)](http://doc.myntai.com/resource/sdk/mynt-eye-s-sdk-guide-2.3.2-zh-Hans/mynt-eye-s-sdk-guide-2.3.2-zh-Hans/index.html)
 
 > Supported languages: `en`, `zh-Hans`.
 

doc/en/api.doxyfile

@@ -38,7 +38,7 @@ PROJECT_NAME           = "MYNT EYE S SDK"
 # could be handy for archiving the generated documentation or if some version
 # control system is used.
 
-PROJECT_NUMBER         = 2.3.0
+PROJECT_NUMBER         = 2.3.2
 
 # Using the PROJECT_BRIEF tag one can provide an optional one line description
 # for a project that appears at the top of each page and should give viewer a

doc/en/spec_hardware_info.md

@@ -12,3 +12,5 @@
 | Lens type | lens_type | - | × | √ Get/Set | 4 | vendor(2),product(2); default: 0 |
 | IMU type | imu_type | - | × | √ Get/Set | 4 | vendor(2),product(2); default: 0 |
 | Nominal baseline | nominal_baseline | - | × | √ Get/Set | 2 | unit: mm; default: 0 |
+| Auxiliary chip version | auxiliary_chip_version | - | × | √ Get | 2 | major,minor |
+| isp version | isp_version | - | × | √ Get | 2 | major,minor |

doc/zh-Hans/api.doxyfile

@@ -38,7 +38,7 @@ PROJECT_NAME           = "MYNT EYE S SDK"
 # could be handy for archiving the generated documentation or if some version
 # control system is used.
 
-PROJECT_NUMBER         = 2.3.0
+PROJECT_NUMBER         = 2.3.2
 
 # Using the PROJECT_BRIEF tag one can provide an optional one line description
 # for a project that appears at the top of each page and should give viewer a

doc/zh-Hans/spec_hardware_info.md

@@ -12,6 +12,8 @@
 | 镜头类型 | lens_type | - | × | √ Get/Set | 4 | vendor(2),product(2) ，未 Set 默认 0 |
 | IMU 类型 | imu_type | - | × | √ Get/Set | 4 | vendor(2),product(2) ，未 Set 默认 0 |
 | 基线长度 | nominal_baseline | - | × | √ Get/Set | 2 | 单位 mm ，未 set 默认 0 |
+| 辅助芯片版本 | auxiliary_chip_version | - | × | √ Get | 2 | major,minor |
+| ISP版本 | isp_version | - | × | √ Get | 2 | major,minor |
 
 * 描述符获取：指通用 USB 设备信息，可用工具查看。
 * 拓展通道获取：指通过拓展通道（UVC Extension Unit）问硬件获取到的信息，需要读取。

include/mynteye/api/api.h

@@ -30,6 +30,7 @@ MYNTEYE_BEGIN_NAMESPACE
 
 struct DeviceInfo;
 
+class Correspondence;
 class Device;
 class Synthetic;
 
@@ -91,6 +92,8 @@ class MYNTEYE_API API {
   using stream_callback_t = std::function<void(const api::StreamData &data)>;
   /** The api::MotionData callback. */
   using motion_callback_t = std::function<void(const api::MotionData &data)>;
+  /** The enable/disable switch callback. */
+  using stream_switch_callback_t = std::function<void(const Stream &stream)>;
 
   explicit API(std::shared_ptr<Device> device, CalibrationModel calib_model);
   virtual ~API();
@@ -280,11 +283,31 @@ class MYNTEYE_API API {
    * still support this stream.
    */
   void EnableStreamData(const Stream &stream);
+
+  /**
+   * Enable the data of stream.
+   * callback function will call before the father processor enable.
+   * when try_tag is true, the function will do nothing except callback. 
+   */
+  void EnableStreamData(
+      const Stream &stream,
+      stream_switch_callback_t callback,
+      bool try_tag = false);
   /**
    * Disable the data of stream.
    */
   void DisableStreamData(const Stream &stream);
 
+  /**
+   * Disable the data of stream.
+   * callback function will call before the children processor disable.
+   * when try_tag is true, the function will do nothing except callback. 
+   */
+  void DisableStreamData(
+      const Stream &stream,
+      stream_switch_callback_t callback,
+      bool try_tag = false);
+
   /**
    * Get the latest data of stream.
    */
@@ -305,6 +328,11 @@ class MYNTEYE_API API {
    */
   std::vector<api::MotionData> GetMotionDatas();
 
+  /**
+   * Enable motion datas with timestamp correspondence of some stream.
+   */
+  void EnableTimestampCorrespondence(const Stream &stream);
+
   /**
    * Enable the plugin.
    */
@@ -317,6 +345,10 @@ class MYNTEYE_API API {
 
   std::unique_ptr<Synthetic> synthetic_;
 
+  std::unique_ptr<Correspondence> correspondence_;
+
+  motion_callback_t callback_;
+
   void CheckImageParams();
 };
 

include/mynteye/device/device.h

@@ -278,6 +278,10 @@ class MYNTEYE_API Device {
    */
   std::vector<device::StreamData> GetStreamDatas(const Stream &stream);
 
+  /**
+   * Disable cache motion datas.
+   */
+  void DisableMotionDatas();
   /**
    * Enable cache motion datas.
    */

include/mynteye/device/types.h

@@ -162,6 +162,8 @@ struct MYNTEYE_API DeviceInfo {
   Type lens_type;
   Type imu_type;
   std::uint16_t nominal_baseline;
+  Version auxiliary_chip_version;
+  Version isp_version;
 };
 
 #undef MYNTEYE_PROPERTY

include/mynteye/types.h

@@ -122,6 +122,12 @@ enum class Info : std::uint8_t {
   IMU_TYPE,
   /** Nominal baseline */
   NOMINAL_BASELINE,
+  /** Auxiliary chip version */
+  AUXILIARY_CHIP_VERSION,
+  /** Isp version */
+  ISP_VERSION,
+  /** SDK version*/
+  SDK_VERSION,
   /** Last guard */
   LAST
 };

samples/tutorials/CMakeLists.txt

@@ -105,6 +105,10 @@ if(PCL_FOUND)
     WITH_OPENCV WITH_PCL
   )
 endif()
+make_executable2(get_imu_correspondence
+  SRCS data/get_imu_correspondence.cc util/cv_painter.cc
+  WITH_OPENCV
+)
 make_executable2(get_imu SRCS data/get_imu.cc util/cv_painter.cc WITH_OPENCV)
 make_executable2(get_from_callbacks
   SRCS data/get_from_callbacks.cc util/cv_painter.cc

samples/tutorials/control/framerate.cc

@@ -34,10 +34,6 @@ int main(int argc, char *argv[]) {
 
   // Set frame rate options for s1030
   if (model == Model::STANDARD) {
-    // Attention: must set FRAME_RATE and IMU_FREQUENCY together,
-    // otherwise won't.
-    // succeed.
-
     // FRAME_RATE values: 10, 15, 20, 25, 30, 35, 40, 45, 50, 55, 60
     api->SetOptionValue(Option::FRAME_RATE, 25);
     // IMU_FREQUENCY values: 100, 200, 250, 333, 500

samples/tutorials/data/get_device_info.cc

@@ -28,6 +28,9 @@ int main(int argc, char *argv[]) {
   LOG(INFO) << "Lens type: " << api->GetInfo(Info::LENS_TYPE);
   LOG(INFO) << "IMU type: " << api->GetInfo(Info::IMU_TYPE);
   LOG(INFO) << "Nominal baseline: " << api->GetInfo(Info::NOMINAL_BASELINE);
+  LOG(INFO) << "Auxiliary chip version: "
+            << api->GetInfo(Info::AUXILIARY_CHIP_VERSION);
+  LOG(INFO) << "Nominal baseline: " << api->GetInfo(Info::ISP_VERSION);
 
   return 0;
 }

samples/tutorials/data/get_imu_correspondence.cc

@@ -0,0 +1,85 @@
+// 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 <opencv2/highgui/highgui.hpp>
+
+#include "mynteye/logger.h"
+#include "mynteye/api/api.h"
+
+#include "util/cv_painter.h"
+
+MYNTEYE_USE_NAMESPACE
+
+int main(int argc, char *argv[]) {
+  auto &&api = API::Create(argc, argv);
+  if (!api) return 1;
+
+  bool ok;
+  auto &&request = api->SelectStreamRequest(&ok);
+  if (!ok) return 1;
+  api->ConfigStreamRequest(request);
+
+  // Enable motion datas with timestamp correspondence of some stream
+  api->EnableTimestampCorrespondence(Stream::LEFT);
+
+  api->Start(Source::ALL);
+
+  CVPainter painter;
+
+  cv::namedWindow("frame");
+
+  std::uint64_t prev_img_stamp = 0;
+  std::uint64_t prev_imu_stamp = 0;
+  while (true) {
+    api->WaitForStreams();
+
+    auto &&left_data = api->GetStreamData(Stream::LEFT);
+    auto &&right_data = api->GetStreamData(Stream::RIGHT);
+
+    auto img_stamp = left_data.img->timestamp;
+    LOG(INFO) << "Img timestamp: " << img_stamp
+        << ", diff_prev=" << (img_stamp - prev_img_stamp);
+    prev_img_stamp = img_stamp;
+
+    cv::Mat img;
+    cv::hconcat(left_data.frame, right_data.frame, img);
+
+    auto &&motion_datas = api->GetMotionDatas();
+    LOG(INFO) << "Imu count: " << motion_datas.size();
+    for (auto &&data : motion_datas) {
+      auto imu_stamp = data.imu->timestamp;
+      LOG(INFO) << "Imu timestamp: " << imu_stamp
+          << ", diff_prev=" << (imu_stamp - prev_imu_stamp)
+          << ", diff_img=" << (1.f + imu_stamp - img_stamp);
+      prev_imu_stamp = imu_stamp;
+    }
+    LOG(INFO);
+
+    /*
+    painter.DrawImgData(img, *left_data.img);
+    if (!motion_datas.empty()) {
+      painter.DrawImuData(img, *motion_datas[0].imu);
+    }
+    */
+
+    cv::imshow("frame", img);
+
+    char key = static_cast<char>(cv::waitKey(1));
+    if (key == 27 || key == 'q' || key == 'Q') {  // ESC/Q
+      break;
+    }
+  }
+
+  api->Stop(Source::ALL);
+  return 0;
+}

src/mynteye/api/api.cc

@@ -22,9 +22,11 @@
 #include <thread>
 
 #include "mynteye/logger.h"
+#include "mynteye/api/correspondence.h"
 #include "mynteye/api/dl.h"
 #include "mynteye/api/plugin.h"
 #include "mynteye/api/synthetic.h"
+#include "mynteye/api/version_checker.h"
 #include "mynteye/device/device.h"
 #include "mynteye/device/utils.h"
 
@@ -208,7 +210,7 @@ std::vector<std::string> get_plugin_paths() {
 }  // namespace
 
 API::API(std::shared_ptr<Device> device, CalibrationModel calib_model)
-    : device_(device) {
+    : device_(device), correspondence_(nullptr) {
   VLOG(2) << __func__;
   // std::dynamic_pointer_cast<StandardDevice>(device_);
   synthetic_.reset(new Synthetic(this, calib_model));
@@ -221,7 +223,10 @@ API::~API() {
 std::shared_ptr<API> API::Create(int argc, char *argv[]) {
   auto &&device = device::select();
   if (!device) return nullptr;
-  return Create(argc, argv, device);
+  auto api = Create(argc, argv, device);
+  if (api && checkFirmwareVersion(api))
+    return api;
+  return nullptr;
 }
 
 std::shared_ptr<API> API::Create(
@@ -260,7 +265,7 @@ std::shared_ptr<API> API::Create(const std::shared_ptr<Device> &device) {
     }
   } else {
     LOG(ERROR) <<"no device!";
-    api = std::make_shared<API>(device, CalibrationModel::UNKNOW);
+    return nullptr;
   }
   return api;
 }
@@ -323,6 +328,20 @@ std::shared_ptr<DeviceInfo> API::GetInfo() const {
 }
 
 std::string API::GetInfo(const Info &info) const {
+  if (info == Info::SDK_VERSION) {
+    std::string info_path =
+        utils::get_sdk_install_dir();
+    info_path.append(MYNTEYE_OS_SEP "share" \
+        MYNTEYE_OS_SEP "mynteye" MYNTEYE_OS_SEP "build.info");
+
+    cv::FileStorage fs(info_path, cv::FileStorage::READ);
+    if (!fs.isOpened()) {
+      LOG(WARNING) << "build.info not found: " << info_path;
+      return "null";
+    }
+    return fs["MYNTEYE_VERSION"];
+  }
+
   return device_->GetInfo(info);
 }
 
@@ -377,10 +396,15 @@ void API::SetStreamCallback(const Stream &stream, stream_callback_t callback) {
 }
 
 void API::SetMotionCallback(motion_callback_t callback) {
-  static auto callback_ = callback;
+  if (correspondence_) {
+    correspondence_->SetMotionCallback(callback);
+    return;
+  }
+  callback_ = callback;
   if (callback_) {
-    device_->SetMotionCallback(
+    device_->SetMotionCallback([this](const device::MotionData &data) {
-        [](const device::MotionData &data) { callback_({data.imu}); }, true);
+      callback_({data.imu});
+    }, true);
   } else {
     device_->SetMotionCallback(nullptr);
   }
@@ -435,7 +459,11 @@ void API::Stop(const Source &source) {
 }
 
 void API::WaitForStreams() {
-  synthetic_->WaitForStreams();
+  if (correspondence_) {
+    correspondence_->WaitForStreams();
+  } else {
+    synthetic_->WaitForStreams();
+  }
 }
 
 void API::EnableStreamData(const Stream &stream) {
@@ -446,24 +474,69 @@ void API::DisableStreamData(const Stream &stream) {
   synthetic_->DisableStreamData(stream);
 }
 
+void API::EnableStreamData(
+    const Stream &stream, stream_switch_callback_t callback,
+    bool try_tag) {
+  synthetic_->EnableStreamData(stream, callback, try_tag);
+}
+void API::DisableStreamData(
+    const Stream &stream, stream_switch_callback_t callback,
+    bool try_tag) {
+  synthetic_->DisableStreamData(stream, callback, try_tag);
+}
+
 api::StreamData API::GetStreamData(const Stream &stream) {
-  return synthetic_->GetStreamData(stream);
+  if (correspondence_ && correspondence_->Watch(stream)) {
+    return correspondence_->GetStreamData(stream);
+  } else {
+    return synthetic_->GetStreamData(stream);
+  }
 }
 
 std::vector<api::StreamData> API::GetStreamDatas(const Stream &stream) {
-  return synthetic_->GetStreamDatas(stream);
+  if (correspondence_ && correspondence_->Watch(stream)) {
+    return correspondence_->GetStreamDatas(stream);
+  } else {
+    return synthetic_->GetStreamDatas(stream);
+  }
 }
 
 void API::EnableMotionDatas(std::size_t max_size) {
+  if (correspondence_) return;  // not cache them
   device_->EnableMotionDatas(max_size);
 }
 
 std::vector<api::MotionData> API::GetMotionDatas() {
-  std::vector<api::MotionData> datas;
+  if (correspondence_) {
-  for (auto &&data : device_->GetMotionDatas()) {
+    return correspondence_->GetMotionDatas();
-    datas.push_back({data.imu});
+  } else {
+    std::vector<api::MotionData> datas;
+    for (auto &&data : device_->GetMotionDatas()) {
+      datas.push_back({data.imu});
+    }
+    return datas;
+  }
+}
+
+void API::EnableTimestampCorrespondence(const Stream &stream) {
+  if (correspondence_ == nullptr) {
+    correspondence_.reset(new Correspondence(device_, stream));
+    {
+      device_->DisableMotionDatas();
+      if (callback_) {
+        correspondence_->SetMotionCallback(callback_);
+        callback_ = nullptr;
+      }
+    }
+    using namespace std::placeholders;  // NOLINT
+    device_->SetMotionCallback(
+        std::bind(&Correspondence::OnMotionDataCallback,
+            correspondence_.get(), _1),
+        true);
+    synthetic_->SetStreamDataListener(
+        std::bind(&Correspondence::OnStreamDataCallback,
+            correspondence_.get(), _1, _2));
   }
-  return datas;
 }
 
 void API::EnablePlugin(const std::string &path) {

src/mynteye/api/correspondence.cc

@@ -0,0 +1,277 @@
+// 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/api/correspondence.h"
+
+#include "mynteye/device/device.h"
+#include "mynteye/logger.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+Correspondence::Correspondence(const std::shared_ptr<Device> &device,
+    const Stream &stream)
+  : device_(device), stream_(stream), ready_image_timestamp_(0) {
+  VLOG(2) << __func__;
+  // set matched stream to be watched too,
+  // aim to make stream and matched stream correspondence
+  if (stream_ == Stream::LEFT) {
+    stream_match_ = Stream::RIGHT;
+  } else if (stream_ == Stream::RIGHT) {
+    stream_match_ = Stream::LEFT;
+  } else if (stream_ == Stream::LEFT_RECTIFIED) {
+    stream_match_ = Stream::RIGHT_RECTIFIED;
+  } else if (stream_ == Stream::RIGHT_RECTIFIED) {
+    stream_match_ = Stream::LEFT_RECTIFIED;
+  } else {
+    stream_match_ = Stream::LAST;
+  }
+  EnableStreamMatch();
+
+  auto framerate = device_->GetOptionValue(Option::FRAME_RATE);
+  stream_interval_us_ = 1000000.f / framerate;
+  stream_interval_us_half_ = 0.5f * stream_interval_us_;
+  VLOG(2) << "framerate: " << framerate
+      << ", interval_us: " << stream_interval_us_;
+}
+
+Correspondence::~Correspondence() {
+  VLOG(2) << __func__;
+}
+
+bool Correspondence::Watch(const Stream &stream) const {
+  if (stream == stream_) return true;
+  if (stream_match_enabled_ && stream == stream_match_) return true;
+  return false;
+}
+
+void Correspondence::OnStreamDataCallback(
+    const Stream &stream, const api::StreamData &data) {
+  if (!Watch(stream)) {
+    return;  // unwatched
+  }
+  // LOG(INFO) << __func__ << ", " << stream
+  //     << ", id: " << data.frame_id << ", stamp: " << data.img->timestamp;
+  // if (data.img == nullptr) {
+  //   LOG(FATAL) << "stream data image info is empty!";
+  // }
+  std::lock_guard<std::recursive_mutex> _(mtx_stream_datas_);
+  if (stream == stream_) {
+    stream_datas_.push_back(std::move(data));
+  } else if (/*stream_match_enabled_ && */stream == stream_match_) {
+    stream_datas_match_.push_back(std::move(data));
+  }
+  NotifyStreamDataReady();
+}
+
+void Correspondence::OnMotionDataCallback(const device::MotionData &data) {
+  // LOG(INFO) << __func__ << ", id: " << data.imu->frame_id
+  //     << ", stamp: " << data.imu->timestamp;
+  {
+    std::lock_guard<std::recursive_mutex> _(mtx_motion_datas_);
+    motion_datas_.push_back(data);
+  }
+  if (motion_callback_) {
+    motion_callback_({data.imu});
+  }
+}
+
+void Correspondence::SetMotionCallback(API::motion_callback_t callback) {
+  // LOG(INFO) << __func__;
+  motion_callback_ = callback;
+}
+
+void Correspondence::WaitForStreams() {
+  if (stream_ == Stream::LEFT || stream_ == Stream::RIGHT) {
+    // Wait native stream ready, avoid get these stream empty
+    // Todo: determine native stream according to device
+    WaitStreamDataReady();
+    return;
+  }
+  device_->WaitForStreams();
+}
+
+api::StreamData Correspondence::GetStreamData(const Stream &stream) {
+  auto datas = GetStreamDatas(stream);
+  return datas.empty() ? api::StreamData{} : datas.back();
+}
+
+std::vector<api::StreamData> Correspondence::GetStreamDatas(
+    const Stream &stream) {
+  if (!Watch(stream)) {
+    LOG(ERROR) << "Get unwatched stream data of " << stream;
+    return {};
+  }
+
+  std::lock_guard<std::recursive_mutex> _(mtx_stream_datas_);
+  static std::uint32_t stream_count_ = 0;
+  static std::uint32_t stream_match_count_ = 0;
+
+  if (stream == stream_) {
+    auto datas = GetReadyStreamData(false);
+
+    if (stream_count_ < 10) {
+      ++stream_count_;
+    } else {
+      // get stream, but not get matched stream, disable it
+      if (stream_match_count_ == 0) {
+        DisableStreamMatch();
+      }
+    }
+
+    return datas;
+  } else if (/*stream_match_enabled_ && */stream == stream_match_) {
+    auto datas = GetReadyStreamData(true);
+
+    if (stream_match_count_ < 10) {
+      ++stream_match_count_;
+    }
+
+    return datas;
+  }
+
+  return {};
+}
+
+std::vector<api::MotionData> Correspondence::GetMotionDatas() {
+  return GetReadyMotionDatas();
+}
+
+void Correspondence::EnableStreamMatch() {
+  stream_match_enabled_ = true;
+}
+
+void Correspondence::DisableStreamMatch() {
+  stream_match_enabled_ = false;
+  stream_datas_match_.clear();
+}
+
+void Correspondence::WaitStreamDataReady() {
+  std::unique_lock<std::recursive_mutex> lock(mtx_stream_datas_);
+  auto ready = std::bind(&Correspondence::IsStreamDataReady, this);
+  bool ok = cond_stream_datas_.wait_for(lock, std::chrono::seconds(3), ready);
+  if (!ok) {
+    LOG(FATAL) << "Timeout waiting for key frames. Please use USB 3.0, and not "
+                  "in virtual machine.";
+  }
+}
+
+void Correspondence::NotifyStreamDataReady() {
+  cond_stream_datas_.notify_one();
+}
+
+bool Correspondence::IsStreamDataReady() {
+  if (stream_datas_.empty()) return false;
+  if (stream_match_enabled_) {
+    if (stream_datas_match_.empty()) return false;
+  }
+  if (motion_datas_.empty()) return false;
+
+  std::uint64_t img_stamp = 0;
+  std::uint64_t img_macth_stamp = 0;
+  {
+    std::lock_guard<std::recursive_mutex> _(mtx_stream_datas_);
+    auto data = stream_datas_.front();
+    if (data.img == nullptr) {
+      LOG(FATAL) << "stream data image info is empty!";
+    }
+    img_stamp = data.img->timestamp;
+
+    if (stream_match_enabled_) {
+      img_macth_stamp = stream_datas_match_.front().img->timestamp;
+    }
+  }
+  std::uint64_t imu_stamp = 0;
+  {
+    std::lock_guard<std::recursive_mutex> _(mtx_motion_datas_);
+    auto data = motion_datas_.back();
+    if (data.imu == nullptr) {
+      LOG(FATAL) << "motion data imu info is empty!";
+    }
+    imu_stamp = data.imu->timestamp;
+  }
+
+  if (stream_match_enabled_) {
+    return img_stamp + stream_interval_us_half_ < imu_stamp
+        && img_macth_stamp + stream_interval_us_half_ < imu_stamp;
+  } else {
+    return img_stamp + stream_interval_us_half_ < imu_stamp;
+  }
+}
+
+std::vector<api::StreamData> Correspondence::GetReadyStreamData(bool matched) {
+  std::uint64_t imu_stamp = 0;
+  {
+    std::lock_guard<std::recursive_mutex> _(mtx_motion_datas_);
+    if (motion_datas_.empty()) {
+      LOG(WARNING) << "motion data is unexpected empty!"
+          "\n\n  Please ensure Start(Source::MOTION_TRACKING) "
+          "or Start(Source::ALL)\n";
+      std::lock_guard<std::recursive_mutex> _(mtx_stream_datas_);
+      return std::move(matched ? stream_datas_match_ : stream_datas_);
+    }
+    imu_stamp = motion_datas_.back().imu->timestamp;
+  }
+  std::lock_guard<std::recursive_mutex> _(mtx_stream_datas_);
+
+  std::vector<api::StreamData> &datas =
+      matched ? stream_datas_match_ : stream_datas_;
+
+  // LOG(INFO) << "datas.size: " << datas.size() << ", matched: " << matched;
+  std::vector<api::StreamData> result;
+
+  for (auto it = datas.begin(); it != datas.end(); ) {
+    // LOG(INFO) << "data.id: " << it->frame_id;
+    auto img_stamp = it->img->timestamp;
+    if (img_stamp + stream_interval_us_half_ < imu_stamp) {
+      // LOG(INFO) << "data.id: " << it->frame_id << " > result";
+      result.push_back(std::move(*it));
+      it = datas.erase(it);
+    } else {
+      // ++it;
+      break;
+    }
+  }
+  // LOG(INFO) << "datas.size: " << datas.size()
+  //     << ", result.size: " << result.size();
+
+  if (!matched && !result.empty()) {
+    // last match stream timestamp
+    ready_image_timestamp_ = result.back().img->timestamp;
+  }
+  return result;
+}
+
+std::vector<api::MotionData> Correspondence::GetReadyMotionDatas() {
+  if (ready_image_timestamp_ == 0) return {};
+  std::lock_guard<std::recursive_mutex> _(mtx_motion_datas_);
+
+  std::vector<api::MotionData> result;
+
+  auto &&datas = motion_datas_;
+  for (auto it = datas.begin(); it != datas.end(); ) {
+    auto imu_stamp = it->imu->timestamp;
+    if (imu_stamp < ready_image_timestamp_ - stream_interval_us_half_) {
+      it = datas.erase(it);
+    } else if (imu_stamp > ready_image_timestamp_ + stream_interval_us_half_) {
+      // ++it;
+      break;
+    } else {
+      result.push_back({it->imu});
+      it = datas.erase(it);
+    }
+  }
+
+  return result;
+}
+
+MYNTEYE_END_NAMESPACE

src/mynteye/api/correspondence.h

@@ -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.
+#ifndef MYNTEYE_API_CONFIG_H_
+#define MYNTEYE_API_CONFIG_H_
+#pragma once
+
+#include <atomic>
+#include <condition_variable>
+#include <memory>
+#include <mutex>
+#include <vector>
+
+#include "mynteye/api/api.h"
+#include "mynteye/device/callbacks.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+class Correspondence {
+ public:
+  Correspondence(const std::shared_ptr<Device> &device, const Stream &stream);
+  ~Correspondence();
+
+  bool Watch(const Stream &stream) const;
+
+  void OnStreamDataCallback(const Stream &stream, const api::StreamData &data);
+  void OnMotionDataCallback(const device::MotionData &data);
+
+  void SetMotionCallback(API::motion_callback_t callback);
+
+  void WaitForStreams();
+  api::StreamData GetStreamData(const Stream &stream);
+  std::vector<api::StreamData> GetStreamDatas(const Stream &stream);
+  std::vector<api::MotionData> GetMotionDatas();
+
+ private:
+  void EnableStreamMatch();
+  void DisableStreamMatch();
+
+  void WaitStreamDataReady();
+  void NotifyStreamDataReady();
+
+  bool IsStreamDataReady();
+
+  std::vector<api::StreamData> GetReadyStreamData(bool matched);
+  std::vector<api::MotionData> GetReadyMotionDatas();
+
+  std::shared_ptr<Device> device_;
+  Stream stream_;
+  Stream stream_match_;
+  std::atomic_bool stream_match_enabled_;
+
+  float stream_interval_us_;
+  float stream_interval_us_half_;
+
+  API::motion_callback_t motion_callback_;
+  std::vector<device::MotionData> motion_datas_;
+  std::recursive_mutex mtx_motion_datas_;
+
+  std::vector<api::StreamData> stream_datas_;
+  std::vector<api::StreamData> stream_datas_match_;
+  std::recursive_mutex mtx_stream_datas_;
+  std::condition_variable_any cond_stream_datas_;
+
+  std::uint64_t ready_image_timestamp_;
+};
+
+MYNTEYE_END_NAMESPACE
+
+#endif  // MYNTEYE_API_CONFIG_H_

src/mynteye/api/synthetic.cc

@@ -74,6 +74,44 @@ void process_childs(
   }
 }
 
+// ObjMat/ObjMat2 > api::StreamData
+
+api::StreamData obj_data_first(const ObjMat2 *obj) {
+  return {obj->first_data, obj->first, nullptr, obj->first_id};
+}
+
+api::StreamData obj_data_second(const ObjMat2 *obj) {
+  return {obj->second_data, obj->second, nullptr, obj->second_id};
+}
+
+api::StreamData obj_data(const ObjMat *obj) {
+  return {obj->data, obj->value, nullptr, obj->id};
+}
+
+api::StreamData obj_data_first(const std::shared_ptr<ObjMat2> &obj) {
+  return {obj->first_data, obj->first, nullptr, obj->first_id};
+}
+
+api::StreamData obj_data_second(const std::shared_ptr<ObjMat2> &obj) {
+  return {obj->second_data, obj->second, nullptr, obj->second_id};
+}
+
+api::StreamData obj_data(const std::shared_ptr<ObjMat> &obj) {
+  return {obj->data, obj->value, nullptr, obj->id};
+}
+
+// api::StreamData > ObjMat/ObjMat2
+
+ObjMat data_obj(const api::StreamData &data) {
+  return ObjMat{data.frame, data.frame_id, data.img};
+}
+
+ObjMat2 data_obj(const api::StreamData &first, const api::StreamData &second) {
+  return ObjMat2{
+      first.frame, first.frame_id, first.img,
+      second.frame, second.frame_id, second.img};
+}
+
 }  // namespace
 
 void Synthetic::InitCalibInfo() {
@@ -105,7 +143,8 @@ Synthetic::Synthetic(API *api, CalibrationModel calib_model)
     : api_(api),
       plugin_(nullptr),
       calib_model_(calib_model),
-      calib_default_tag_(false) {
+      calib_default_tag_(false),
+      stream_data_listener_(nullptr) {
   VLOG(2) << __func__;
   CHECK_NOTNULL(api_);
   InitCalibInfo();
@@ -121,6 +160,10 @@ Synthetic::~Synthetic() {
   }
 }
 
+void Synthetic::SetStreamDataListener(stream_data_listener_t listener) {
+  stream_data_listener_ = listener;
+}
+
 void Synthetic::NotifyImageParamsChanged() {
   if (!calib_default_tag_) {
     intr_left_ = api_->GetIntrinsicsBase(Stream::LEFT);
@@ -195,27 +238,6 @@ bool Synthetic::checkControlDateWithStream(const Stream& stream) const {
   return false;
 }
 
-void Synthetic::EnableStreamData(const Stream &stream) {
-  // Activate processors of synthetic stream
-  auto processor = getProcessorWithStream(stream);
-  iterate_processor_CtoP_before(processor,
-      [](std::shared_ptr<Processor> proce){
-        auto streams = proce->getTargetStreams();
-        int act_tag = 0;
-        for (unsigned int i = 0; i < proce->getStreamsSum() ; i++) {
-          if (proce->target_streams_[i].enabled_mode_ == MODE_LAST) {
-            act_tag++;
-            proce->target_streams_[i].enabled_mode_ = MODE_SYNTHETIC;
-          }
-        }
-        if (act_tag > 0 && !proce->IsActivated()) {
-          // std::cout << proce->Name() << " Active now" << std::endl;
-          proce->Activate();
-        }
-      });
-}
-
-
 bool Synthetic::Supports(const Stream &stream) const {
   return checkControlDateWithStream(stream);
 }
@@ -228,16 +250,45 @@ Synthetic::mode_t Synthetic::SupportsMode(const Stream &stream) const {
   return MODE_LAST;
 }
 
-void Synthetic::DisableStreamData(const Stream &stream) {
+void Synthetic::EnableStreamData(
+    const Stream &stream, stream_switch_callback_t callback,
+    bool try_tag) {
+  // Activate processors of synthetic stream
+  auto processor = getProcessorWithStream(stream);
+  iterate_processor_CtoP_before(processor,
+      [callback, try_tag](std::shared_ptr<Processor> proce){
+        auto streams = proce->getTargetStreams();
+        int act_tag = 0;
+        for (unsigned int i = 0; i < proce->getStreamsSum() ; i++) {
+          if (proce->target_streams_[i].enabled_mode_ == MODE_LAST) {
+            callback(proce->target_streams_[i].stream);
+            if (!try_tag) {
+              act_tag++;
+              proce->target_streams_[i].enabled_mode_ = MODE_SYNTHETIC;
+            }
+          }
+        }
+        if (act_tag > 0 && !proce->IsActivated()) {
+          // std::cout << proce->Name() << " Active now" << std::endl;
+          proce->Activate();
+        }
+      });
+}
+void Synthetic::DisableStreamData(
+    const Stream &stream, stream_switch_callback_t callback,
+    bool try_tag) {
   auto processor = getProcessorWithStream(stream);
   iterate_processor_PtoC_before(processor,
-      [](std::shared_ptr<Processor> proce){
+      [callback, try_tag](std::shared_ptr<Processor> proce){
         auto streams = proce->getTargetStreams();
         int act_tag = 0;
         for (unsigned int i = 0; i < proce->getStreamsSum() ; i++) {
           if (proce->target_streams_[i].enabled_mode_ == MODE_SYNTHETIC) {
-            act_tag++;
+            callback(proce->target_streams_[i].stream);
-            proce->target_streams_[i].enabled_mode_ = MODE_LAST;
+            if (!try_tag) {
+              act_tag++;
+              proce->target_streams_[i].enabled_mode_ = MODE_LAST;
+            }
           }
         }
         if (act_tag > 0 && proce->IsActivated()) {
@@ -247,6 +298,20 @@ void Synthetic::DisableStreamData(const Stream &stream) {
       });
 }
 
+void Synthetic::EnableStreamData(const Stream &stream) {
+  EnableStreamData(stream, [](const Stream &stream){
+        // std::cout << stream << "enabled in callback" << std::endl;
+        MYNTEYE_UNUSED(stream);
+      }, false);
+}
+
+void Synthetic::DisableStreamData(const Stream &stream) {
+  DisableStreamData(stream, [](const Stream &stream){
+        // std::cout << stream << "disabled in callback" << std::endl;
+        MYNTEYE_UNUSED(stream);
+      }, false);
+}
+
 bool Synthetic::IsStreamDataEnabled(const Stream &stream) const {
   if (checkControlDateWithStream(stream)) {
     auto data = getControlDateWithStream(stream);
@@ -335,7 +400,7 @@ api::StreamData Synthetic::GetStreamData(const Stream &stream) {
       if (out != nullptr) {
         auto &&output = Object::Cast<ObjMat>(out);
         if (output != nullptr) {
-          return {output->data, output->value, nullptr, output->id};
+          return obj_data(output);
         }
         VLOG(2) << "Rectify not ready now";
       }
@@ -349,15 +414,9 @@ api::StreamData Synthetic::GetStreamData(const Stream &stream) {
         for (auto it : streams) {
           if (it.stream == stream) {
             if (num == 1) {
-              return {output->first_data,
+              return obj_data_first(output);
-                  output->first,
-                  nullptr,
-                  output->first_id};
             } else {
-              return {output->second_data,
+              return obj_data_second(output);
-                  output->second,
-                  nullptr,
-                  output->second_id};
             }
           }
           num++;
@@ -452,66 +511,60 @@ bool Synthetic::IsStreamEnabledSynthetic(const Stream &stream) const {
 
 void Synthetic::InitProcessors() {
   std::shared_ptr<Processor> rectify_processor = nullptr;
-#ifdef WITH_CAM_MODELS
+  std::shared_ptr<Processor> points_processor = nullptr;
-  std::shared_ptr<RectifyProcessor> rectify_processor_imp = nullptr;
+  std::shared_ptr<Processor> depth_processor = nullptr;
-#endif
+
-  cv::Mat Q;
-  if (calib_model_ ==  CalibrationModel::PINHOLE) {
-    auto &&rectify_processor_ocv =
-        std::make_shared<RectifyProcessorOCV>(intr_left_, intr_right_, extr_,
-                                              RECTIFY_PROC_PERIOD);
-    Q = rectify_processor_ocv->Q;
-    rectify_processor = rectify_processor_ocv;
-#ifdef WITH_CAM_MODELS
-  } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) {
-    rectify_processor_imp =
-        std::make_shared<RectifyProcessor>(intr_left_, intr_right_, extr_,
-                                           RECTIFY_PROC_PERIOD);
-    rectify_processor = rectify_processor_imp;
-#endif
-  } else {
-    LOG(ERROR) << "Unknow calib model type in device: "
-              << calib_model_ << ", use default pinhole model";
-    auto &&rectify_processor_ocv =
-        std::make_shared<RectifyProcessorOCV>(intr_left_, intr_right_, extr_,
-                                              RECTIFY_PROC_PERIOD);
-    rectify_processor = rectify_processor_ocv;
-  }
   auto &&disparity_processor =
       std::make_shared<DisparityProcessor>(DisparityComputingMethod::SGBM,
                                            DISPARITY_PROC_PERIOD);
   auto &&disparitynormalized_processor =
       std::make_shared<DisparityNormalizedProcessor>(
           DISPARITY_NORM_PROC_PERIOD);
-  std::shared_ptr<Processor> points_processor = nullptr;
+
-  if (calib_model_ ==  CalibrationModel::PINHOLE) {
+  auto root_processor =
+        std::make_shared<RootProcessor>(ROOT_PROC_PERIOD);
+
+  if (calib_model_ == CalibrationModel::PINHOLE) {
+    // PINHOLE
+    auto &&rectify_processor_ocv =
+        std::make_shared<RectifyProcessorOCV>(intr_left_, intr_right_, extr_,
+                                              RECTIFY_PROC_PERIOD);
+    rectify_processor = rectify_processor_ocv;
     points_processor = std::make_shared<PointsProcessorOCV>(
-        Q, POINTS_PROC_PERIOD);
+        rectify_processor_ocv->Q, POINTS_PROC_PERIOD);
+    depth_processor = std::make_shared<DepthProcessorOCV>(DEPTH_PROC_PERIOD);
+
+    root_processor->AddChild(rectify_processor);
+    rectify_processor->AddChild(disparity_processor);
+    disparity_processor->AddChild(disparitynormalized_processor);
+    disparity_processor->AddChild(points_processor);
+    points_processor->AddChild(depth_processor);
 #ifdef WITH_CAM_MODELS
   } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) {
+    // KANNALA_BRANDT
+    auto rectify_processor_imp =
+        std::make_shared<RectifyProcessor>(intr_left_, intr_right_, extr_,
+                                           RECTIFY_PROC_PERIOD);
+    rectify_processor = rectify_processor_imp;
     points_processor = std::make_shared<PointsProcessor>(
         rectify_processor_imp -> getCalibInfoPair(),
         POINTS_PROC_PERIOD);
-#endif
-  } else {
-    points_processor = std::make_shared<PointsProcessorOCV>(
-        Q, POINTS_PROC_PERIOD);
-  }
-  std::shared_ptr<Processor> depth_processor = nullptr;
-  if (calib_model_ ==  CalibrationModel::PINHOLE) {
-    depth_processor = std::make_shared<DepthProcessorOCV>(DEPTH_PROC_PERIOD);
-#ifdef WITH_CAM_MODELS
-  } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) {
     depth_processor = std::make_shared<DepthProcessor>(
         rectify_processor_imp -> getCalibInfoPair(),
         DEPTH_PROC_PERIOD);
+
+    root_processor->AddChild(rectify_processor);
+    rectify_processor->AddChild(disparity_processor);
+    disparity_processor->AddChild(disparitynormalized_processor);
+    disparity_processor->AddChild(depth_processor);
+    depth_processor->AddChild(points_processor);
 #endif
   } else {
-    depth_processor = std::make_shared<DepthProcessorOCV>(DEPTH_PROC_PERIOD);
+    // UNKNOW
+    LOG(ERROR) << "Unknow calib model type in device: "
+               << calib_model_;
+    return;
   }
-  auto root_processor =
-        std::make_shared<RootProcessor>(ROOT_PROC_PERIOD);
-  root_processor->AddChild(rectify_processor);
 
   rectify_processor->addTargetStreams(
       {Stream::LEFT_RECTIFIED, Mode::MODE_LAST, Mode::MODE_LAST, nullptr});
@@ -559,30 +612,14 @@ void Synthetic::InitProcessors() {
   depth_processor->SetPostProcessCallback(
       std::bind(&Synthetic::OnDepthPostProcess, this, _1));
 
-  if (calib_model_ == CalibrationModel::PINHOLE) {
+  processor_ = root_processor;
-    // PINHOLE
-    rectify_processor->AddChild(disparity_processor);
-    disparity_processor->AddChild(disparitynormalized_processor);
-    disparity_processor->AddChild(points_processor);
-    points_processor->AddChild(depth_processor);
-  } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) {
-    // KANNALA_BRANDT
-    rectify_processor->AddChild(disparity_processor);
-    disparity_processor->AddChild(disparitynormalized_processor);
-    disparity_processor->AddChild(depth_processor);
-    depth_processor->AddChild(points_processor);
-  } else {
-    // UNKNOW
-    LOG(ERROR) << "Unknow calib model type in device: "
-               << calib_model_;
-  }
-
-  processor_ = rectify_processor;
 }
 
 void Synthetic::ProcessNativeStream(
     const Stream &stream, const api::StreamData &data) {
+  NotifyStreamData(stream, data);
   if (stream == Stream::LEFT || stream == Stream::RIGHT) {
+    std::unique_lock<std::mutex> lk(mtx_left_right_ready_);
     static api::StreamData left_data, right_data;
     if (stream == Stream::LEFT) {
       left_data = data;
@@ -603,9 +640,7 @@ void Synthetic::ProcessNativeStream(
                   << calib_model_ << ", use default pinhole model";
         processor = find_processor<RectifyProcessorOCV>(processor_);
       }
-      processor->Process(ObjMat2{
+      processor->Process(data_obj(left_data, right_data));
-          left_data.frame, left_data.frame_id, left_data.img,
-          right_data.frame, right_data.frame_id, right_data.img});
     }
     return;
   }
@@ -627,34 +662,28 @@ void Synthetic::ProcessNativeStream(
         name = RectifyProcessor::NAME;
 #endif
       }
-      process_childs(
+      process_childs(processor_, name,
-          processor_, name, ObjMat2{
+          data_obj(left_rect_data, right_rect_data));
-              left_rect_data.frame, left_rect_data.frame_id, left_rect_data.img,
-              right_rect_data.frame, right_rect_data.frame_id,
-              right_rect_data.img});
     }
     return;
   }
 
   switch (stream) {
     case Stream::DISPARITY: {
-      process_childs(processor_, DisparityProcessor::NAME,
+      process_childs(processor_, DisparityProcessor::NAME, data_obj(data));
-          ObjMat{data.frame, data.frame_id, data.img});
     } break;
     case Stream::DISPARITY_NORMALIZED: {
       process_childs(processor_, DisparityNormalizedProcessor::NAME,
-          ObjMat{data.frame, data.frame_id, data.img});
+          data_obj(data));
     } break;
     case Stream::POINTS: {
       if (calib_model_ == CalibrationModel::PINHOLE) {
         // PINHOLE
-        process_childs(processor_, PointsProcessorOCV::NAME,
+        process_childs(processor_, PointsProcessorOCV::NAME, data_obj(data));
-            ObjMat{data.frame, data.frame_id, data.img});
 #ifdef WITH_CAM_MODELS
       } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) {
         // KANNALA_BRANDT
-        process_childs(processor_, PointsProcessor::NAME,
+        process_childs(processor_, PointsProcessor::NAME, data_obj(data));
-            ObjMat{data.frame, data.frame_id, data.img});
 #endif
       } else {
         // UNKNOW
@@ -665,13 +694,11 @@ void Synthetic::ProcessNativeStream(
     case Stream::DEPTH: {
       if (calib_model_ == CalibrationModel::PINHOLE) {
         // PINHOLE
-        process_childs(processor_, DepthProcessorOCV::NAME,
+        process_childs(processor_, DepthProcessorOCV::NAME, data_obj(data));
-            ObjMat{data.frame, data.frame_id, data.img});
 #ifdef WITH_CAM_MODELS
       } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) {
         // KANNALA_BRANDT
-        process_childs(processor_, DepthProcessor::NAME,
+        process_childs(processor_, DepthProcessor::NAME, data_obj(data));
-            ObjMat{data.frame, data.frame_id, data.img});
 #endif
       } else {
         // UNKNOW
@@ -737,51 +764,51 @@ bool Synthetic::OnDepthProcess(
 
 void Synthetic::OnRectifyPostProcess(Object *const out) {
   const ObjMat2 *output = Object::Cast<ObjMat2>(out);
+  NotifyStreamData(Stream::LEFT_RECTIFIED, obj_data_first(output));
+  NotifyStreamData(Stream::RIGHT_RECTIFIED, obj_data_second(output));
   if (HasStreamCallback(Stream::LEFT_RECTIFIED)) {
     auto data = getControlDateWithStream(Stream::LEFT_RECTIFIED);
-    data.stream_callback(
+    data.stream_callback(obj_data_first(output));
-        {output->first_data, output->first, nullptr, output->first_id});
   }
   if (HasStreamCallback(Stream::RIGHT_RECTIFIED)) {
     auto data = getControlDateWithStream(Stream::RIGHT_RECTIFIED);
-    data.stream_callback(
+    data.stream_callback(obj_data_second(output));
-        {output->second_data, output->second, nullptr, output->second_id});
   }
 }
 
 void Synthetic::OnDisparityPostProcess(Object *const out) {
   const ObjMat *output = Object::Cast<ObjMat>(out);
+  NotifyStreamData(Stream::DISPARITY, obj_data(output));
   if (HasStreamCallback(Stream::DISPARITY)) {
     auto data = getControlDateWithStream(Stream::DISPARITY);
-    data.stream_callback(
+    data.stream_callback(obj_data(output));
-        {output->data, output->value, nullptr, output->id});
   }
 }
 
 void Synthetic::OnDisparityNormalizedPostProcess(Object *const out) {
   const ObjMat *output = Object::Cast<ObjMat>(out);
+  NotifyStreamData(Stream::DISPARITY_NORMALIZED, obj_data(output));
   if (HasStreamCallback(Stream::DISPARITY_NORMALIZED)) {
     auto data = getControlDateWithStream(Stream::DISPARITY_NORMALIZED);
-    data.stream_callback(
+    data.stream_callback(obj_data(output));
-        {output->data, output->value, nullptr, output->id});
   }
 }
 
 void Synthetic::OnPointsPostProcess(Object *const out) {
   const ObjMat *output = Object::Cast<ObjMat>(out);
+  NotifyStreamData(Stream::POINTS, obj_data(output));
   if (HasStreamCallback(Stream::POINTS)) {
     auto data = getControlDateWithStream(Stream::POINTS);
-    data.stream_callback(
+    data.stream_callback(obj_data(output));
-        {output->data, output->value, nullptr, output->id});
   }
 }
 
 void Synthetic::OnDepthPostProcess(Object *const out) {
   const ObjMat *output = Object::Cast<ObjMat>(out);
+  NotifyStreamData(Stream::DEPTH, obj_data(output));
   if (HasStreamCallback(Stream::DEPTH)) {
     auto data = getControlDateWithStream(Stream::DEPTH);
-    data.stream_callback(
+    data.stream_callback(obj_data(output));
-        {output->data, output->value, nullptr, output->id});
   }
 }
 
@@ -796,4 +823,11 @@ void Synthetic::SetDisparityComputingMethodType(
   LOG(ERROR) << "ERROR: no suited processor for disparity computing.";
 }
 
+void Synthetic::NotifyStreamData(
+    const Stream &stream, const api::StreamData &data) {
+  if (stream_data_listener_) {
+    stream_data_listener_(stream, data);
+  }
+}
+
 MYNTEYE_END_NAMESPACE

src/mynteye/api/synthetic.h

@@ -19,6 +19,7 @@
 #include <memory>
 #include <string>
 #include <vector>
+#include <mutex>
 
 #include "mynteye/api/api.h"
 #include "mynteye/api/config.h"
@@ -34,6 +35,9 @@ struct Object;
 class Synthetic {
  public:
   using stream_callback_t = API::stream_callback_t;
+  using stream_data_listener_t =
+      std::function<void(const Stream &stream, const api::StreamData &data)>;
+  using stream_switch_callback_t = API::stream_switch_callback_t;
 
   typedef enum Mode {
     MODE_NATIVE,     // Native stream
@@ -51,6 +55,8 @@ class Synthetic {
   explicit Synthetic(API *api, CalibrationModel calib_model);
   ~Synthetic();
 
+  void SetStreamDataListener(stream_data_listener_t listener);
+
   void NotifyImageParamsChanged();
 
   bool Supports(const Stream &stream) const;
@@ -58,6 +64,11 @@ class Synthetic {
 
   void EnableStreamData(const Stream &stream);
   void DisableStreamData(const Stream &stream);
+
+  void EnableStreamData(
+      const Stream &stream, stream_switch_callback_t callback, bool try_tag);
+  void DisableStreamData(
+      const Stream &stream, stream_switch_callback_t callback, bool try_tag);
   bool IsStreamDataEnabled(const Stream &stream) const;
 
   void SetStreamCallback(const Stream &stream, stream_callback_t callback);
@@ -125,6 +136,8 @@ class Synthetic {
   void OnPointsPostProcess(Object *const out);
   void OnDepthPostProcess(Object *const out);
 
+  void NotifyStreamData(const Stream &stream, const api::StreamData &data);
+
   API *api_;
 
   std::shared_ptr<Processor> processor_;
@@ -132,6 +145,7 @@ class Synthetic {
   std::shared_ptr<Plugin> plugin_;
 
   CalibrationModel calib_model_;
+  std::mutex mtx_left_right_ready_;
 
   std::shared_ptr<IntrinsicsBase> intr_left_;
   std::shared_ptr<IntrinsicsBase> intr_right_;
@@ -139,6 +153,8 @@ class Synthetic {
   bool calib_default_tag_;
 
   std::vector<std::shared_ptr<Processor>> processors_;
+
+  stream_data_listener_t stream_data_listener_;
 };
 
 class SyntheticProcessorPart {

src/mynteye/api/version_checker.cc

@@ -0,0 +1,136 @@
+// 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/api/version_checker.h"
+#include "mynteye/device/utils.h"
+#include "mynteye/logger.h"
+#include "mynteye/types.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+typedef struct {
+  const std::string device_type;
+  const std::string sdk_version;
+  const std::string firmware_version;
+  const std::string status;
+}firmware_version_match_table_unit;
+
+const char* ERRO_DESCRIPTION_F =
+    "Please update the firmware at first";
+const char* ERRO_DESCRIPTION_S =
+    "Please update the SDK at first";
+const char* WARN_DESCRIPTION_F =
+    "We suggest that you should update the firmware";
+const char* WARN_DESCRIPTION_S =
+    "We suggest that you should update the SDK";
+const char* PASS_DESCRIPTION = "pass";
+
+/** firmware/sdk version matched table */
+/**----device type-----sdk version---firmware version-----pass tag-----*/
+static const firmware_version_match_table_unit FSVM_TABLE[] ={
+/** S1030 */
+  {"MYNT-EYE-S1030",    ">2.3.0",         ">2.2.0",   PASS_DESCRIPTION},
+  {"MYNT-EYE-S1030",    ">2.3.0",         "2.2.0",    WARN_DESCRIPTION_F},
+  {"MYNT-EYE-S1030",    ">2.3.0",         "<2.2.0",   ERRO_DESCRIPTION_F},
+  {"MYNT-EYE-S1030",    "<2.3.1",         "<2.2.0",   WARN_DESCRIPTION_S},
+/** S2100 */
+  {"MYNT-EYE-S2100",    ">2.3.0",         "1.0",      PASS_DESCRIPTION},
+  {"MYNT-EYE-S2100",    "<2.3.1",         "1.0",      ERRO_DESCRIPTION_S},
+/** S210A */
+  {"MYNT-EYE-S210A",    ">2.3.0",         "1.0",      PASS_DESCRIPTION},
+  {"MYNT-EYE-S210A",    "<2.3.1",         "1.0",      ERRO_DESCRIPTION_S},
+};
+
+void getVersion(const std::string &str, char *version) {
+  std::string st1("");
+  int j = 0;
+  for (size_t i = 0; i < str.size(); i++) {
+    if (str[i] == '.') {
+      version[j++] = atoi(st1.c_str());
+      st1 = "";
+    } else {
+      st1 += str[i];
+    }
+  }
+  version[j++] = atoi(st1.c_str());
+}
+
+bool conditionMatch(const std::string& condition, const std::string& target) {
+  char version[4] = {0};
+  char version_c[4] = {0};
+  getVersion(target, version);
+  int tag_c = 0;
+  std::string condition_c;
+  if (condition[0] == '>') {
+    tag_c = 1;
+    condition_c = condition.substr(1);
+  } else if (condition[0] == '<') {
+    tag_c = -1;
+    condition_c = condition.substr(1);
+  } else {
+    tag_c = 0;
+    condition_c = condition;
+  }
+  getVersion(condition_c, version_c);
+  int tag_big = memcmp(version, version_c, 4);
+  if (tag_big * tag_c > 0 || (tag_big == 0 && tag_c == 0)) return true;
+  return false;
+}
+
+enum STATUS_UNIT {
+  ST_PASS,
+  ST_ERRO_F,
+  ST_ERRO_S,
+  ST_NOT_PASS
+};
+
+STATUS_UNIT checkUnit(const std::string& sdkv,
+    const std::string& devn,
+    const std::string& firmv,
+    const firmware_version_match_table_unit& condition) {
+    if (condition.device_type == devn &&
+        conditionMatch(condition.sdk_version, sdkv) &&
+        conditionMatch(condition.firmware_version, firmv)) {
+      if (condition.status == ERRO_DESCRIPTION_F) return ST_ERRO_F;
+      if (condition.status == ERRO_DESCRIPTION_S) return ST_ERRO_S;
+      if (condition.status == WARN_DESCRIPTION_F ||
+          condition.status == WARN_DESCRIPTION_S) {
+        LOG(WARNING) << condition.status;
+      }
+      return ST_PASS;
+    }
+  return ST_NOT_PASS;
+}
+
+bool checkFirmwareVersion(const std::shared_ptr<API> api) {
+  auto sdkv = api->GetInfo(Info::SDK_VERSION);
+  auto devn = api->GetInfo(Info::DEVICE_NAME);
+  auto firmv = api->GetInfo(Info::FIRMWARE_VERSION);
+
+  for (size_t i =0;
+      i < sizeof(FSVM_TABLE)/sizeof(firmware_version_match_table_unit);
+      i++) {
+    auto res = checkUnit(sdkv, devn, firmv, FSVM_TABLE[i]);
+    if (res == ST_PASS) {
+      return true;
+    } else if (res == ST_ERRO_S || res == ST_ERRO_F) {
+      LOG(ERROR) << FSVM_TABLE[i].status;
+      return false;
+    }
+  }
+  LOG(ERROR) << ERRO_DESCRIPTION_S;
+  return false;
+}
+
+MYNTEYE_END_NAMESPACE
+

src/mynteye/api/version_checker.h

@@ -0,0 +1,25 @@
+// 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_API_VERSION_CHECKER_H_
+#define MYNTEYE_API_VERSION_CHECKER_H_
+#pragma once
+
+#include <string>
+#include "mynteye/api/api.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+bool checkFirmwareVersion(const std::shared_ptr<API> api);
+MYNTEYE_END_NAMESPACE
+
+#endif  // MYNTEYE_API_VERSION_CHECKER_H_

src/mynteye/device/channel/channels.cc

@@ -460,6 +460,7 @@ bool Channels::GetFiles(
     while (i < end) {
       std::uint8_t file_id = *(data + i);
       std::uint16_t file_size = bytes::_from_data<std::uint16_t>(data + i + 1);
+      LOG(INFO) << "GetFiles:data_size : " << file_size;
       VLOG(2) << "GetFiles id: " << static_cast<int>(file_id)
               << ", size: " << file_size;
       i += 3;

src/mynteye/device/channel/file_channel.cc

@@ -32,6 +32,7 @@ std::size_t FileChannel::GetDeviceInfoFromData(
     const std::uint8_t *data, const std::uint16_t &data_size,
     device_info_t *info) {
   auto n = dev_info_parser_->GetFromData(data, data_size, info);
+  LOG(INFO) << "GetDeviceInfoFromData:data_size : " << data_size;
   auto spec_version = info->spec_version;
   img_params_parser_->SetSpecVersion(spec_version);
   imu_params_parser_->SetSpecVersion(spec_version);
@@ -113,6 +114,22 @@ std::size_t DeviceInfoParser::GetFromData(
   info->nominal_baseline = bytes::_from_data<std::uint16_t>(data + i);
   i += 2;
 
+  if (info->spec_version >= Version(1, 2)) {
+    // auxiliary_chip_version, 2
+    info->auxiliary_chip_version.set_major(data[i]);
+    info->auxiliary_chip_version.set_minor(data[i + 1]);
+    i += 2;
+    // isp_version, 2
+    info->isp_version.set_major(data[i]);
+    info->isp_version.set_minor(data[i + 1]);
+    i += 2;
+  } else {
+    info->auxiliary_chip_version.set_major(0);
+    info->auxiliary_chip_version.set_minor(0);
+    info->isp_version.set_major(0);
+    info->isp_version.set_minor(0);
+  }
+
   // get other infos according to spec_version
 
   MYNTEYE_UNUSED(data_size)
@@ -155,6 +172,17 @@ std::size_t DeviceInfoParser::SetToData(
   bytes::_to_data(info->nominal_baseline, data + i);
   i += 2;
 
+  if (info->spec_version >= Version(1, 2)) {
+    // auxiliary_chip_version, 2
+    data[i] = info->auxiliary_chip_version.major();
+    data[i + 1] = info->auxiliary_chip_version.minor();
+    i += 2;
+    // isp_version, 2
+    data[i] = info->isp_version.major();
+    data[i + 1] = info->isp_version.minor();
+    i += 2;
+  }
+
   // set other infos according to spec_version
 
   // others
@@ -181,7 +209,7 @@ std::size_t ImgParamsParser::GetFromData(
     return GetFromData_v1_0(data, data_size, img_params);
   }
   // s210a old params
-  if (spec_version_ == Version(1, 1) && data_size == 404) {
+  if (spec_version_ >= Version(1, 1) && data_size == 404) {
     return GetFromData_v1_1(data, data_size, img_params);
   }
   // get img params with new version format
@@ -406,7 +434,7 @@ std::size_t ImuParamsParser::GetFromData(
     return GetFromData_old(data, data_size, imu_params);
   }
   // s210a old params
-  if (spec_version_ == Version(1, 1) && data_size == 384) {
+  if (spec_version_ >= Version(1, 1) && data_size == 384) {
     return GetFromData_old(data, data_size, imu_params);
   }
   // get imu params with new version format

src/mynteye/device/device.cc

@@ -234,6 +234,10 @@ std::string Device::GetInfo(const Info &info) const {
       return device_info_->imu_type.to_string();
     case Info::NOMINAL_BASELINE:
       return std::to_string(device_info_->nominal_baseline);
+    case Info::AUXILIARY_CHIP_VERSION:
+      return device_info_->auxiliary_chip_version.to_string();
+    case Info::ISP_VERSION:
+      return device_info_->isp_version.to_string();
     default:
       LOG(WARNING) << "Unknown device info";
       return "";
@@ -349,6 +353,9 @@ OptionInfo Device::GetOptionInfo(const Option &option) const {
 
 std::int32_t Device::GetOptionValue(const Option &option) const {
   if (!Supports(option)) {
+    if (option == Option::FRAME_RATE) {
+      return GetStreamRequest().fps;
+    }
     LOG(WARNING) << "Unsupported option: " << option;
     return -1;
   }
@@ -466,6 +473,11 @@ std::vector<device::StreamData> Device::GetStreamDatas(const Stream &stream) {
   return streams_->GetStreamDatas(stream);
 }
 
+void Device::DisableMotionDatas() {
+  CHECK_NOTNULL(motions_);
+  motions_->DisableMotionDatas();
+}
+
 void Device::EnableMotionDatas() {
   EnableMotionDatas(std::numeric_limits<std::size_t>::max());
 }

src/mynteye/device/motions.cc

@@ -66,7 +66,10 @@ void Motions::SetMotionCallback(motion_callback_t callback) {
 
         std::lock_guard<std::mutex> _(mtx_datas_);
         motion_data_t data = {imu};
-        if (motion_datas_enabled_) {
+        if (motion_datas_enabled_ && motion_datas_max_size_ > 0) {
+          if (motion_datas_.size() >= motion_datas_max_size_) {
+            motion_datas_.erase(motion_datas_.begin());
+          }
           motion_datas_.push_back(data);
         }
 
@@ -98,13 +101,21 @@ void Motions::StopMotionTracking() {
   }
 }
 
+void Motions::DisableMotionDatas() {
+  std::lock_guard<std::mutex> _(mtx_datas_);
+  motion_datas_enabled_ = false;
+  motion_datas_max_size_ = 0;
+  motion_datas_.clear();
+}
+
 void Motions::EnableMotionDatas(std::size_t max_size) {
   if (max_size <= 0) {
     LOG(WARNING) << "Could not enable motion datas with max_size <= 0";
     return;
   }
+  std::lock_guard<std::mutex> _(mtx_datas_);
   motion_datas_enabled_ = true;
-  motion_datas_max_size = max_size;
+  motion_datas_max_size_ = max_size;
 }
 
 Motions::motion_datas_t Motions::GetMotionDatas() {

src/mynteye/device/motions.h

@@ -42,6 +42,7 @@ class Motions {
   void StartMotionTracking();
   void StopMotionTracking();
 
+  void DisableMotionDatas();
   void EnableMotionDatas(std::size_t max_size);
   motion_datas_t GetMotionDatas();
 
@@ -52,7 +53,7 @@ class Motions {
 
   motion_datas_t motion_datas_;
   bool motion_datas_enabled_;
-  std::size_t motion_datas_max_size;
+  std::size_t motion_datas_max_size_;
 
   bool is_imu_tracking;
 

src/mynteye/types.cc

@@ -92,6 +92,8 @@ const char *to_string(const Info &value) {
     CASE(LENS_TYPE)
     CASE(IMU_TYPE)
     CASE(NOMINAL_BASELINE)
+    CASE(AUXILIARY_CHIP_VERSION)
+    CASE(ISP_VERSION)
     default:
       CHECK(is_valid(value));
       return "Info::UNKNOWN";

test/types_test.cc

@@ -53,6 +53,8 @@ TEST(Info, VerifyToString) {
   EXPECT_STREQ("Info::LENS_TYPE", to_string(Info::LENS_TYPE));
   EXPECT_STREQ("Info::IMU_TYPE", to_string(Info::IMU_TYPE));
   EXPECT_STREQ("Info::NOMINAL_BASELINE", to_string(Info::NOMINAL_BASELINE));
+  EXPECT_STREQ("Info::AUXILIARY_CHIP_VERSION", to_string(Info::AUXILIARY_CHIP_VERSION));
+  EXPECT_STREQ("Info::ISP_VERSION", to_string(Info::ISP_VERSION));
 }
 
 TEST(Option, VerifyToString) {

tools/writer/config/S210A/device.info

@@ -2,9 +2,11 @@
 ---
 device_name: MYNT-EYE-S210A
 serial_number: "07C40D1C0009071F"
-firmware_version: "0.1"
+firmware_version: "1.1"
 hardware_version: "1.0"
-spec_version: "1.1"
+spec_version: "1.2"
-lens_type: "0000"
+lens_type: "0001"
-imu_type: "0000"
+imu_type: "0001"
 nominal_baseline: 0
+auxiliary_chip_version: "1.0"
+isp_version: "1.0"

tools/writer/device_writer.cc

@@ -52,7 +52,11 @@ bool DeviceWriter::WriteDeviceInfo(const dev_info_t &info) {
               << ", spec_version: " << dev_info->spec_version.to_string()
               << ", lens_type: " << dev_info->lens_type.to_string()
               << ", imu_type: " << dev_info->imu_type.to_string()
-              << ", nominal_baseline: " << dev_info->nominal_baseline << "}";
+              << ", nominal_baseline: " << dev_info->nominal_baseline
+              << ", auxiliary_chip_version: "
+              << dev_info->auxiliary_chip_version.to_string()
+              << ", isp_version: "
+              << dev_info->isp_version.to_string()<< "}";
     return true;
   } else {
     LOG(ERROR) << "Write device info failed";
@@ -215,6 +219,8 @@ bool DeviceWriter::SaveDeviceInfo(
   fs << "lens_type" << info.lens_type.to_string();
   fs << "imu_type" << info.imu_type.to_string();
   fs << "nominal_baseline" << info.nominal_baseline;
+  fs << "auxiliary_chip_version" << info.auxiliary_chip_version.to_string();
+  fs << "isp_version" << info.isp_version.to_string();
   // save other infos according to spec_version
   fs.release();
   return true;

wrappers/python/src/mynteye_py.cc

@@ -247,6 +247,8 @@ BOOST_PYTHON_MODULE(mynteye_py) {
       .value("LENS_TYPE", Info::LENS_TYPE)
       .value("IMU_TYPE", Info::IMU_TYPE)
       .value("NOMINAL_BASELINE", Info::NOMINAL_BASELINE)
+      .value("AUXILIARY_CHIP_VERSION", Info::AUXILIARY_CHIP_VERSION)
+      .value("ISP_VERSION", Info::ISP_VERSION)
 #ifdef ENUM_EXPORT_VALUES
       .export_values()
 #endif

wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch

@@ -153,7 +153,7 @@
   <!-- <arg name="standard2/ir_control" default="0" /> -->
 
   <!-- standard2/accel_range range: [6,48] -->
-  <arg name="standard2/accel_range" default="-1" />
+  <arg name="standard2/accel_range" default="24" />
   <!-- <arg name="standard2/accel_range" default="6" /> -->
 
   <!-- standard2/gyro_range range: [250,4000] -->
@@ -347,6 +347,10 @@
         - 'image_transport/compressedDepth'
       </rosparam>
     </group>
-
+    <group ns="$(arg depth_topic)">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
   </group> <!-- mynteye -->
 </launch>

wrappers/ros/src/mynt_eye_ros_wrapper/scripts/get_device_info.py

@@ -54,6 +54,8 @@ def main():
       'LENS_TYPE': GetInfoRequest.LENS_TYPE,
       'IMU_TYPE': GetInfoRequest.IMU_TYPE,
       'NOMINAL_BASELINE': GetInfoRequest.NOMINAL_BASELINE,
+      'AUXILIARY_CHIP_VERSION': GetInfoRequest.AUXILIARY_CHIP_VERSION,
+      'ISP_VERSION': GetInfoRequest.ISP_VERSION,
   }
   for k, v in get_device_info(**keys).items():
     print('{}: {}'.format(k, v))

wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc

@@ -315,7 +315,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
       for (auto &&it = mono_topics.begin(); it != mono_topics.end(); ++it) {
         auto &&topic = mono_topics[it->first];
         if (it->first == Stream::LEFT || it->first == Stream::RIGHT) {
-          mono_publishers_[it->first] = it_mynteye.advertiseCamera(topic, 1);
+          mono_publishers_[it->first] = it_mynteye.advertise(topic, 1);
         }
         NODELET_INFO_STREAM("Advertized on topic " << topic);
       }
@@ -407,6 +407,12 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
       case Request::NOMINAL_BASELINE:
         res.value = api_->GetInfo(Info::NOMINAL_BASELINE);
         break;
+      case Request::AUXILIARY_CHIP_VERSION:
+        res.value = api_->GetInfo(Info::AUXILIARY_CHIP_VERSION);
+        break;
+      case Request::ISP_VERSION:
+        res.value = api_->GetInfo(Info::ISP_VERSION);
+        break;
       case Request::IMG_INTRINSICS:
       {
         auto intri_left = api_->GetIntrinsicsBase(Stream::LEFT);
@@ -554,75 +560,41 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     return true;
   }
 
-  void SetIsPublished(const Stream &stream) {
+  void publishData(
-    is_published_[stream] = false;
+      const Stream &stream, const api::StreamData &data, std::uint32_t seq,
-    switch (stream) {
+      ros::Time stamp) {
-      case Stream::LEFT_RECTIFIED: {
+    if (stream == Stream::LEFT || stream == Stream::RIGHT) {
-        if (is_published_[Stream::RIGHT_RECTIFIED]) {
+      return;
-          SetIsPublished(Stream::RIGHT_RECTIFIED);
+    } else if (stream == Stream::POINTS) {
-        }
+      publishPoints(data, seq, stamp);
-        if (is_published_[Stream::DISPARITY]) {
+    } else {
-          SetIsPublished(Stream::DISPARITY);
+      publishCamera(stream, data, seq, stamp);
-        }
-      } break;
-      case Stream::RIGHT_RECTIFIED: {
-        if (is_published_[Stream::LEFT_RECTIFIED]) {
-          SetIsPublished(Stream::LEFT_RECTIFIED);
-        }
-        if (is_published_[Stream::DISPARITY]) {
-          SetIsPublished(Stream::DISPARITY);
-        }
-      } break;
-      case Stream::DISPARITY: {
-        if (is_published_[Stream::DISPARITY_NORMALIZED]) {
-          SetIsPublished(Stream::DISPARITY_NORMALIZED);
-        }
-        if (is_published_[Stream::POINTS]) {
-          SetIsPublished(Stream::POINTS);
-        }
-      } break;
-      case Stream::DISPARITY_NORMALIZED: {
-      } break;
-      case Stream::POINTS: {
-        if (is_published_[Stream::DEPTH]) {
-          SetIsPublished(Stream::DEPTH);
-        }
-      } break;
-      case Stream::DEPTH: {
-      } break;
-      default:
-        return;
     }
   }
 
-  void publishPoint(const Stream &stream) {
+  int getStreamSubscribers(const Stream &stream) {
-    auto &&points_num = points_publisher_.getNumSubscribers();
+    if (stream == Stream::POINTS) {
-    if (points_num == 0 && is_published_[stream]) {
+      return points_publisher_.getNumSubscribers();
-      SetIsPublished(stream);
-      api_->DisableStreamData(stream);
-    } else if (points_num > 0 && !is_published_[Stream::POINTS]) {
-      api_->EnableStreamData(Stream::POINTS);
-      api_->SetStreamCallback(
-          Stream::POINTS, [this](const api::StreamData &data) {
-            // ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
-            ros::Time stamp = checkUpTimeStamp(
-                data.img->timestamp, Stream::POINTS);
-            static std::size_t count = 0;
-            ++count;
-            publishPoints(data, count, stamp);
-          });
-      is_published_[Stream::POINTS] = true;
     }
+    auto pub = camera_publishers_[stream];
+    if (pub)
+      return pub.getNumSubscribers();
+    return -1;
   }
 
   void publishOthers(const Stream &stream) {
-    auto stream_num = camera_publishers_[stream].getNumSubscribers();
+    // std::cout << stream << "===============================" << std::endl;
-    if (stream_num == 0 && is_published_[stream]) {
+    // int enable_tag = 0;
-      // Stop computing when was not subcribed
+    // api_->EnableStreamData(stream, [&](const Stream &stream) {
-      SetIsPublished(stream);
+    //       enable_tag += getStreamSubscribers(stream);
-      api_->DisableStreamData(stream);
+    //       std::cout << "EnableStreamData callback test"
-    } else if (stream_num > 0 && !is_published_[stream]) {
+    //                 << stream << "|| Sum:"
-      // Start computing and publishing when was subcribed
+    //                 << getStreamSubscribers(stream) << std::endl;
+    //     }, true);
+    if (getStreamSubscribers(stream) > 0 && !is_published_[stream]) {
+      // std::cout << stream
+      //           <<"  enableStreamData tag = 0 return" << std::endl;
+      // std::cout << "enable " << stream << std::endl;
       api_->EnableStreamData(stream);
       api_->SetStreamCallback(
           stream, [this, stream](const api::StreamData &data) {
@@ -631,81 +603,121 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
                 data.img->timestamp, stream);
             static std::size_t count = 0;
             ++count;
-            publishCamera(stream, data, count, stamp);
+            publishData(stream, data, count, stamp);
           });
       is_published_[stream] = true;
+      return;
+    }
+
+    int disable_tag = 0;
+    api_->DisableStreamData(stream, [&](const Stream &stream) {
+            disable_tag += getStreamSubscribers(stream);
+            // std::cout << "DisableStreamData callback test: "
+            // << stream << "|| Sum:"<< getStreamSubscribers(stream) << std::endl;
+          }, true);
+    if (disable_tag == 0 && is_published_[stream]) {
+      api_->DisableStreamData(stream, [&](const Stream &stream) {
+            // std::cout << "disable " << stream << std::endl;
+            api_->SetStreamCallback(stream, nullptr);
+            is_published_[stream] = false;
+          });
+      return;
     }
   }
 
   void publishTopics() {
-    if ((camera_publishers_[Stream::LEFT].getNumSubscribers() > 0 ||
+    std::vector<Stream> all_streams{
-        mono_publishers_[Stream::LEFT].getNumSubscribers() > 0) &&
+        Stream::RIGHT,
-        !is_published_[Stream::LEFT]) {
+        Stream::LEFT,
-      api_->SetStreamCallback(
+        Stream::LEFT_RECTIFIED,
-          Stream::LEFT, [&](const api::StreamData &data) {
+        Stream::RIGHT_RECTIFIED,
-            ++left_count_;
+        Stream::DISPARITY,
-            if (left_count_ > 10) {
+        Stream::DISPARITY_NORMALIZED,
-              // ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
+        Stream::POINTS,
-              ros::Time stamp = checkUpTimeStamp(
+        Stream::DEPTH
-                  data.img->timestamp, Stream::LEFT);
+        };
 
-              // static double img_time_prev = -1;
+    static int sum = 0;
-              // NODELET_INFO_STREAM("ros_time_beg: " << FULL_PRECISION <<
+    int sum_c = 0;
-              // ros_time_beg
+    for (auto &&stream : all_streams) {
-              //     << ", img_time_elapsed: " << FULL_PRECISION
+      sum_c += getStreamSubscribers(stream);
-              //     << ((data.img->timestamp - img_time_beg) * 0.00001f)
-              //     << ", img_time_diff: " << FULL_PRECISION
-              //     << ((img_time_prev < 0) ? 0
-              //         : (data.img->timestamp - img_time_prev) * 0.01f) << "
-              //         ms");
-              // img_time_prev = data.img->timestamp;
-              publishCamera(Stream::LEFT, data, left_count_, stamp);
-              publishMono(Stream::LEFT, data, left_count_, stamp);
-              NODELET_DEBUG_STREAM(
-                  Stream::LEFT << ", count: " << left_count_
-                      << ", frame_id: " << data.img->frame_id
-                      << ", timestamp: " << data.img->timestamp
-                      << ", exposure_time: " << data.img->exposure_time);
-            }
-          });
-      left_time_beg_ = ros::Time::now().toSec();
-      is_published_[Stream::LEFT] = true;
     }
 
-    if ((camera_publishers_[Stream::RIGHT].getNumSubscribers() > 0 ||
+    if (sum_c != sum) {
-        mono_publishers_[Stream::RIGHT].getNumSubscribers() > 0) &&
+      if (sum_c == 0) {
-        !is_published_[Stream::RIGHT]) {
+        api_->Stop(Source::VIDEO_STREAMING);
-      api_->SetStreamCallback(
+        for (auto &&stream : all_streams) {
-          Stream::RIGHT, [&](const api::StreamData &data) {
+          is_published_[stream] = false;
-            ++right_count_;
+        }
-            if (right_count_ > 10) {
+        api_->Start(Source::VIDEO_STREAMING);
-              // ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
-              ros::Time stamp = checkUpTimeStamp(
-                  data.img->timestamp, Stream::RIGHT);
-              publishCamera(Stream::RIGHT, data, right_count_, stamp);
-              publishMono(Stream::RIGHT, data, right_count_, stamp);
-              NODELET_DEBUG_STREAM(
-                  Stream::RIGHT << ", count: " << right_count_
-                      << ", frame_id: " << data.img->frame_id
-                      << ", timestamp: " << data.img->timestamp
-                      << ", exposure_time: " << data.img->exposure_time);
-            }
-          });
-      right_time_beg_ = ros::Time::now().toSec();
-      is_published_[Stream::RIGHT] = true;
-    }
-
-    std::vector<Stream> other_streams{
-        Stream::LEFT_RECTIFIED, Stream::RIGHT_RECTIFIED,
-        Stream::DISPARITY,      Stream::DISPARITY_NORMALIZED,
-        Stream::POINTS,         Stream::DEPTH};
-
-    for (auto &&stream : other_streams) {
-      if (stream != Stream::POINTS) {
-        publishOthers(stream);
       } else {
-        publishPoint(stream);
+        if ((camera_publishers_[Stream::LEFT].getNumSubscribers() > 0 ||
+            mono_publishers_[Stream::LEFT].getNumSubscribers() > 0) &&
+            !is_published_[Stream::LEFT]) {
+          api_->SetStreamCallback(
+              Stream::LEFT, [&](const api::StreamData &data) {
+                ++left_count_;
+                if (left_count_ > 10) {
+                  // ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
+                  ros::Time stamp = checkUpTimeStamp(
+                      data.img->timestamp, Stream::LEFT);
+
+                  // static double img_time_prev = -1;
+                  // NODELET_INFO_STREAM("ros_time_beg: " << FULL_PRECISION <<
+                  // ros_time_beg
+                  //     << ", img_time_elapsed: " << FULL_PRECISION
+                  //     << ((data.img->timestamp - img_time_beg) * 0.00001f)
+                  //     << ", img_time_diff: " << FULL_PRECISION
+                  //     << ((img_time_prev < 0) ? 0
+                  //         : (data.img->timestamp - img_time_prev) * 0.01f) << "
+                  //         ms");
+                  // img_time_prev = data.img->timestamp;
+                  publishCamera(Stream::LEFT, data, left_count_, stamp);
+                  publishMono(Stream::LEFT, data, left_count_, stamp);
+                  NODELET_DEBUG_STREAM(
+                      Stream::LEFT << ", count: " << left_count_
+                          << ", frame_id: " << data.img->frame_id
+                          << ", timestamp: " << data.img->timestamp
+                          << ", exposure_time: " << data.img->exposure_time);
+                }
+              });
+          left_time_beg_ = ros::Time::now().toSec();
+          is_published_[Stream::LEFT] = true;
+        }
+
+        if ((camera_publishers_[Stream::RIGHT].getNumSubscribers() > 0 ||
+            mono_publishers_[Stream::RIGHT].getNumSubscribers() > 0) &&
+            !is_published_[Stream::RIGHT]) {
+          api_->SetStreamCallback(
+              Stream::RIGHT, [&](const api::StreamData &data) {
+                ++right_count_;
+                if (right_count_ > 10) {
+                  // ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
+                  ros::Time stamp = checkUpTimeStamp(
+                      data.img->timestamp, Stream::RIGHT);
+                  publishCamera(Stream::RIGHT, data, right_count_, stamp);
+                  publishMono(Stream::RIGHT, data, right_count_, stamp);
+                  NODELET_DEBUG_STREAM(
+                      Stream::RIGHT << ", count: " << right_count_
+                          << ", frame_id: " << data.img->frame_id
+                          << ", timestamp: " << data.img->timestamp
+                          << ", exposure_time: " << data.img->exposure_time);
+                }
+              });
+          right_time_beg_ = ros::Time::now().toSec();
+          is_published_[Stream::RIGHT] = true;
+        }
+
+        std::vector<Stream> other_streams{
+            Stream::LEFT_RECTIFIED, Stream::RIGHT_RECTIFIED,
+            Stream::DISPARITY,      Stream::DISPARITY_NORMALIZED,
+            Stream::POINTS,         Stream::DEPTH
+            };
+        for (auto &&stream : other_streams) {
+          publishOthers(stream);
+        }
       }
+      sum = sum_c;
     }
 
     if (!is_motion_published_) {
@@ -822,9 +834,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     cv::cvtColor(data.frame, mono, CV_RGB2GRAY);
     auto &&msg = cv_bridge::CvImage(header, enc::MONO8, mono).toImageMsg();
     pthread_mutex_unlock(&mutex_data_);
-    auto &&info = getCameraInfo(stream);
+    mono_publishers_[stream].publish(msg);
-    info->header.stamp = msg->header.stamp;
-    mono_publishers_[stream].publish(msg, info);
   }
 
   void publishPoints(
@@ -1284,7 +1294,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
       }
       for (int i = 0; i < p.rows; i++) {
         for (int j = 0; j < p.cols; j++) {
-          camera_info->P.at(i * p.cols + j) = p.at<double>(i, j);
+          int scale = (i == 2 && j == 2)?1:1000;
+          camera_info->P.at(i * p.cols + j) = p.at<double>(i, j) / scale;
         }
       }
 
@@ -1510,7 +1521,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
   std::map<Stream, std::string> image_encodings_;
 
   // mono: LEFT, RIGHT
-  std::map<Stream, image_transport::CameraPublisher> mono_publishers_;
+  std::map<Stream, image_transport::Publisher> mono_publishers_;
 
   // pointcloud: POINTS
   ros::Publisher points_publisher_;

wrappers/ros/src/mynt_eye_ros_wrapper/srv/GetInfo.srv

@@ -6,10 +6,12 @@ uint32 SPEC_VERSION=4
 uint32 LENS_TYPE=5
 uint32 IMU_TYPE=6
 uint32 NOMINAL_BASELINE=7
-uint32 IMG_INTRINSICS=8
+uint32 AUXILIARY_CHIP_VERSION=8
-uint32 IMG_EXTRINSICS_RTOL=9
+uint32 ISP_VERSION=9
-uint32 IMU_INTRINSICS=10
+uint32 IMG_INTRINSICS=10
-uint32 IMU_EXTRINSICS=11
+uint32 IMG_EXTRINSICS_RTOL=11
+uint32 IMU_INTRINSICS=12
+uint32 IMU_EXTRINSICS=13
 uint32 key
 ---
 string value