feat(correspondence): support keep imu sequence

include/mynteye/api/api.h

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

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;
 }

src/mynteye/api/api.cc

@@ -518,9 +518,11 @@ std::vector<api::MotionData> 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_) {

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> &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<api::StreamData> Correspondence::GetStreamDatas(
 }
 
 std::vector<api::MotionData> 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<api::MotionData> Correspondence::GetReadyMotionDatas() {
   return result;
 }
 
+void Correspondence::KeepAccelThenGyro(std::vector<api::MotionData> &datas) {
+  auto n = datas.size();
+  if (n == 0) return;
+
+  static std::shared_ptr<ImuData> 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

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<api::StreamData> GetReadyStreamData(bool matched);
   std::vector<api::MotionData> GetReadyMotionDatas();
 
+  void KeepAccelThenGyro(std::vector<api::MotionData> &datas);  // NOLINT
+
   std::shared_ptr<Device> 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