From ccf7e7cb08ac7afa6566ad6447a8f7d88527e31e Mon Sep 17 00:00:00 2001 From: Osenberg Date: Fri, 15 Mar 2019 19:46:20 +0800 Subject: [PATCH] feat(src): added imu process api --- include/mynteye/api/api.h | 5 ++ include/mynteye/device/device.h | 5 ++ include/mynteye/types.h | 24 ++++++++ src/mynteye/api/api.cc | 8 +++ src/mynteye/device/device.cc | 9 +++ src/mynteye/device/motions.cc | 101 +++++++++++++++++++++++++++++++- src/mynteye/device/motions.h | 9 +++ 7 files changed, 160 insertions(+), 1 deletion(-) diff --git a/include/mynteye/api/api.h b/include/mynteye/api/api.h index 8b16330..fa9ae71 100644 --- a/include/mynteye/api/api.h +++ b/include/mynteye/api/api.h @@ -349,6 +349,11 @@ class MYNTEYE_API API { std::shared_ptr 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_; diff --git a/include/mynteye/device/device.h b/include/mynteye/device/device.h index e188918..f991e88 100644 --- a/include/mynteye/device/device.h +++ b/include/mynteye/device/device.h @@ -295,6 +295,11 @@ class MYNTEYE_API Device { */ std::vector 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 device() const { return device_; diff --git a/include/mynteye/types.h b/include/mynteye/types.h index 8255b39..0f8fda6 100644 --- a/include/mynteye/types.h +++ b/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(rhs); +} +inline +std::int32_t operator&(const ProcessMode& lhs, const std::int32_t& rhs) { + return static_cast(lhs) & rhs; +} +inline +std::int32_t operator&(const ProcessMode& lhs, const ProcessMode& rhs) { + return static_cast(lhs) & static_cast(rhs); +} + MYNTEYE_API const char *to_string(const Format &value); inline std::ostream &operator<<(std::ostream &os, const Format &value) { diff --git a/src/mynteye/api/api.cc b/src/mynteye/api/api.cc index 2e4fb1d..c1e1694 100644 --- a/src/mynteye/api/api.cc +++ b/src/mynteye/api/api.cc @@ -592,4 +592,12 @@ void API::CheckImageParams() { } } +void API::EnableProcessMode(const ProcessMode& mode) { + EnableProcessMode(static_cast(mode)); +} + +void API::EnableProcessMode(const std::int32_t& mode) { + device_->EnableProcessMode(mode); +} + MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/device/device.cc b/src/mynteye/device/device.cc index 0ed6d4a..97fb4a3 100644 --- a/src/mynteye/device/device.cc +++ b/src/mynteye/device/device.cc @@ -330,6 +330,7 @@ void Device::SetMotionIntrinsics(const MotionIntrinsics &in) { motion_intrinsics_ = std::make_shared(); } *motion_intrinsics_ = in; + motions_->SetMotionIntrinsics(motion_intrinsics_); } void Device::SetMotionExtrinsics(const Stream &from, const Extrinsics &ex) { @@ -704,4 +705,12 @@ bool Device::SetFiles( return channels_->SetFiles(info, img_params, imu_params); } +void Device::EnableProcessMode(const ProcessMode& mode) { + EnableProcessMode(static_cast(mode)); +} + +void Device::EnableProcessMode(const std::int32_t& mode) { + motions_->EnableProcessMode(mode); +} + MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/device/motions.cc b/src/mynteye/device/motions.cc index d7ebf93..6fbaea4 100644 --- a/src/mynteye/device/motions.cc +++ b/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), motion_callback_(nullptr), motion_datas_enabled_(false), - is_imu_tracking(false) { + is_imu_tracking(false), + proc_mode_(static_cast(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 _(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 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 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& in) { + motion_intrinsics_ = in; +} + +void Motions::EnableProcessMode(const std::int32_t& mode) { + proc_mode_ = mode; +} + MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/device/motions.h b/src/mynteye/device/motions.h index 8149a68..74f42a0 100644 --- a/src/mynteye/device/motions.h +++ b/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& in); + void EnableProcessMode(const std::int32_t& mode); + private: + void ProcImuAssembly(std::shared_ptr data) const; + void ProcImuTempDrift(std::shared_ptr data) const; + std::shared_ptr 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 motion_intrinsics_; }; MYNTEYE_END_NAMESPACE