add incremental plotting function

python/gtsam_py/python/utils/plot.py

@@ -366,3 +366,41 @@ def plot_trajectory(fignum, values, scale=1, marginals=None,
 
     fig.suptitle(title)
     fig.canvas.set_window_title(title.lower())
+
+
+def plot_incremental_trajectory(fignum, values, start=0,
+                                scale=1, marginals=None,
+                                time_interval=0.0):
+    """
+    Incrementally plot a complete 3D trajectory using poses in `values`.
+
+    Args:
+        fignum (int): Integer representing the figure number to use for plotting.
+        values (gtsam.Values): Values dict containing the poses.
+        start (int): Starting index to start plotting from.
+        scale (float): Value to scale the poses by.
+        marginals (gtsam.Marginals): Marginalized probability values of the estimation.
+            Used to plot uncertainty bounds.
+        time_interval (float): Time in seconds to pause between each rendering.
+            Used to create animation effect.
+    """
+    fig = plt.figure(fignum)
+    axes = fig.gca(projection='3d')
+
+    pose3Values = gtsam.utilities.allPose3s(values)
+    keys = gtsam.KeyVector(pose3Values.keys())
+
+    for i in range(start, len(keys)):
+        key = keys[i]
+        if values.exists(key):
+            pose_i = values.atPose3(keys[i])
+            plot_pose3(fignum, pose_i, scale)
+
+    # Update the plot space to encompass all plotted points
+    axes.autoscale()
+
+    # Set the 3 axes equal
+    set_axes_equal(fignum)
+
+    # Pause for a fixed amount of seconds
+    plt.pause(time_interval)