From 78e41255c80f83ef3fcdc48abf0f4061fea46713 Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Thu, 11 Jan 2024 17:23:31 -0800 Subject: [PATCH] fixed compile issues, huber k bug --- .../include/factor_adders/loc_factor_adder.h | 2 +- .../vo_smart_projection_factor_adder.h | 2 +- localization/graph_vio/TODO.txt | 29 ++++++- .../nodes/include/nodes/timestamped_nodes.h | 3 + .../parameter_reader/src/node_adders.cc | 4 +- .../scripts/bag_utilities.py | 83 +++++++++---------- .../scripts/vio_results_plotter.py | 2 +- 7 files changed, 78 insertions(+), 47 deletions(-) diff --git a/localization/factor_adders/include/factor_adders/loc_factor_adder.h b/localization/factor_adders/include/factor_adders/loc_factor_adder.h index d1000a0e8a..4d59dc3e41 100644 --- a/localization/factor_adders/include/factor_adders/loc_factor_adder.h +++ b/localization/factor_adders/include/factor_adders/loc_factor_adder.h @@ -149,7 +149,7 @@ int LocFactorAdder::AddLocProjectionFactor( LogError("AddLocProjectionFactors: Failed to get pose key."); return 0; } - const auto world_T_body = node_adder_->values().template Value(*pose_key); + const auto world_T_body = node_adder_->nodes().template Value(*pose_key); if (!world_T_body) { LogError("AddLocProjectionFactors: Failed to get world_T_body at timestamp " << matched_projections_measurement.timestamp << "."); 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 index 06af1ce0ee..7053e41aea 100644 --- 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 @@ -147,7 +147,7 @@ int VoSmartProjectionFactorAdder::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; } diff --git a/localization/graph_vio/TODO.txt b/localization/graph_vio/TODO.txt index 1269ef88e1..138aa8207d 100644 --- a/localization/graph_vio/TODO.txt +++ b/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! diff --git a/localization/nodes/include/nodes/timestamped_nodes.h b/localization/nodes/include/nodes/timestamped_nodes.h index 6347f35d31..b496faf78d 100644 --- a/localization/nodes/include/nodes/timestamped_nodes.h +++ b/localization/nodes/include/nodes/timestamped_nodes.h @@ -22,6 +22,9 @@ namespace nodes { template +// 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 { using Base = TimestampedCombinedNodes; diff --git a/localization/parameter_reader/src/node_adders.cc b/localization/parameter_reader/src/node_adders.cc index ecc31efe13..ee6d648c73 100644 --- a/localization/parameter_reader/src/node_adders.cc +++ b/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(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 diff --git a/tools/localization_analysis/scripts/bag_utilities.py b/tools/localization_analysis/scripts/bag_utilities.py index 205395cb38..4cad5ee819 100755 --- a/tools/localization_analysis/scripts/bag_utilities.py +++ b/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 diff --git a/tools/localization_analysis/scripts/vio_results_plotter.py b/tools/localization_analysis/scripts/vio_results_plotter.py index d9aa7dbd6a..2404e8dc89 100755 --- a/tools/localization_analysis/scripts/vio_results_plotter.py +++ b/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)