feat(correspondence): improve keep imu sequence

samples/tutorials/data/get_imu_correspondence.cc

@@ -48,6 +48,7 @@ int main(int argc, char *argv[]) {
   std::uint8_t prev_imu_flag = 0;
   std::uint64_t imu_count = 0;
   std::uint64_t imu_disorder_count = 0;
+  bool exit = false;
 #endif
   while (true) {
     api->WaitForStreams();
@@ -98,6 +99,9 @@ int main(int argc, char *argv[]) {
         ss << (ok ? " ✓" : " x");
         if (!ok) ++imu_disorder_count;
         prev_imu_flag = imu_flag;
+        if (!exit) {
+          if (!ok) exit = true;
+        }
       }
 #endif
       LOG(INFO) << ss.str();
@@ -107,6 +111,7 @@ int main(int argc, char *argv[]) {
     LOG(INFO);
 #ifdef CHECK_ACCEL_THEN_GYRO
     imu_count += motion_datas.size();
+    if (exit) break;
 #endif
 
     /*
@@ -130,6 +135,8 @@ int main(int argc, char *argv[]) {
   if (imu_disorder_count > 0) {
     LOG(INFO) << "accel_then_gyro, disorder_count: " << imu_disorder_count
         << "/" << imu_count;
+  } else {
+    LOG(INFO) << "accel_then_gyro, ok";
   }
 #endif
   return 0;

src/mynteye/api/correspondence.cc

@@ -152,6 +152,22 @@ std::vector<api::StreamData> Correspondence::GetStreamDatas(
 
 std::vector<api::MotionData> Correspondence::GetMotionDatas() {
   auto &&datas = GetReadyMotionDatas();
+  /*
+  for (auto data : datas) {
+    auto imu_flag = data.imu->flag;
+    auto imu_stamp = data.imu->timestamp;
+    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;
+    LOG(INFO) << ss.str();
+  }
+  */
   if (keep_accel_then_gyro_ && device_->GetModel() != Model::STANDARD) {
     KeepAccelThenGyro(datas);  // only s2 need do this
   }
@@ -287,16 +303,30 @@ std::vector<api::MotionData> Correspondence::GetReadyMotionDatas() {
 }
 
 void Correspondence::KeepAccelThenGyro(std::vector<api::MotionData> &datas) {
-  auto n = datas.size();
+  if (datas.size() == 0) return;
-  if (n == 0) return;
 
   static std::shared_ptr<ImuData> last_imu = nullptr;
 
+  // process last imu
+  if (datas[0].imu->flag == MYNTEYE_IMU_SEQ_SECOND) {
+    if (last_imu && last_imu->flag == MYNTEYE_IMU_SEQ_FIRST) {
+      datas.insert(datas.begin(), {last_imu});
+    }
+  }
+  last_imu = nullptr;
+
+  // if only one
+  if (datas.size() == 1) {
+    last_imu = datas[0].imu;
+    datas.clear();
+    return;
+  }
+
   std::uint8_t prev_flag = 0;
   for (auto it = datas.begin(); it != datas.end(); ) {
     auto flag = it->imu->flag;
     if (flag == 0) {
-      ++it;  // unexpected
+      ++it;  // unexpected, keep it
       continue;
     }
 
@@ -312,20 +342,26 @@ void Correspondence::KeepAccelThenGyro(std::vector<api::MotionData> &datas) {
         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 {
+      if (is_last) {
+        // if tail not ok, retain last imu
+        last_imu = it->imu;
+      }
       it = datas.erase(it);
     }
   }
+
+  // if tail is not second
+  if (datas.size() > 0) {
+    auto it = datas.end() - 1;
+    if (it->imu->flag != MYNTEYE_IMU_SEQ_SECOND) {
+      datas.erase(it);
+    }
+  }
 }
 
 MYNTEYE_END_NAMESPACE