started graph loc plotter

localization/graph_vio/TODO.txt

@@ -24,15 +24,14 @@
     - add examples package! make sure these compile!!!
       - add unit tests for these!!!!
 
+- start plotting graph loc!! (AAAA)
+
 - start plotting VIO!
     - finish plot vio script! 
             - add initilizing first vio value using groundtruth value! 
               - why is first pose start way off??
                 - print adjusted pose positions! (AAA)
 
-            - add integrated velocities plot! (B)
-              - add positions object! use for pose!
-
             - add integrated imu values??? (C)
 
             - add plotting num features! (D)

tools/localization_analysis/scripts/graph_loc_state.py

@@ -0,0 +1,34 @@
+#!/usr/bin/python
+#
+# Copyright (c) 2017, United States Government, as represented by the
+# Administrator of the National Aeronautics and Space Administration.
+#
+# All rights reserved.
+#
+# The Astrobee platform is licensed under the Apache License, Version 2.0
+# (the "License"); you may not use this file except in compliance with the
+# License. You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+# License for the specific language governing permissions and limitations
+# under the License.
+
+# GraphLocState object containing information from a GraphLocState Msg
+class GraphLocState():
+    def __init__(self):
+        self.timestamp = None
+        self.pose_with_covariance = None
+        self.num_detected_ar_features = None
+        self.num_detected_ml_features = None
+        self.optimization_iterations = None
+        self.optimization_time = None
+        self.update_time = None
+        self.num_factors = None
+        self.num_ml_projection_factors = None
+        self.num_ml_pose_factors = None
+        self.num_states = None
+        # TODO: change this using bag start time??

tools/localization_analysis/scripts/loc_results_plotter.py

@@ -0,0 +1,212 @@
+#!/usr/bin/python
+#
+# Copyright (c) 2017, United States Government, as represented by the
+# Administrator of the National Aeronautics and Space Administration.
+#
+# All rights reserved.
+#
+# The Astrobee platform is licensed under the Apache License, Version 2.0
+# (the "License"); you may not use this file except in compliance with the
+# License. You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+# License for the specific language governing permissions and limitations
+# under the License.
+"""
+Creates plots for Localization results. Adds plots for poses and detectected
+features counts. Optionally plots groundtruth poses along with other pose data if groundtruth is provided.
+"""
+
+import argparse
+import os
+import sys
+
+import message_reader 
+from multipose_plotter import MultiPosePlotter
+from multivector3d_plotter import MultiVector3dPlotter
+import plot_conversions
+from timestamped_pose import TimestampedPose
+#import plotting_utilities
+
+import matplotlib
+matplotlib.use("pdf")
+import csv
+import math
+
+import geometry_msgs
+import matplotlib.pyplot as plt
+import rosbag
+from matplotlib.backends.backend_pdf import PdfPages
+
+def plot_loc_results(
+    pdf,
+    groundtruth_poses,
+    graph_loc_states,
+):
+    poses_plotter = MultiPosePlotter("Time (s)", "Position (m)", "Graph vs. Groundtruth Position", True)
+    poses_plotter.add_poses(
+        "Groundtruth Poses", 
+        groundtruth_poses,
+        linestyle="None",
+        marker="o",
+        markeredgewidth=0.1,
+        markersize=1.5,
+    )
+
+    graph_loc_poses = plot_conversions.poses_from_graph_loc_states(graph_loc_states) 
+    poses_plotter.add_poses(
+        "Graph Loc Poses", 
+        graph_loc_poses,
+        linestyle="-",
+    )
+
+#    if ar_tag_poses.times:
+#        position_plotter.add_pose_position(
+#            ar_tag_poses,
+#            linestyle="None",
+#            marker="x",
+#            markeredgewidth=0.1,
+#            markersize=1.5,
+#        )
+    poses_plotter.plot(pdf)
+
+    #of_count_plotter = plot_conversions.optical_flow_feature_count_plotter_from_graph_loc_states(graph_loc_states)
+    #of_count_plotter.plot(pdf)
+
+    #of_num_factors_plotter = plot_conversions.optical_flow_factor_count_plotter_from_graph_loc_states(graph_loc_states)
+    #of_num_factors_plotter.plot(pdf)
+
+#
+#    # Imu Augmented Loc vs. Loc
+#    position_plotter = vector3d_plotter.Vector3dPlotter(
+#        "Time (s)", "Position (m)", "Graph vs. IMU Augmented Graph Position", True
+#    )
+#    position_plotter.add_pose_position(
+#        graph_localization_states,
+#        linestyle="None",
+#        marker="o",
+#        markeredgewidth=0.1,
+#        markersize=1.5,
+#    )
+#
+#    position_plotter.add_pose_position(
+#        imu_augmented_graph_localization_poses, linewidth=0.5
+#    )
+#    position_plotter.plot(pdf)
+#
+#    # orientations
+#    orientation_plotter = vector3d_plotter.Vector3dPlotter(
+#        "Time (s)",
+#        "Orientation (deg)",
+#        "Graph vs. IMU Augmented Graph Orientation",
+#        True,
+#    )
+#    orientation_plotter.add_pose_orientation(
+#        graph_localization_states, marker="o", markeredgewidth=0.1, markersize=1.5
+#    )
+#    orientation_plotter.add_pose_orientation(
+#        imu_augmented_graph_localization_poses, linewidth=0.5
+#    )
+#    orientation_plotter.plot(pdf)
+#
+
+
+# Loads poses from the provided bagfile, generates plots, and saves results to a pdf and csv file.
+# The csv file contains results in (TODO: define format).
+# The RMSE rel start and end time define the time range for evaluating the RMSE, in case the bag starts or ends with little/uninteresting movement 
+# that shouldn't be included in the RMSE calculation.
+# The groundtruth bag must have the same start time as other bagfile, otherwise RMSE calculations will be flawed
+def load_data_and_create_loc_plots(
+    bagfile,
+    output_pdf_file,
+    output_csv_file="loc_results.csv",
+    groundtruth_bagfile=None,
+    rmse_rel_start_time=0,
+    rmse_rel_end_time=-1,
+):
+    # Load bagfile with localization results
+    bag = rosbag.Bag(bagfile)
+    # Load bagfile with groundtruth poses. Assume groundtruth is in the same results bag
+    # if no separate bagfile for groundtruth is provided.
+    groundtruth_bag = rosbag.Bag(groundtruth_bagfile) if groundtruth_bagfile else bag
+    bag_start_time = bag.get_start_time()
+
+    # Load groundtruth poses
+    # Use sparse mapping poses as groundtruth. 
+    groundtruth_poses = []
+    message_reader.load_poses(groundtruth_poses, "/sparse_mapping/pose", groundtruth_bag, bag_start_time)
+
+
+    # Load graph localization states 
+    graph_loc_states = []
+    message_reader.load_graph_loc_states(graph_loc_states, "/graph_loc/state", bag, bag_start_time)
+    bag.close()
+
+    with PdfPages(output_pdf_file) as pdf:
+        plot_loc_results(
+            pdf,
+            groundtruth_poses,
+            #ar_tag_poses,
+            graph_loc_states,
+            #imu_augmented_graph_localization_states,
+        )
+    #    add_other_loc_plots(
+    #        pdf, graph_localization_states, graph_localization_states
+    #    )
+    #    plot_loc_state_stats(
+    #    pdf,
+    #    graph_localization_states,
+    #    groundtruth_poses,
+    #    output_csv_file,
+    #    rmse_rel_start_time=rmse_rel_start_time,
+    #    rmse_rel_end_time=rmse_rel_end_time,
+    #)
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(
+        description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter
+    )
+    parser.add_argument("bagfile", help="Input bagfile.")
+    parser.add_argument("--output-file", default="output.pdf", help="Output pdf file.")
+    parser.add_argument(
+        "--output-csv-file",
+        default="results.csv",
+        help="Output csv file containing localization stats.",
+    )
+    parser.add_argument(
+        "-g",
+        "--groundtruth-bagfile",
+        default=None,
+        help="Optional bagfile containing groundtruth poses to use as a comparison for poses in the input bagfile. If none provided, sparse mapping poses are used as groundtruth from the input bagfile if available.",
+    )
+    parser.add_argument(
+        "--rmse-rel-start-time",
+        type=float,
+        default=0,
+        help="Optional start time for plots.",
+    )
+    parser.add_argument(
+        "--rmse-rel-end-time",
+        type=float,
+        default=-1,
+        help="Optional end time for plots.",
+    )
+    args = parser.parse_args()
+    if not os.path.isfile(args.bagfile):
+        print(("Bag file " + args.bagfile + " does not exist."))
+        sys.exit()
+    if args.groundtruth_bagfile and not os.path.isfile(args.groundtruth_bagfile):
+        print(("Groundtruth Bag file " + args.groundtruth_bagfile + " does not exist."))
+        sys.exit()
+    load_data_and_create_loc_plots(
+        args.bagfile,
+        args.output_file,
+        args.output_csv_file,
+        args.groundtruth_bagfile,
+        args.rmse_rel_start_time,
+        args.rmse_rel_end_time,
+    )

tools/localization_analysis/scripts/message_conversions.py

@@ -19,6 +19,7 @@
 
 import numpy as np
 from accelerometer_bias import AccelerometerBias
+from graph_loc_state import GraphLocState
 from graph_vio_state import GraphVIOState
 from gyroscope_bias import GyroscopeBias
 from imu_bias import ImuBias
@@ -98,3 +99,19 @@ def graph_vio_state_from_msg(msg, bag_start_time=0):
     graph_vio_state.num_detected_of_features = msg.num_detected_of_features
     graph_vio_state.num_of_factors = msg.num_of_factors
     return graph_vio_state
+
+# Create a graph loc state from a msg using relative bag time. 
+def graph_loc_state_from_msg(msg, bag_start_time=0):
+    graph_loc_state = GraphLocState()
+    graph_loc_state.timestamp = relative_timestamp(msg.header.stamp, bag_start_time)
+    graph_loc_state.pose_with_covariance = pose_with_covariance_from_msg(msg.pose)
+    graph_loc_state.num_detected_ar_features = msg.num_detected_ar_features
+    graph_loc_state.num_detected_ml_features = msg.num_detected_ml_features
+    graph_loc_state.optimization_iterations = msg.optimization_iterations
+    graph_loc_state.optimization_time = msg.optimization_time
+    graph_loc_state.update_time = msg.update_time
+    graph_loc_state.num_factors = msg.num_factors
+    graph_loc_state.num_ml_projection_factors = msg.num_ml_projection_factors
+    graph_loc_state.num_ml_pose_factors = msg.num_ml_pose_factors
+    graph_loc_state.num_states = msg.num_states
+    return graph_loc_state

tools/localization_analysis/scripts/message_reader.py

@@ -38,6 +38,15 @@ def load_graph_vio_states(graph_vio_states, topic, bag, bag_start_time):
         if msg_topic == topic:
             graph_vio_states.append(message_conversions.graph_vio_state_from_msg(msg, bag_start_time))
 
+# Load graph loc states from a bag file for a given topic.
+# Start at the provided bag start time.
+def load_graph_loc_states(graph_loc_states, topic, bag, bag_start_time):
+    for msg_topic, msg, t in bag.read_messages(topic):
+        if msg_topic == topic:
+            graph_loc_states.append(message_conversions.graph_loc_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.