added loading velocity

localization/graph_vio/TODO.txt

@@ -27,10 +27,15 @@
 - start plotting VIO!
     - finish plot vio script! (AA)
           - fix vio plotting! (AAA)
-      - plot graph vio vels
-      - plot graph vio imu biases 
-    - test vio plot script! (AB)
+            - add plottig velocity and bias values! (AAAA)
+            - add plotting imu values! (AAAAB)
+            - add initilizing first vio value using groundtruth value! (AAAC)
+              - make all future poses relative to this!
+                - add function to do this??
+            - whats going wrong??
     - add comments!
+    - rename pose node adder to relative pose node adder??
+      - update comments/readme/test/documentation?
     - update readme!
 
 - add regression tests?

tools/localization_analysis/scripts/graph_vio_state.py

@@ -22,7 +22,7 @@ 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.velocity_with_covariance = None
         #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

tools/localization_analysis/scripts/message_conversions.py

@@ -23,6 +23,7 @@
 from timestamped_pose import TimestampedPose
 from timestamped_pose_with_covariance import TimestampedPoseWithCovariance
 import timestamped_velocity 
+from velocity_with_covariance import VelocityWithCovariance
 import scipy.spatial.transform
 
 # Subtract the bag start time from the timestamp
@@ -68,15 +69,19 @@ def timestamped_pose_with_covariance_from_msg(pose_msg, bag_start_time=0):
     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):
+def timestamped_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)
+    return TimestampedVelocity(velocity_msg.velocity.x, velocity_msg.velocity.y, velocity_msg.velocity.z, timestamp)
+
+# Create a timestamped velocity from a velocity msg using relative bag time. 
+def velocity_with_covariance_from_msg(velocity_msg, bag_start_time=0):
+    return VelocityWithCovariance(velocity_msg.velocity.x, velocity_msg.velocity.y, velocity_msg.velocity.z, velocity_msg.covariance)
 
 # 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)
+    graph_vio_state.pose_with_covariance = pose_with_covariance_from_msg(msg.combined_nav_states.combined_nav_states[-1].pose)
+    graph_vio_state.velocity_with_covariance = velocity_with_covariance_from_msg(msg.combined_nav_states.combined_nav_states[-1].velocity)
     return graph_vio_state

tools/localization_analysis/scripts/velocity.py

@@ -17,6 +17,7 @@
 # License for the specific language governing permissions and limitations
 # under the License.
 
+# Velocity object
 class Velocity(object):
     def __init__(self, x, y, z):
         self.x = x

tools/localization_analysis/scripts/velocity_with_covariance.py

@@ -21,6 +21,6 @@
 
 # Class that contains a velocity and covariance.
 class VelocityWithCovariance(Velocity):
-    def __init__(self, velocity, covariance):
-        Velocity(self, velocity)
+    def __init__(self, x, y, z, covariance):
+        super(VelocityWithCovariance, self).__init__(x, y, z)
         self.covariance = covariance