Add motions & times

CMakeLists.txt

@@ -94,6 +94,7 @@ set(MYNTEYE_SRCS
   ${UVC_SRC}
   src/internal/channels.cc
   src/internal/config.cc
+  src/internal/motions.cc
   src/internal/streams.cc
   src/internal/strings.cc
   src/internal/types.cc

samples/device/camera.cc

@@ -6,6 +6,8 @@
 #include "device/context.h"
 #include "device/device.h"
 
+#include "internal/times.h"
+
 MYNTEYE_USE_NAMESPACE
 
 int main(int argc, char *argv[]) {
@@ -86,22 +88,23 @@ int main(int argc, char *argv[]) {
   device->SetMotionCallback([&imu_count](const device::MotionData &data) {
     CHECK_NOTNULL(data.imu);
     ++imu_count;
-    LOG(INFO) << "Imu count: " << imu_count;
-    LOG(INFO) << "  frame_id: " << data.imu->frame_id
-              << ", timestamp: " << data.imu->timestamp
-              << ", accel_x: " << data.imu->accel[0]
-              << ", accel_y: " << data.imu->accel[1]
-              << ", accel_z: " << data.imu->accel[2]
-              << ", gyro_x: " << data.imu->gyro[0]
-              << ", gyro_y: " << data.imu->gyro[1]
-              << ", gyro_z: " << data.imu->gyro[2]
-              << ", temperature: " << data.imu->temperature;
+    VLOG(2) << "Imu count: " << imu_count;
+    VLOG(2) << "  frame_id: " << data.imu->frame_id
+            << ", timestamp: " << data.imu->timestamp
+            << ", accel_x: " << data.imu->accel[0]
+            << ", accel_y: " << data.imu->accel[1]
+            << ", accel_z: " << data.imu->accel[2]
+            << ", gyro_x: " << data.imu->gyro[0]
+            << ", gyro_y: " << data.imu->gyro[1]
+            << ", gyro_z: " << data.imu->gyro[2]
+            << ", temperature: " << data.imu->temperature;
   });
 
   device->Start(Source::ALL);
 
   cv::namedWindow("frame");
 
+  auto &&time_beg = times::now();
   while (true) {
     device->WaitForStreams();
 
@@ -124,7 +127,20 @@ int main(int argc, char *argv[]) {
       break;
     }
   }
+  auto &&time_end = times::now();
 
   device->Stop(Source::ALL);
+
+  float elapsed_ms =
+      times::count<times::microseconds>(time_end - time_beg) * 0.001f;
+  LOG(INFO) << "Time beg: " << times::to_local_string(time_beg)
+            << ", end: " << times::to_local_string(time_end)
+            << ", cost: " << elapsed_ms << "ms";
+  LOG(INFO) << "Left count: " << left_count
+            << ", fps: " << (1000.f * left_count / elapsed_ms);
+  LOG(INFO) << "Right count: " << right_count
+            << ", fps: " << (1000.f * right_count / elapsed_ms);
+  LOG(INFO) << "Imu count: " << imu_count
+            << ", hz: " << (1000.f * imu_count / elapsed_ms);
   return 0;
 }

src/device/device.cc

@@ -8,6 +8,7 @@
 #include "device/device_s.h"
 #include "internal/channels.h"
 #include "internal/config.h"
+#include "internal/motions.h"
 #include "internal/streams.h"
 #include "internal/strings.h"
 #include "internal/types.h"
@@ -45,7 +46,8 @@ Device::Device(const Model &model, std::shared_ptr<uvc::device> device)
       model_(model),
       device_(device),
       streams_(nullptr),
-      channels_(std::make_shared<Channels>(device)) {
+      channels_(std::make_shared<Channels>(device)),
+      motions_(std::make_shared<Motions>(channels_)) {
   VLOG(2) << __func__;
   ReadDeviceInfo();
 }
@@ -276,6 +278,17 @@ device::StreamData Device::GetLatestStreamData(const Stream &stream) {
   return streams_->GetLatestStreamData(stream);
 }
 
+void Device::EnableMotionDatas(std::size_t max_size) {
+  CHECK_NOTNULL(motions_);
+  motions_->EnableMotionDatas(max_size);
+}
+
+std::vector<device::MotionData> Device::GetMotionDatas() {
+  CHECK(motion_tracking_);
+  CHECK_NOTNULL(motions_);
+  return motions_->GetMotionDatas();
+}
+
 const StreamRequest &Device::GetStreamRequest(const Capabilities &capability) {
   try {
     return stream_config_requests_.at(capability);
@@ -367,27 +380,12 @@ void Device::StartMotionTracking() {
     LOG(WARNING) << "Cannot start motion tracking without first stopping it";
     return;
   }
-  channels_->StartImuTracking([this](const ImuPacket &packet) {
-    if (!HasMotionCallback())
-      return;
-    for (auto &&seg : packet.segments) {
-      auto &&imu = std::make_shared<ImuData>();
-      imu->frame_id = seg.frame_id;
-      if (seg.offset < 0 &&
-          static_cast<uint32_t>(-seg.offset) > packet.timestamp) {
-        LOG(WARNING) << "Imu timestamp offset is incorrect";
-      }
-      imu->timestamp = packet.timestamp + seg.offset;
-      imu->accel[0] = seg.accel[0] * 8.f / 0x10000;
-      imu->accel[1] = seg.accel[1] * 8.f / 0x10000;
-      imu->accel[2] = seg.accel[2] * 8.f / 0x10000;
-      imu->gyro[0] = seg.gyro[0] * 1000.f / 0x10000;
-      imu->gyro[1] = seg.gyro[1] * 1000.f / 0x10000;
-      imu->gyro[2] = seg.gyro[2] * 1000.f / 0x10000;
-      imu->temperature = seg.temperature / 326.8f + 25;
-      motion_callback_({imu});
+  motions_->SetMotionCallback([this](const device::MotionData &data) {
+    if (motion_callback_) {
+      motion_callback_(data);
     }
   });
+  motions_->StartMotionTracking();
   motion_tracking_ = true;
 }
 
@@ -396,7 +394,7 @@ void Device::StopMotionTracking() {
     LOG(WARNING) << "Cannot stop motion tracking without first starting it";
     return;
   }
-  channels_->StopImuTracking();
+  motions_->StopMotionTracking();
   motion_tracking_ = false;
 }
 

src/device/device.h

@@ -2,6 +2,7 @@
 #define MYNTEYE_DEVICE_H_
 #pragma once
 
+#include <limits>
 #include <map>
 #include <memory>
 #include <mutex>
@@ -24,6 +25,7 @@ struct device;
 struct DeviceInfo;
 
 class Channels;
+class Motions;
 class Streams;
 
 class Device {
@@ -83,6 +85,10 @@ class Device {
   std::vector<device::StreamData> GetStreamDatas(const Stream &stream);
   device::StreamData GetLatestStreamData(const Stream &stream);
 
+  void EnableMotionDatas(
+      std::size_t max_size = std::numeric_limits<std::size_t>::max());
+  std::vector<device::MotionData> GetMotionDatas();
+
  protected:
   std::shared_ptr<uvc::device> device() const {
     return device_;
@@ -126,6 +132,8 @@ class Device {
 
   std::shared_ptr<Channels> channels_;
 
+  std::shared_ptr<Motions> motions_;
+
   void ReadDeviceInfo();
 
   void WriteImgIntrinsics(const ImgIntrinsics &intrinsics);

src/internal/motions.cc

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

src/internal/motions.h

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

src/internal/times.h

@@ -0,0 +1,210 @@
+#ifndef MYNTEYE_INTERNAL_TIMES_H_  // NOLINT
+#define MYNTEYE_INTERNAL_TIMES_H_
+#pragma once
+
+#include <chrono>
+#include <cmath>
+#include <cstdint>
+#include <ctime>
+#include <iomanip>
+#include <ratio>
+#include <sstream>
+#include <string>
+
+#include "mynteye/mynteye.h"
+
+MYNTEYE_BEGIN_NAMESPACE
+
+namespace times {
+
+using system_clock = std::chrono::system_clock;
+
+using nanoseconds = std::chrono::nanoseconds;
+using microseconds = std::chrono::microseconds;
+using milliseconds = std::chrono::milliseconds;
+using seconds = std::chrono::seconds;
+using minutes = std::chrono::minutes;
+using hours = std::chrono::hours;
+using days = std::chrono::duration<std::int64_t, std::ratio<86400>>;
+
+// to
+
+template <typename Duration>
+inline Duration to_duration(const std::int64_t &t) {
+  return Duration{t};
+}
+
+template <typename Duration>
+inline system_clock::time_point to_time_point(const Duration &d) {
+  return system_clock::time_point(d);
+}
+
+template <typename Duration>
+inline system_clock::time_point to_time_point(const std::int64_t &t) {
+  return system_clock::time_point(Duration{t});
+}
+
+inline system_clock::time_point to_time_point(std::tm *tm) {
+  return system_clock::from_time_t(std::mktime(tm));
+}
+
+inline struct std::tm *to_local_tm(const system_clock::time_point &t) {
+  auto t_c = system_clock::to_time_t(t);
+  return std::localtime(&t_c);
+}
+
+inline struct std::tm *to_utc_tm(const system_clock::time_point &t) {
+  auto t_c = system_clock::to_time_t(t);
+  return std::gmtime(&t_c);
+}
+
+// cast
+
+template <typename FromDuration, typename ToDuration>
+inline ToDuration cast(const FromDuration &d) {
+  return std::chrono::duration_cast<ToDuration>(d);
+}
+
+template <typename Duration>
+inline Duration cast(const system_clock::duration &d) {
+  return cast<system_clock::duration, Duration>(d);
+}
+
+template <typename FromDuration, typename ToDuration>
+inline std::int64_t cast(const std::int64_t &t) {
+  return cast<FromDuration, ToDuration>(FromDuration{t}).count();
+}
+
+template <typename Duration>
+inline system_clock::time_point cast(const system_clock::time_point &d) {
+  // C++17, floor
+  return std::chrono::time_point_cast<Duration>(d);
+}
+
+template <typename Duration>
+inline system_clock::duration cast_mod(const system_clock::time_point &t) {
+  return t - cast<Duration>(t);
+}
+
+// count
+
+template <typename FromDuration, typename ToDuration>
+inline std::int64_t count(const FromDuration &d) {
+  return cast<FromDuration, ToDuration>(d).count();
+}
+
+template <typename Duration>
+inline std::int64_t count(const system_clock::duration &d) {
+  return cast<Duration>(d).count();
+}
+
+// day
+
+inline std::tm *day_beg(std::tm *tm) {
+  tm->tm_hour = 0;
+  tm->tm_min = 0;
+  tm->tm_sec = 0;
+  return tm;
+}
+
+inline std::tm *day_end(std::tm *tm) {
+  tm->tm_hour = 23;
+  tm->tm_min = 59;
+  tm->tm_sec = 59;
+  return tm;
+}
+
+inline system_clock::time_point day_beg(const system_clock::time_point &t) {
+  return cast<days>(t);
+}
+
+inline system_clock::time_point day_end(const system_clock::time_point &t) {
+  return day_beg(t) + days(1) - system_clock::duration(1);
+}
+
+inline system_clock::duration day_time(const system_clock::time_point &t) {
+  return cast_mod<days>(t);
+}
+
+// between
+
+template <typename Duration>
+inline std::int64_t between(
+    const system_clock::time_point &t1, const system_clock::time_point &t2) {
+  return count<Duration>(t2 - t1);
+}
+
+inline std::int64_t between_days(
+    const system_clock::time_point &t1, const system_clock::time_point &t2) {
+  return between<days>(day_beg(t1), day_beg(t2));
+}
+
+template <typename Duration>
+inline std::int64_t between_days(
+    const std::int64_t &t1, const std::int64_t &t2) {
+  return between_days(to_time_point<Duration>(t1), to_time_point<Duration>(t2));
+}
+
+// epoch
+
+inline system_clock::time_point epoch() {
+  return system_clock::time_point(system_clock::duration{0});
+}
+
+template <typename Duration>
+inline std::int64_t since_epoch(const system_clock::time_point &t) {
+  return count<Duration>(t.time_since_epoch());
+}
+
+// now
+
+inline system_clock::time_point now() {
+  return system_clock::now();
+}
+
+template <typename Duration>
+inline std::int64_t now() {
+  return since_epoch<Duration>(now());
+}
+
+// string
+
+inline std::string to_string(
+    const system_clock::time_point &t, const std::tm *tm,
+    const char *fmt = "%F %T", std::int32_t precision = 6) {
+  std::stringstream ss;
+#if defined(OS_ANDROID) || defined(OS_LINUX)
+  char foo[20];
+  strftime(foo, sizeof(foo), fmt, tm);
+  ss << foo;
+#else
+  ss << std::put_time(tm, fmt);
+#endif
+  if (precision > 0) {
+    if (precision > 6)
+      precision = 6;
+    ss << '.' << std::setfill('0') << std::setw(precision)
+       << static_cast<std::int32_t>(
+              count<microseconds>(cast_mod<seconds>(t)) /
+              std::pow(10, 6 - precision));
+  }
+  return ss.str();
+}
+
+inline std::string to_local_string(
+    const system_clock::time_point &t, const char *fmt = "%F %T",
+    const std::int32_t &precision = 6) {
+  return to_string(t, to_local_tm(t), fmt, precision);
+}
+
+inline std::string to_utc_string(
+    const system_clock::time_point &t, const char *fmt = "%F %T",
+    const std::int32_t &precision = 6) {
+  return to_string(t, to_utc_tm(t), fmt, precision);
+}
+
+}  // namespace times
+
+MYNTEYE_END_NAMESPACE
+
+#endif  // MYNTEYE_INTERNAL_TIMES_H_ NOLINT