Merge branch 'develop' of http://gitlab.mynt.com/mynteye/mynt-eye-s-sdk into develop

include/mynteye/api/api.h

@@ -349,6 +349,11 @@ class MYNTEYE_API API {
 
   std::shared_ptr<Device> device();
 
+  /** Enable process mode, e.g. imu assembly, temp_drift */
+  void EnableProcessMode(const ProcessMode& mode);
+  /** Enable process mode, e.g. imu assembly, temp_drift */
+  void EnableProcessMode(const std::int32_t& mode);
+
  private:
   std::shared_ptr<Device> device_;
 

include/mynteye/device/device.h

@@ -295,6 +295,11 @@ class MYNTEYE_API Device {
    */
   std::vector<device::MotionData> GetMotionDatas();
 
+  /** Enable process mode, e.g. imu assembly, temp_drift */
+  void EnableProcessMode(const ProcessMode& mode);
+  /** Enable process mode, e.g. imu assembly, temp_drift */
+  void EnableProcessMode(const std::int32_t& mode);
+
  protected:
   std::shared_ptr<uvc::device> device() const {
     return device_;

include/mynteye/types.h

@@ -329,6 +329,30 @@ enum class Format : std::uint32_t {
 
 #undef MYNTEYE_FOURCC
 
+/**
+ * @ingroup enumerations
+ * @brief Process modes.
+ */
+enum class ProcessMode : std::int32_t {
+  PROC_NONE           = 0,
+  PROC_IMU_ASSEMBLY   = 1,
+  PROC_IMU_TEMP_DRIFT = 2,
+  PROC_IMU_ALL        = PROC_IMU_ASSEMBLY | PROC_IMU_TEMP_DRIFT
+};
+
+inline
+std::int32_t operator&(const std::int32_t& lhs, const ProcessMode& rhs) {
+  return lhs & static_cast<std::int32_t>(rhs);
+}
+inline
+std::int32_t operator&(const ProcessMode& lhs, const std::int32_t& rhs) {
+  return static_cast<std::int32_t>(lhs) & rhs;
+}
+inline
+std::int32_t operator&(const ProcessMode& lhs, const ProcessMode& rhs) {
+  return static_cast<std::int32_t>(lhs) & static_cast<std::int32_t>(rhs);
+}
+
 MYNTEYE_API const char *to_string(const Format &value);
 
 inline std::ostream &operator<<(std::ostream &os, const Format &value) {

src/mynteye/api/api.cc

@@ -592,4 +592,12 @@ void API::CheckImageParams() {
   }
 }
 
+void API::EnableProcessMode(const ProcessMode& mode) {
+  EnableProcessMode(static_cast<std::int32_t>(mode));
+}
+
+void API::EnableProcessMode(const std::int32_t& mode) {
+  device_->EnableProcessMode(mode);
+}
+
 MYNTEYE_END_NAMESPACE

src/mynteye/device/device.cc

@@ -330,6 +330,7 @@ void Device::SetMotionIntrinsics(const MotionIntrinsics &in) {
     motion_intrinsics_ = std::make_shared<MotionIntrinsics>();
   }
   *motion_intrinsics_ = in;
+  motions_->SetMotionIntrinsics(motion_intrinsics_);
 }
 
 void Device::SetMotionExtrinsics(const Stream &from, const Extrinsics &ex) {
@@ -715,4 +716,12 @@ bool Device::SetFiles(
   return channels_->SetFiles(info, img_params, imu_params);
 }
 
+void Device::EnableProcessMode(const ProcessMode& mode) {
+  EnableProcessMode(static_cast<std::int32_t>(mode));
+}
+
+void Device::EnableProcessMode(const std::int32_t& mode) {
+  motions_->EnableProcessMode(mode);
+}
+
 MYNTEYE_END_NAMESPACE

src/mynteye/device/motions.cc

@@ -18,11 +18,39 @@
 
 MYNTEYE_BEGIN_NAMESPACE
 
+namespace {
+
+void matrix_3x1(const double (*src1)[3], const double (*src2)[1],
+    double (*dst)[1]) {
+  for (int i = 0; i < 3; i++) {
+    for (int j = 0; j < 1; j++) {
+      for (int k = 0; k < 3; k++) {
+        dst[i][j] += src1[i][k] * src2[k][j];
+      }
+    }
+  }
+}
+
+void matrix_3x3(const double (*src1)[3], const double (*src2)[3],
+    double (*dst)[3]) {
+  for (int i = 0; i < 3; i++) {
+    for (int j = 0; j < 3; j++) {
+      for (int k = 0; k < 3; k++) {
+        dst[i][j] += src1[i][k] * src2[k][j];
+      }
+    }
+  }
+}
+
+} // namespace
+
 Motions::Motions(std::shared_ptr<Channels> channels)
     : channels_(channels),
       motion_callback_(nullptr),
       motion_datas_enabled_(false),
-      is_imu_tracking(false) {
+      is_imu_tracking(false),
+      proc_mode_(static_cast<const std::int32_t>(ProcessMode::PROC_NONE)),
+      motion_intrinsics_(nullptr) {
   CHECK_NOTNULL(channels_);
   VLOG(2) << __func__;
 }
@@ -64,6 +92,17 @@ void Motions::SetMotionCallback(motion_callback_t callback) {
         imu->gyro[1] = seg.gyro[1] * 1.f * gyro_range / 0x10000;
         imu->gyro[2] = seg.gyro[2] * 1.f * gyro_range / 0x10000;
 
+        bool proc_assembly = ((proc_mode_ & ProcessMode::PROC_IMU_ASSEMBLY) > 0);
+        bool proc_temp_drift = ((proc_mode_ & ProcessMode::PROC_IMU_TEMP_DRIFT) > 0);
+        if (proc_assembly && proc_temp_drift) {
+          ProcImuTempDrift(imu);
+          ProcImuAssembly(imu);
+        } else if (proc_assembly) {
+          ProcImuAssembly(imu);
+        } else if (proc_temp_drift) {
+          ProcImuTempDrift(imu);
+        }
+
         std::lock_guard<std::mutex> _(mtx_datas_);
         motion_data_t data = {imu};
         if (motion_datas_enabled_ && motion_datas_max_size_ > 0) {
@@ -129,4 +168,64 @@ Motions::motion_datas_t Motions::GetMotionDatas() {
   return datas;
 }
 
+void Motions::ProcImuAssembly(std::shared_ptr<ImuData> data) const {
+  if (nullptr == motion_intrinsics_) return;
+
+  double dst[3][3] = {0};
+  if (data->flag == 1) {
+    matrix_3x3(motion_intrinsics_->accel.scale,
+        motion_intrinsics_->accel.assembly, dst);
+    double s[3][1] = {0};
+    double d[3][1] = {0};
+    for (int i = 0; i < 3; i++) {
+      s[i][0] = data->accel[i];
+    }
+    matrix_3x1(dst, s, d);
+    for (int i = 0; i < 3; i++) {
+      data->accel[i] = d[i][0];
+    }
+  } else if (data->flag == 2) {
+    matrix_3x3(motion_intrinsics_->gyro.scale,
+        motion_intrinsics_->gyro.assembly, dst);
+    double s[3][1] = {0};
+    double d[3][1] = {0};
+    for (int i = 0; i < 3; i++) {
+      s[i][0] = data->gyro[i];
+    }
+    matrix_3x1(dst, s, d);
+    for (int i = 0; i < 3; i++) {
+      data->gyro[i] = d[i][0];
+    }
+  }
+}
+
+void Motions::ProcImuTempDrift(std::shared_ptr<ImuData> data) const {
+  if (nullptr == motion_intrinsics_) return;
+
+  double temp = data->temperature;
+  if (data->flag == 1) {
+    data->accel[0] -= motion_intrinsics_->accel.x[1] * temp
+      + motion_intrinsics_->accel.x[0];
+    data->accel[1] -= motion_intrinsics_->accel.y[1] * temp
+      + motion_intrinsics_->accel.y[0];
+    data->accel[2] -= motion_intrinsics_->accel.z[1] * temp
+      + motion_intrinsics_->accel.z[0];
+  } else if (data->flag == 2) {
+    data->gyro[0] -= motion_intrinsics_->gyro.x[1] * temp
+      + motion_intrinsics_->gyro.x[0];
+    data->gyro[1] -= motion_intrinsics_->gyro.y[1] * temp
+      + motion_intrinsics_->gyro.y[0];
+    data->gyro[2] -= motion_intrinsics_->gyro.z[1] * temp
+      + motion_intrinsics_->gyro.z[0];
+  }
+}
+
+void Motions::SetMotionIntrinsics(const std::shared_ptr<MotionIntrinsics>& in) {
+  motion_intrinsics_ = in;
+}
+
+void Motions::EnableProcessMode(const std::int32_t& mode) {
+  proc_mode_ = mode;
+}
+
 MYNTEYE_END_NAMESPACE

src/mynteye/device/motions.h

@@ -46,7 +46,13 @@ class Motions {
   void EnableMotionDatas(std::size_t max_size);
   motion_datas_t GetMotionDatas();
 
+  void SetMotionIntrinsics(const std::shared_ptr<MotionIntrinsics>& in);
+  void EnableProcessMode(const std::int32_t& mode);
+
  private:
+  void ProcImuAssembly(std::shared_ptr<ImuData> data) const;
+  void ProcImuTempDrift(std::shared_ptr<ImuData> data) const;
+
   std::shared_ptr<Channels> channels_;
 
   motion_callback_t motion_callback_;
@@ -61,6 +67,9 @@ class Motions {
 
   int accel_range;
   int gyro_range;
+
+  std::int32_t proc_mode_;
+  std::shared_ptr<MotionIntrinsics> motion_intrinsics_;
 };
 
 MYNTEYE_END_NAMESPACE