diff --git a/python/gtsam_py/python/utils/plot.py b/python/gtsam_py/python/utils/plot.py index ce602f506..90f5fe5e7 100644 --- a/python/gtsam_py/python/utils/plot.py +++ b/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) \ No newline at end of file