From 3cc158231a9ad8db145f66c7f5b3b66c2fddbbff Mon Sep 17 00:00:00 2001 From: John Zhao Date: Wed, 6 Mar 2019 15:55:32 +0800 Subject: [PATCH] feat(correspondence): support keep imu sequence --- include/mynteye/api/api.h | 7 ++- .../tutorials/data/get_imu_correspondence.cc | 57 +++++++++++++++++- src/mynteye/api/api.cc | 4 +- src/mynteye/api/correspondence.cc | 58 ++++++++++++++++++- src/mynteye/api/correspondence.h | 5 ++ 5 files changed, 122 insertions(+), 9 deletions(-) diff --git a/include/mynteye/api/api.h b/include/mynteye/api/api.h index 699247c..177a641 100644 --- a/include/mynteye/api/api.h +++ b/include/mynteye/api/api.h @@ -290,7 +290,7 @@ class MYNTEYE_API API { /** * Enable the data of stream. * callback function will call before the father processor enable. - * when try_tag is true, the function will do nothing except callback. + * when try_tag is true, the function will do nothing except callback. */ void EnableStreamData( const Stream &stream, @@ -304,7 +304,7 @@ class MYNTEYE_API API { /** * Disable the data of stream. * callback function will call before the children processor disable. - * when try_tag is true, the function will do nothing except callback. + * when try_tag is true, the function will do nothing except callback. */ void DisableStreamData( const Stream &stream, @@ -334,7 +334,8 @@ class MYNTEYE_API API { /** * Enable motion datas with timestamp correspondence of some stream. */ - void EnableTimestampCorrespondence(const Stream &stream); + void EnableTimestampCorrespondence(const Stream &stream, + bool keep_accel_then_gyro = true); /** * Enable the plugin. diff --git a/samples/tutorials/data/get_imu_correspondence.cc b/samples/tutorials/data/get_imu_correspondence.cc index 9cec957..7557cfb 100644 --- a/samples/tutorials/data/get_imu_correspondence.cc +++ b/samples/tutorials/data/get_imu_correspondence.cc @@ -18,6 +18,10 @@ #include "util/cv_painter.h" +// #define CHECK_ACCEL_THEN_GYRO +#define SEQ_FIRST 1 // accel +#define SEQ_SECOND 2 // gyro + MYNTEYE_USE_NAMESPACE int main(int argc, char *argv[]) { @@ -40,6 +44,11 @@ int main(int argc, char *argv[]) { std::uint64_t prev_img_stamp = 0; std::uint64_t prev_imu_stamp = 0; +#ifdef CHECK_ACCEL_THEN_GYRO + std::uint8_t prev_imu_flag = 0; + std::uint64_t imu_count = 0; + std::uint64_t imu_disorder_count = 0; +#endif while (true) { api->WaitForStreams(); @@ -56,14 +65,49 @@ int main(int argc, char *argv[]) { auto &&motion_datas = api->GetMotionDatas(); LOG(INFO) << "Imu count: " << motion_datas.size(); - for (auto &&data : motion_datas) { + for (size_t i = 0, n = motion_datas.size() - 1; i <= n; ++i) { + auto data = motion_datas[i]; + auto imu_flag = data.imu->flag; auto imu_stamp = data.imu->timestamp; - LOG(INFO) << "Imu timestamp: " << imu_stamp + + std::stringstream ss; + if (imu_flag == 0) { // accel + gyro + ss << "Imu"; + } else if (imu_flag == 1) { // accel + ss << "Accel"; + } else if (imu_flag == 2) { // gyro + ss << "Gyro"; + } + ss << " timestamp: " << imu_stamp << ", diff_prev=" << (imu_stamp - prev_imu_stamp) - << ", diff_img=" << (1.f + imu_stamp - img_stamp); + << ", diff_img=" << (1.0f + imu_stamp - img_stamp); +#ifdef CHECK_ACCEL_THEN_GYRO + if (imu_flag > 0) { + bool ok = false; + if (i == 0) { // first + ok = (imu_flag == SEQ_FIRST); + } else if (i == n) { // last + ok = (imu_flag == SEQ_SECOND); + } else { + if (imu_flag == SEQ_FIRST) { + ok = (prev_imu_flag == SEQ_SECOND); + } else if (imu_flag == SEQ_SECOND) { + ok = (prev_imu_flag == SEQ_FIRST); + } + } + ss << (ok ? " ✓" : " x"); + if (!ok) ++imu_disorder_count; + prev_imu_flag = imu_flag; + } +#endif + LOG(INFO) << ss.str(); + prev_imu_stamp = imu_stamp; } LOG(INFO); +#ifdef CHECK_ACCEL_THEN_GYRO + imu_count += motion_datas.size(); +#endif /* painter.DrawImgData(img, *left_data.img); @@ -81,5 +125,12 @@ int main(int argc, char *argv[]) { } api->Stop(Source::ALL); + +#ifdef CHECK_ACCEL_THEN_GYRO + if (imu_disorder_count > 0) { + LOG(INFO) << "accel_then_gyro, disorder_count: " << imu_disorder_count + << "/" << imu_count; + } +#endif return 0; } diff --git a/src/mynteye/api/api.cc b/src/mynteye/api/api.cc index 3f522db..29a736a 100644 --- a/src/mynteye/api/api.cc +++ b/src/mynteye/api/api.cc @@ -518,9 +518,11 @@ std::vector API::GetMotionDatas() { } } -void API::EnableTimestampCorrespondence(const Stream &stream) { +void API::EnableTimestampCorrespondence(const Stream &stream, + bool keep_accel_then_gyro) { if (correspondence_ == nullptr) { correspondence_.reset(new Correspondence(device_, stream)); + correspondence_->KeepAccelThenGyro(keep_accel_then_gyro); { device_->DisableMotionDatas(); if (callback_) { diff --git a/src/mynteye/api/correspondence.cc b/src/mynteye/api/correspondence.cc index d4af6cd..9b0ae85 100644 --- a/src/mynteye/api/correspondence.cc +++ b/src/mynteye/api/correspondence.cc @@ -16,11 +16,15 @@ #include "mynteye/device/device.h" #include "mynteye/logger.h" +#define MYNTEYE_IMU_SEQ_FIRST 1 // accel +#define MYNTEYE_IMU_SEQ_SECOND 2 // gyro + MYNTEYE_BEGIN_NAMESPACE Correspondence::Correspondence(const std::shared_ptr &device, const Stream &stream) - : device_(device), stream_(stream), ready_image_timestamp_(0) { + : device_(device), stream_(stream), ready_image_timestamp_(0), + keep_accel_then_gyro_(false) { VLOG(2) << __func__; // set matched stream to be watched too, // aim to make stream and matched stream correspondence @@ -54,6 +58,10 @@ bool Correspondence::Watch(const Stream &stream) const { return false; } +void Correspondence::KeepAccelThenGyro(bool enabled) { + keep_accel_then_gyro_ = enabled; +} + void Correspondence::OnStreamDataCallback( const Stream &stream, const api::StreamData &data) { if (!Watch(stream)) { @@ -143,7 +151,11 @@ std::vector Correspondence::GetStreamDatas( } std::vector Correspondence::GetMotionDatas() { - return GetReadyMotionDatas(); + auto &&datas = GetReadyMotionDatas(); + if (keep_accel_then_gyro_ && device_->GetModel() != Model::STANDARD) { + KeepAccelThenGyro(datas); // only s2 need do this + } + return datas; } void Correspondence::EnableStreamMatch() { @@ -274,4 +286,46 @@ std::vector Correspondence::GetReadyMotionDatas() { return result; } +void Correspondence::KeepAccelThenGyro(std::vector &datas) { + auto n = datas.size(); + if (n == 0) return; + + static std::shared_ptr last_imu = nullptr; + + std::uint8_t prev_flag = 0; + for (auto it = datas.begin(); it != datas.end(); ) { + auto flag = it->imu->flag; + if (flag == 0) { + ++it; // unexpected + continue; + } + + bool is_first = (it == datas.begin()); + bool is_last = (it == datas.end() - 1); + bool ok = false; + if (is_first) { + ok = (flag == MYNTEYE_IMU_SEQ_FIRST); + } else { + if (flag == MYNTEYE_IMU_SEQ_FIRST) { + ok = (prev_flag == MYNTEYE_IMU_SEQ_SECOND); + } else if (flag == MYNTEYE_IMU_SEQ_SECOND) { + ok = (prev_flag == MYNTEYE_IMU_SEQ_FIRST); + } + } + if (ok && is_last) { + ok = (flag == MYNTEYE_IMU_SEQ_SECOND); + } + + if (ok) { + prev_flag = flag; + if (is_last) { + last_imu = nullptr; + } + ++it; + } else { + it = datas.erase(it); + } + } +} + MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/api/correspondence.h b/src/mynteye/api/correspondence.h index df822e0..cdc4682 100644 --- a/src/mynteye/api/correspondence.h +++ b/src/mynteye/api/correspondence.h @@ -32,6 +32,7 @@ class Correspondence { ~Correspondence(); bool Watch(const Stream &stream) const; + void KeepAccelThenGyro(bool enabled); void OnStreamDataCallback(const Stream &stream, const api::StreamData &data); void OnMotionDataCallback(const device::MotionData &data); @@ -55,6 +56,8 @@ class Correspondence { std::vector GetReadyStreamData(bool matched); std::vector GetReadyMotionDatas(); + void KeepAccelThenGyro(std::vector &datas); // NOLINT + std::shared_ptr device_; Stream stream_; Stream stream_match_; @@ -73,6 +76,8 @@ class Correspondence { std::condition_variable_any cond_stream_datas_; std::uint64_t ready_image_timestamp_; + + bool keep_accel_then_gyro_; }; MYNTEYE_END_NAMESPACE