feat(src): added imu process api

This commit is contained in:
Osenberg 2019-03-15 19:46:20 +08:00
parent a6f7112f82
commit ccf7e7cb08
7 changed files with 160 additions and 1 deletions

View File

@ -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_;

View File

@ -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_;

View File

@ -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) {

View File

@ -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

View File

@ -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) {
@ -704,4 +705,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

View File

@ -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

View File

@ -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