feat(api): add timestamp correspondence

CMakeLists.txt

@@ -219,6 +219,7 @@ 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
   )
   if(WITH_CAM_MODELS)
     list(APPEND MYNTEYE_SRCS

include/mynteye/api/api.h

@@ -30,6 +30,7 @@ MYNTEYE_BEGIN_NAMESPACE
 
 struct DeviceInfo;
 
+class Correspondence;
 class Device;
 class Synthetic;
 
@@ -305,6 +306,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 +323,10 @@ class MYNTEYE_API API {
 
   std::unique_ptr<Synthetic> synthetic_;
 
+  std::unique_ptr<Correspondence> correspondence_;
+
+  motion_callback_t callback_;
+
   void CheckImageParams();
 };
 

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/data/get_imu_correspondence.cc

@@ -0,0 +1,77 @@
+// 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");
+
+  while (true) {
+    api->WaitForStreams();
+
+    auto &&left_data = api->GetStreamData(Stream::LEFT);
+    auto &&right_data = api->GetStreamData(Stream::RIGHT);
+
+    LOG(INFO) << "Img frame_id: " << left_data.img->frame_id
+              << ", timestamp: " << left_data.img->timestamp;
+
+    cv::Mat img;
+    cv::hconcat(left_data.frame, right_data.frame, img);
+
+    auto &&motion_datas = api->GetMotionDatas();
+    for (auto &&data : motion_datas) {
+      LOG(INFO) << "Imu frame_id: " << data.imu->frame_id
+                << ", timestamp: " << data.imu->timestamp;
+    }
+    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,6 +22,7 @@
 #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"
@@ -208,7 +209,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));
@@ -377,10 +378,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);
   }
@@ -455,16 +461,42 @@ std::vector<api::StreamData> API::GetStreamDatas(const Stream &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() {
+  if (correspondence_) {
+    return correspondence_->GetMotionDatas();
+  } 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));
+  }
+}
 
 void API::EnablePlugin(const std::string &path) {
   static DL dl;

src/mynteye/api/correspondence.cc

@@ -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.
+#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) {
+  VLOG(2) << __func__;
+}
+
+Correspondence::~Correspondence() {
+  VLOG(2) << __func__;
+}
+
+void Correspondence::OnStreamDataCallback(
+    const Stream &stream, const api::StreamData &data) {
+  // LOG(INFO) << __func__ << ", " << stream
+  //     << ", id: " << data.frame_id << ", stamp: " << data.img->timestamp;
+}
+
+void Correspondence::OnMotionDataCallback(const device::MotionData &data) {
+  // LOG(INFO) << __func__ << ", id: " << data.imu->frame_id
+  //     << ", stamp: " << data.imu->timestamp;
+  {
+    std::lock_guard<std::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;
+}
+
+std::vector<api::MotionData> Correspondence::GetMotionDatas() {
+  std::lock_guard<std::mutex> _(mtx_motion_datas_);
+  std::vector<api::MotionData> datas;
+  for (auto &&data : motion_datas_) {
+    datas.push_back({data.imu});
+  }
+  motion_datas_.clear();
+  return datas;
+}
+
+MYNTEYE_END_NAMESPACE

src/mynteye/api/correspondence.h

@@ -0,0 +1,49 @@
+// 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 <memory>
+#include <mutex>
+#include <vector>
+
+#include "mynteye/api.h"
+#include "mynteye/device/callbacks.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+class Correspondence {
+ public:
+  Correspondence(const std::shared_ptr<Device> &device, const Stream &stream);
+  ~Correspondence();
+
+  void OnStreamDataCallback(const Stream &stream, const api::StreamData &data);
+  void OnMotionDataCallback(const device::MotionData &data);
+
+  void SetMotionCallback(API::motion_callback_t callback);
+
+  std::vector<api::MotionData> GetMotionDatas();
+
+ private:
+  std::shared_ptr<Device> device_;
+  Stream stream_;
+  API::motion_callback_t motion_callback_;
+  std::vector<device::MotionData> motion_datas_;
+  std::mutex mtx_motion_datas_;
+};
+
+MYNTEYE_END_NAMESPACE
+
+#endif  // MYNTEYE_API_CONFIG_H_