Use forward difference for synthetic imu measurements

wavelab · Aug 8, 2017 · 6908f80 · 6908f80
1 parent 298dfdc
commit 6908f80
Showing 1 changed file with 11 additions and 4 deletions.
diff --git a/wave_vision/src/dataset/VioDatasetGenerator.cpp b/wave_vision/src/dataset/VioDatasetGenerator.cpp
@@ -109,13 +109,20 @@ VioDataset VioDatasetGenerator::generate() {
 
     // Handle measurements
     for (auto i = 0u; i < vo.states.size(); ++i) {
-        addVoStateToDataset(i, time_start, vo.states[i], dataset);
+        const auto time_point =
+          timePointAfterStartTime(time_start, vo.states[i].time);
+
+        addVoStateToDataset(i, time_point, vo.states[i], dataset);
 
         // Calculate imu measurements
-        // (the last one is missing)
-        if (i > 0) {
+        // (use forward difference)
+        if (i + 1 < vo.states.size()) {
+            addImuStatesToDataset(
+              time_point, vo.states[i], vo.states[i + 1], dataset);
+        } else {
+            // for simplicity just copy the second-last imu reading as the last
             addImuStatesToDataset(
-              time_start, vo.states[i - 1], vo.states[i], dataset);
+              time_point, vo.states[i - 1], vo.states[i], dataset);
         }
     }