added plotting graph vio poses

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

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!!!

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??

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

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):

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

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

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,26 +172,20 @@ 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:
         plot_vio_results(
             pdf,
             groundtruth_poses,
             #ar_tag_poses,
-            #graph_vio_states,
+            graph_vio_states,
             #imu_augmented_graph_localization_states,
         )
     #    add_other_loc_plots(