fixed compile issues, huber k bug

localization/factor_adders/include/factor_adders/loc_factor_adder.h

@@ -149,7 +149,7 @@ int LocFactorAdder<PoseNodeAdderType>::AddLocProjectionFactor(
     LogError("AddLocProjectionFactors: Failed to get pose key.");
     return 0;
   }
-  const auto world_T_body = node_adder_->values().template Value<gtsam::Pose3>(*pose_key);
+  const auto world_T_body = node_adder_->nodes().template Value<gtsam::Pose3>(*pose_key);
   if (!world_T_body) {
     LogError("AddLocProjectionFactors: Failed to get world_T_body at timestamp "
              << matched_projections_measurement.timestamp << ".");

localization/factor_adders/include/factor_adders/vo_smart_projection_factor_adder.h

@@ -147,7 +147,7 @@ int VoSmartProjectionFactorAdder<PoseNodeAdderType>::AddMeasurementBasedFactors(
   int num_added_factors = 0;
   num_added_factors = AddFactorsUsingDownsampledMeasurements(factors);
   // Attempt to fix broken factors if enabled
-  if (params_.fix_invalid_factors) FixSmartFactors(node_adder_->values().gtsam_values(), factors);
+  if (params_.fix_invalid_factors) FixSmartFactors(node_adder_->nodes().gtsam_values(), factors);
   return num_added_factors;
 }
 

localization/graph_vio/TODO.txt

@@ -1,3 +1,20 @@
+- gt creation:
+  4 min 13 sec per image for matching
+      - 75 threads for ba
+      - images per bag: 6847 before remove low momemvent, 545 after
+      - images per surf base map: 2969!!!
+      - TODO: why is it only matching to last 1000 images in surf base map?? 
+        - starts with image 2970, not continuous but in increasing number (skips some numbers), goes to image 3
+        - what is asearch vs b search when finding matching tracks?? 
+          - search containers, cid_to_cid map for finding matches
+      - TODO: test with maxed threads, much faster matching per image?? 
+          - max threads (96) (0 image speed): 6m 49 sec
+          - 75 threads: 6m 20 sec 
+      - TODO: change jacobian to be analytic! (AB)
+      - TODO: find smaller base surf map????
+        - check sizes of other gt surf maps in maps dir! what is the range? (AA)
+          - 2972, similar
+
 - update tutorial with simple loc and odom!
   - update simple localizer section!
   - add simple odometry section!
@@ -8,8 +25,18 @@
       - add unit tests for these!!!!
 
 - start plotting VIO!
+    - run new localizer using a bag file, save results to bag!
+      - does this already exist??? test! (AAAA)
     - add super init calls to inherited pose states!
-    - finish plot vio script! test! (AA)
+    - finish plot vio script! (AA)
+      - Avoid iterating through bag multiple times!!!
+        - add bag reader that has callback for certain msg types!!!! (AAAAA)
+      - load groundtruth poses! -> save to poses vector! (A)
+      - load graph_vio_states! (B)
+      - plot graph_vio_states poses vs groundtruth
+      - plot graph vio vels
+      - plot graph vio imu biases 
+    - test vio plot script! (AB)
     - add comments!
     - update readme!
 

localization/nodes/include/nodes/timestamped_nodes.h

@@ -22,6 +22,9 @@
 
 namespace nodes {
 template <typename NodeType>
+// Container for timestamped nodes with a single value per node.
+// Stores the templated NodeType at various timestamps for use with a graph optimizer.
+// Child class of TimestampedCombinedNodes which handles multiple values per node/timestamp.
 class TimestampedNodes : public TimestampedCombinedNodes<NodeType> {
   using Base = TimestampedCombinedNodes<NodeType>;
 

localization/parameter_reader/src/node_adders.cc

@@ -76,8 +76,10 @@ void LoadPoseNodeAdderParams(config_reader::ConfigReader& config, na::PoseNodeAd
                              const std::string& prefix) {
   LOAD_PARAM(params.starting_prior_translation_stddev, config, prefix);
   LOAD_PARAM(params.starting_prior_quaternion_stddev, config, prefix);
-  params.SetStartNoiseModels();
+  LOAD_PARAM(params.starting_prior_translation_stddev, config, prefix);
   LoadBaseTimestampedNodeAdderParams<gtsam::Pose3>(config, params, prefix);
+  // Set start noise models after params have been loaded, since this relies on huber_k value
+  params.SetStartNoiseModels();
   // Load start node from measurements
 }
 }  // namespace parameter_reader

tools/localization_analysis/scripts/bag_utilities.py

@@ -24,57 +24,56 @@
 
 # Load poses from a bag file for a set of poses with desired topics.
 # Start at the provided bag start time.
-def load_pose_msgs(vec_of_poses, bag, bag_start_time):
-    topics = [poses.topic for poses in vec_of_poses]
+def load_pose_msgs(poses_vector, topics, bag, bag_start_time):
     for topic, msg, t in bag.read_messages(topics):
-        for poses in vec_of_poses:
-            if poses.topic == topic:
+        for poses, pose_topic in zip(poses_vector, topics):
+            if pose_topic == topic:
                 poses.add_msg(msg, msg.header.stamp, bag_start_time)
                 break
 
 # Load odometry poses from a bag file for a set of odometry poses with desired topics.
 # Start at the provided bag start time.
-def load_odometry_msgs(vec_of_poses, bag, bag_start_time):
-    topics = [poses.topic for poses in vec_of_poses]
-    for topic, msg, t in bag.read_messages(topics):
-        for poses in vec_of_poses:
-            if poses.topic == topic:
-                poses.add_msg_with_covariance(
-                    msg.odometry.body_F_source_T_target,
-                    msg.header.stamp,
-                    bag_start_time,
-                )
-                break
+#def load_odometry_msgs(vec_of_poses, bag, bag_start_time):
+#    topics = [poses.topic for poses in vec_of_poses]
+#    for topic, msg, t in bag.read_messages(topics):
+#        for poses in vec_of_poses:
+#            if poses.topic == topic:
+#                poses.add_msg_with_covariance(
+#                    msg.odometry.body_F_source_T_target,
+#                    msg.header.stamp,
+#                    bag_start_time,
+#                )
+#                break
 
 # Loads covariances from a bag file given a set of covariance topics.
 # Start at the provided bag start time.
-def load_pose_with_cov_msgs(vec_of_poses, bag, bag_start_time):
-    topics = [poses.topic for poses in vec_of_poses]
-    for topic, msg, t in bag.read_messages(topics):
-        for poses in vec_of_poses:
-            if poses.topic == topic:
-                poses.add_msg_with_covariance(
-                    msg.pose, msg.header.stamp, bag_start_time
-                )
-                break
-
-# Loads localization states from a bag file given a set of localization topics.
-# Start at the provided bag start time.
-def load_loc_state_msgs(vec_of_loc_states, bag, bag_start_time):
-    topics = [loc_states.topic for loc_states in vec_of_loc_states]
-    for topic, msg, t in bag.read_messages(topics):
-        for loc_states in vec_of_loc_states:
-            if loc_states.topic == topic:
-                loc_states.add_loc_state(msg, bag_start_time)
-                break
-
-# Loads velocities from a bag file given a set of velocity topics.
-# Start at the provided bag start time.
-def load_velocity_msgs(velocities, bag, bag_start_time):
-    topics = [velocities.topic]
-    for topic, msg, t in bag.read_messages(topics):
-        velocities.add_msg(msg, msg.header.stamp, bag_start_time)
-
+#def load_pose_with_cov_msgs(vec_of_poses, bag, bag_start_time):
+#    topics = [poses.topic for poses in vec_of_poses]
+#    for topic, msg, t in bag.read_messages(topics):
+#        for poses in vec_of_poses:
+#            if poses.topic == topic:
+#                poses.add_msg_with_covariance(
+#                    msg.pose, msg.header.stamp, bag_start_time
+#                )
+#                break
+#
+## Loads localization states from a bag file given a set of localization topics.
+## Start at the provided bag start time.
+#def load_loc_state_msgs(vec_of_loc_states, bag, bag_start_time):
+#    topics = [loc_states.topic for loc_states in vec_of_loc_states]
+#    for topic, msg, t in bag.read_messages(topics):
+#        for loc_states in vec_of_loc_states:
+#            if loc_states.topic == topic:
+#                loc_states.add_loc_state(msg, bag_start_time)
+#                break
+#
+## Loads velocities from a bag file given a set of velocity topics.
+## Start at the provided bag start time.
+#def load_velocity_msgs(velocities, bag, bag_start_time):
+#    topics = [velocities.topic]
+#    for topic, msg, t in bag.read_messages(topics):
+#        velocities.add_msg(msg, msg.header.stamp, bag_start_time)
+#
 # Returns if the topic is available in the bag file
 def has_topic(bag, topic):
     topics = bag.get_type_and_topic_info().topics

tools/localization_analysis/scripts/vio_results_plotter.py

@@ -159,7 +159,7 @@ def create_plots(
     bag_start_time = bag.get_start_time()
 
     # Load groundtruth poses
-    # Use sparse mapping poses as groundtruth. TODO: pass this as an option?
+    # Use sparse mapping poses as groundtruth. 
     groundtruth_poses = poses.Poses("Groundtruth", "/sparse_mapping/pose")
     groundtruth_vec_of_poses = [groundtruth_poses]
     load_pose_msgs(groundtruth_vec_of_poses, groundtruth_bag, bag_start_time)