feat(api): add timestamp correspondence

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(
-        [](const device::MotionData &data) { callback_({data.imu}); }, true);
+    device_->SetMotionCallback([this](const device::MotionData &data) {
+      callback_({data.imu});
+    }, true);
   } else {
     device_->SetMotionCallback(nullptr);
   }
@@ -455,15 +461,41 @@ 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() {
-  std::vector<api::MotionData> datas;
-  for (auto &&data : device_->GetMotionDatas()) {
-    datas.push_back({data.imu});
+  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));
   }
-  return datas;
 }
 
 void API::EnablePlugin(const std::string &path) {

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_