From bc711479935283da17354f352ed5d71e6692da9e Mon Sep 17 00:00:00 2001 From: Ryan Soussan Date: Wed, 24 Jan 2024 22:14:05 -0800 Subject: [PATCH] added plotting graph vio poses --- .../ff_msgs/msg/CombinedNavState.msg | 2 +- localization/graph_vio/TODO.txt | 3 +- .../scripts/graph_vio_state.py | 35 ++++++------------- .../scripts/message_conversions.py | 31 +++++++++++++--- .../scripts/message_reader.py | 10 +++++- .../scripts/pose_with_covariance.py | 3 +- .../timestamped_pose_with_covariance.py | 4 +-- .../scripts/vio_results_plotter.py | 27 +++++++------- 8 files changed, 65 insertions(+), 50 deletions(-) diff --git a/communications/ff_msgs/msg/CombinedNavState.msg b/communications/ff_msgs/msg/CombinedNavState.msg index a7788b6354..f52a7026ed 100644 --- a/communications/ff_msgs/msg/CombinedNavState.msg +++ b/communications/ff_msgs/msg/CombinedNavState.msg @@ -16,6 +16,6 @@ # under the License. # Header header -geometry_msgs/PoseWithCovariance pose +geometry_msgs/PoseWithCovarianceStamped pose VelocityWithCovariance velocity ImuBiasWithCovariance imu_bias diff --git a/localization/graph_vio/TODO.txt b/localization/graph_vio/TODO.txt index 16b6fa6a0c..b9f5777785 100644 --- a/localization/graph_vio/TODO.txt +++ b/localization/graph_vio/TODO.txt @@ -26,8 +26,7 @@ - start plotting VIO! - run new localizer using a bag file, save results to bag! - - fix plotting orientations and positions in same plots! (A) - - add plotting graph vio states! (B) + - add plotting graph vio states! (A) - add super init calls to inherited pose states! - finish plot vio script! (AA) - Avoid iterating through bag multiple times!!! diff --git a/tools/localization_analysis/scripts/graph_vio_state.py b/tools/localization_analysis/scripts/graph_vio_state.py index 18e03a8374..43bb78a4e3 100644 --- a/tools/localization_analysis/scripts/graph_vio_state.py +++ b/tools/localization_analysis/scripts/graph_vio_state.py @@ -17,28 +17,13 @@ # License for the specific language governing permissions and limitations # under the License. -import pose -import timestamped_imu_bias_with_covariance -import timestamped_pose_with_covariance -import timestamped_velocity_with_covariance -import velocity - -# Object containing information from a GraphVIOState Msg -class GraphVIOState(object): - def __init__(self, msg, bag_start_time): - #TODO: update these functions to conver x_with_covariance_from_msg!! - self.pose_with_covariance = message_conversions.pose_from_msg(msg.pose, bag_start_time) - self.velocity_with_covariance = message_conversions.velocity_from_msg(msg.velocity, bag_start_time) - self.imu_bias_with_covariance = ImuBias(msg.accel_bias, msg.gyro_bias) - self.num_detected_of_features = msg.num_detected_of_features - self.num_of_factors = msg.num_of_factors - self.timestamp = msg.header.stamp - - def timestamped_pose_with_covariance(): - return TimestampedPoseWithCovariance(pose_with_covariance.pose, pose_with_covariance.covariance, timestamp) - - def timestamped_velocity_with_covariance(): - return TimestampedVelocityWithCovariance(velocity_with_covariance.velocity, velocity_with_covariance.covariance, timestamp) - - def timestamped_imu_bias_with_covariance(): - return TimestampedImuBiasWithCovariance(imu_bias_with_covariance.imu_bias, imu_bias_with_covariance.covariance, timestamp) +# GraphVIOState object containing information from a GraphVIOState Msg +class GraphVIOState(): + def __init__(self): + self.timestamp = None + self.pose_with_covariance = None + #self.velocity_with_covariance = message_conversions.velocity_from_msg(msg.velocity, bag_start_time) + #self.imu_bias_with_covariance = ImuBias(msg.accel_bias, msg.gyro_bias) + #self.num_detected_of_features = msg.num_detected_of_features + #self.num_of_factors = msg.num_of_factors + # TODO: change this using bag start time?? diff --git a/tools/localization_analysis/scripts/message_conversions.py b/tools/localization_analysis/scripts/message_conversions.py index 8f581c869f..2914827719 100644 --- a/tools/localization_analysis/scripts/message_conversions.py +++ b/tools/localization_analysis/scripts/message_conversions.py @@ -18,6 +18,8 @@ # under the License. import numpy as np +from graph_vio_state import GraphVIOState +from pose_with_covariance import PoseWithCovariance from timestamped_pose import TimestampedPose from timestamped_pose_with_covariance import TimestampedPoseWithCovariance import timestamped_velocity @@ -28,8 +30,8 @@ def relative_timestamp(timestamp, bag_start_time): return timestamp.secs + 1e-9 * timestamp.nsecs - bag_start_time -# Helper function to return orientation, position, and timestamp from a pose msg. -def pose_info_from_msg(pose_msg, bag_start_time=0): +# Helper function to return orientation and position from a pose msg. +def orientation_position_from_msg(pose_msg): orientation = scipy.spatial.transform.Rotation.from_quat( [ pose_msg.pose.orientation.x, @@ -38,24 +40,43 @@ def pose_info_from_msg(pose_msg, bag_start_time=0): pose_msg.pose.orientation.w, ] ) + return orientation, [pose_msg.pose.position.x, pose_msg.pose.position.y, pose_msg.pose.position.z] + + +# Helper function to return orientation, position, and timestamp from a pose msg. +def orientation_position_timestamp_from_msg(pose_msg, bag_start_time=0): + orientation, position = orientation_position_from_msg(pose_msg) timestamp = relative_timestamp(pose_msg.header.stamp, bag_start_time) - return orientation, [pose_msg.pose.position.x, pose_msg.pose.position.y, pose_msg.pose.position.z], timestamp + return orientation, position, timestamp # Create a timestamped pose with covariance from a pose msg using relative bag time. def timestamped_pose_from_msg(pose_msg, bag_start_time=0): - orientation, position, timestamp = pose_info_from_msg(pose_msg, bag_start_time) + orientation, position, timestamp = orientation_position_timestamp_from_msg(pose_msg, bag_start_time) return TimestampedPose(orientation, position, timestamp) +# Create a pose with covariance from a pose msg +def pose_with_covariance_from_msg(pose_msg): + orientation, position = orientation_position_from_msg(pose_msg) + return PoseWithCovariance(orientation, position, pose_msg.covariance) # Create a timestamped pose with covariance from a pose msg using relative bag time. def timestamped_pose_with_covariance_from_msg(pose_msg, bag_start_time=0): - orientation, position, timestamp = pose_info_from_msg(pose_msg, bag_start_time) + orientation, position, timestamp = orientation_position_timestamp_from_msg(pose_msg, bag_start_time) return TimestampedPoseWithCovariance(orientation, position, pose_msgs.covariance, timestamp) # Create a timestamped velocity from a velocity msg using relative bag time. def velocity_from_msg(velocity_msg, bag_start_time=0): timestamp = relative_timestamp(velocity_msg.header.stamp, bag_start_time) return TimestampedVelocity(velocity_msg.x, velocity_msg.y, velocity_msg.z, timestamp) + +# Create a graph vio state from a msg using relative bag time. +def graph_vio_state_from_msg(msg, bag_start_time=0): + graph_vio_state = GraphVIOState() + # TODO: load all combined nav states??? + # TODO: Remove combined nav state array msg? just use combined nav state []vector? + graph_vio_state.timestamp = relative_timestamp(msg.header.stamp, bag_start_time) + graph_vio_state.pose_with_covariance = pose_with_covariance_from_msg(msg.combined_nav_states.combined_nav_states[0].pose) + return graph_vio_state diff --git a/tools/localization_analysis/scripts/message_reader.py b/tools/localization_analysis/scripts/message_reader.py index 4792ee8eba..2eeef95ca0 100755 --- a/tools/localization_analysis/scripts/message_reader.py +++ b/tools/localization_analysis/scripts/message_reader.py @@ -26,11 +26,19 @@ # Load poses from a bag file for a given topic. # Start at the provided bag start time. -def load_pose_msgs(timestamped_poses, topic, bag, bag_start_time): +def load_poses(timestamped_poses, topic, bag, bag_start_time): for msg_topic, msg, t in bag.read_messages(topic): if msg_topic == topic: timestamped_poses.append(message_conversions.timestamped_pose_from_msg(msg, bag_start_time)) +# Load graph vio states from a bag file for a given topic. +# Start at the provided bag start time. +def load_graph_vio_states(graph_vio_states, topic, bag, bag_start_time): + for msg_topic, msg, t in bag.read_messages(topic): + if msg_topic == topic: + graph_vio_states.append(message_conversions.graph_vio_state_from_msg(msg, bag_start_time)) + + # 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): diff --git a/tools/localization_analysis/scripts/pose_with_covariance.py b/tools/localization_analysis/scripts/pose_with_covariance.py index 425e16b5f8..021429fe47 100644 --- a/tools/localization_analysis/scripts/pose_with_covariance.py +++ b/tools/localization_analysis/scripts/pose_with_covariance.py @@ -22,6 +22,5 @@ # Class that contains a pose and covariance. class PoseWithCovariance(Pose): def __init__(self, orientation, position, covariance): - self.orientation = orientation - self.position = position + super(PoseWithCovariance, self).__init__(orientation, position) self.covariance = covariance diff --git a/tools/localization_analysis/scripts/timestamped_pose_with_covariance.py b/tools/localization_analysis/scripts/timestamped_pose_with_covariance.py index 2e00265d32..cbc930b6c9 100644 --- a/tools/localization_analysis/scripts/timestamped_pose_with_covariance.py +++ b/tools/localization_analysis/scripts/timestamped_pose_with_covariance.py @@ -22,5 +22,5 @@ # Class that contains a timestamped pose and covariance. class TimestampedPoseWithCovariance(PoseWithCovariance): def __init__(self, orientation, position, covariance, timestamp): - PoseWithCovariance.__init__(self, orientation, position, timestamp) - self.covariance = covariance + super(TimestampedPoseWithCovariance, self).__init__(orientation, position) + self.timestamp = timestamp diff --git a/tools/localization_analysis/scripts/vio_results_plotter.py b/tools/localization_analysis/scripts/vio_results_plotter.py index 3aa98e44fa..346543fb2b 100755 --- a/tools/localization_analysis/scripts/vio_results_plotter.py +++ b/tools/localization_analysis/scripts/vio_results_plotter.py @@ -28,6 +28,7 @@ import message_reader import multipose_plotter +from timestamped_pose import TimestampedPose #import plotting_utilities import matplotlib @@ -43,7 +44,7 @@ def plot_vio_results( pdf, groundtruth_poses, - #graph_localization_states, + graph_vio_states, ): poses_plotter = multipose_plotter.MultiPosePlotter("Time (s)", "Position (m)", "Graph vs. Groundtruth Position", True) poses_plotter.add_poses( @@ -54,6 +55,14 @@ def plot_vio_results( markeredgewidth=0.1, markersize=1.5, ) + + graph_vio_poses = [TimestampedPose(graph_vio_state.pose_with_covariance.orientation, graph_vio_state.pose_with_covariance.position, graph_vio_state.timestamp) for graph_vio_state in graph_vio_states] + poses_plotter.add_poses( + "Graph VIO Poses", + graph_vio_poses, + linestyle="-", + ) + # if ar_tag_poses.times: # position_plotter.add_pose_position( # ar_tag_poses, @@ -163,18 +172,12 @@ def load_data_and_create_vio_plots( # Load groundtruth poses # Use sparse mapping poses as groundtruth. groundtruth_poses = [] - message_reader.load_pose_msgs(groundtruth_poses, "/sparse_mapping/pose", groundtruth_bag, bag_start_time) + message_reader.load_poses(groundtruth_poses, "/sparse_mapping/pose", groundtruth_bag, bag_start_time) - # Load VIO msgs - # TODO: create this!! - #graph_vio_states = vio_states.LocStates( - # "Graph Localization", "/graph_loc/state" - #) - #vec_of_vio_states = [ - # graph_vio_states - #] - #load_vio_state_msgs(vec_of_vio_states, bag, bag_start_time) + # Load graph VIO states + graph_vio_states = [] + message_reader.load_graph_vio_states(graph_vio_states, "/graph_vio/state", bag, bag_start_time) bag.close() with PdfPages(output_pdf_file) as pdf: @@ -182,7 +185,7 @@ def load_data_and_create_vio_plots( pdf, groundtruth_poses, #ar_tag_poses, - #graph_vio_states, + graph_vio_states, #imu_augmented_graph_localization_states, ) # add_other_loc_plots(