add smart factor measurements in reverse so latest measurements are a…

…lways in graph
rsoussan · Jan 30, 2024 · 5736891 · 5736891
1 parent b0b398b
commit 5736891
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 10 deletions.
diff --git a/localization/factor_adders/include/factor_adders/vo_smart_projection_factor_adder.h b/localization/factor_adders/include/factor_adders/vo_smart_projection_factor_adder.h
@@ -178,21 +178,23 @@ bool VoSmartProjectionFactorAdder<PoseNodeAdderType>::AddSmartFactor(
                                           params_.rotation_only_fallback, params_.robust, params_.huber_k);
 
   for (int i = 0; i < static_cast<int>(feature_track_points.size()); ++i) {
-    const auto& feature_point = feature_track_points[i];
+    // Start in reverse so most recent measurements are added first
+    const auto& feature_point = feature_track_points[feature_track_points.size() - 1 - i];
     if (i >= params_.max_num_points_per_factor) break;
-    // Add or get pose key
     const auto& timestamp = feature_point.timestamp;
+    // Add or get pose key
     if (!node_adder_->AddNode(timestamp, factors)) {
-      LogError("AddSmartFactor: Failed to add node for timestamp " << timestamp << ".");
+      LogError("AddSmartFactor: Failed to add node for timestamp " << std::setprecision(15) << timestamp << ".");
       return false;
     }
     const auto keys = node_adder_->Keys(timestamp);
     if (keys.empty()) {
-      LogError("AddSmartFactor: Failed to get keys for timestamp " << timestamp << ".");
+      LogError("AddSmartFactor: Failed to get keys for timestamp " << std::setprecision(15) << timestamp << ".");
       return false;
     }
     // Assumes first key is pose
     const auto& pose_key = keys[0];
+
     smart_factor->add(SmartFactorCamera::Measurement(feature_point.image_point), pose_key);
   }
   factors.push_back(smart_factor);

diff --git a/localization/graph_vio/TODO.txt b/localization/graph_vio/TODO.txt
@@ -26,15 +26,18 @@
 
 - start plotting VIO!
     - finish plot vio script! 
-          - fix vio plotting! 
-            - why are there so few vo msgs? why are failures occuring?
-            - add plotting num features! (B)
-              - how to get num detected features???
-            - add initilizing first vio value using groundtruth value! (C)
+            - why are so few smart factors added? (3, 4, etc) (A)
+              - check how many feature traacks have the correct number of points, how many factors should be added
+              - check how many factors already in the graph?? (recreating graph each time?) 
+
+            - add initilizing first vio value using groundtruth value! (B)
               - make all future poses relative to this!
                 - add function to do this??
+
+            - add integrated velocities plot! (C)
 
-            - whats going wrong??
+            - add plotting num features! (D)
+              - how to get num detected features???
 
             - add plotting acceleration values? (these come from imu messages....) (A)
               - bias corrected? raw?