added of count plotter

localization/graph_vio/TODO.txt

@@ -27,11 +27,9 @@
 - start plotting VIO!
     - finish plot vio script! 
           - fix vio plotting! 
-            - add plottig gyro bias values! (AAAA)
-            - add plotting num factors! (B)
-            - add plotting num features! (C)
-
-            - add initilizing first vio value using groundtruth value! (AAAC)
+            - add plotting num factors! (A)
+            - add plotting num features! (B)
+            - add initilizing first vio value using groundtruth value! (C)
               - make all future poses relative to this!
                 - add function to do this??
 

tools/localization_analysis/scripts/graph_vio_state.py

@@ -24,6 +24,6 @@ def __init__(self):
         self.pose_with_covariance = None
         self.velocity_with_covariance = None
         self.imu_bias_with_covariance = None
-        #self.num_detected_of_features = msg.num_detected_of_features
-        #self.num_of_factors = msg.num_of_factors
+        self.num_detected_of_features = None
+        self.num_of_factors = None
         # TODO: change this using bag start time??

tools/localization_analysis/scripts/message_conversions.py

@@ -93,4 +93,6 @@ def graph_vio_state_from_msg(msg, bag_start_time=0):
     gyro_bias = GyroscopeBias(latest_state.imu_bias.gyroscope_bias.x, latest_state.imu_bias.gyroscope_bias.y, latest_state.imu_bias.gyroscope_bias.z) 
     # TODO: load covariance?
     graph_vio_state.imu_bias_with_covariance = ImuBias(accelerometer_bias, gyro_bias)
+    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

tools/localization_analysis/scripts/plot_conversions.py

@@ -18,6 +18,7 @@
 # under the License.
 
 import numpy as np
+from value_plotter import ValuePlotter
 from vector3d_plotter import Vector3dPlotter
 import scipy.spatial.transform
 
@@ -111,3 +112,8 @@ def gyro_bias_plotter_from_graph_vio_states(graph_vio_states):
   xs, ys, zs = xyz_gyro_bias_vectors_from_graph_vio_states(graph_vio_states)
   times = times_from_timestamped_objects(graph_vio_states) 
   return Vector3dPlotter("Graph VIO Gyro Bias", times, xs, ys, zs, ['X', 'Y', 'Z'])
+
+def optical_flow_feature_count_plotter_from_graph_vio_states(graph_vio_states):
+  counts = optical_flow_factor_counts_from_graph_vio_states(graph_vio_states)
+  times = times_from_timestamped_objects(graph_vio_states) 
+  return ValuePlotter("Graph VIO OF Counts", times, counts, "Time (s)", "Count", "OF")

tools/localization_analysis/scripts/value_plotter.py

@@ -0,0 +1,70 @@
+#!/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.
+
+import matplotlib
+matplotlib.use("pdf")
+import matplotlib.pyplot as plt
+import numpy as np
+
+# Class for plotting values.
+class ValuePlotter(object):
+    def __init__(
+        self,
+        title,
+        x_axis_vals,
+        y_axis_vals,
+        xlabel,
+        ylabel,
+        label="",
+        linestyle="-",
+        linewidth=1,
+        marker=None,
+        markeredgewidth=None,
+        markersize=1,
+    ):
+        self.title = title
+        self.x_axis_vals = x_axis_vals
+        self.y_axis_vals = y_axis_vals
+        self.xlabel = xlabel
+        self.ylabel = ylabel
+        self.label = label
+        self.linestyle = linestyle
+        self.linewidth = linewidth
+        self.marker = marker
+        self.markeredgewidth = markeredgewidth
+        self.markersize = markersize
+
+    # Plot values 
+    def plot(self, pdf):
+        plt.figure()
+        plt.plot(
+            self.x_axis_vals,
+            self.y_axis_vals,
+            label=self.label,
+            linestyle=self.linestyle,
+            marker=self.marker,
+            markeredgewidth=self.markeredgewidth,
+            markersize=self.markersize,
+        )
+        plt.xlabel(self.xlabel)
+        plt.ylabel(self.ylabel)
+        plt.title(self.title)
+        plt.legend(prop={"size": 6})
+        pdf.savefig()
+        plt.close()

tools/localization_analysis/scripts/vio_results_plotter.py

@@ -90,6 +90,9 @@ def plot_vio_results(
     gyro_bias_plotters.add(graph_vio_gyro_bias_plotter)
     gyro_bias_plotters.plot(pdf)
 
+    of_count_plotter = plot_conversions.optical_flow_feature_count_plotter_from_graph_vio_states(graph_vio_states)
+    of_count_plotter.plot(pdf)
+