Now plots 2D or 3D trajectories (on a 3D plot)

release/4.3a0
Frank dellaert 2020-08-20 23:25:51 -04:00
parent 84e0bc5351
commit af7ced4112
1 changed files with 31 additions and 40 deletions

View File

@ -1,9 +1,11 @@
"""Various plotting utlities.""" """Various plotting utlities."""
import numpy as np # pylint: disable=no-member, invalid-name
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
import numpy as np
from matplotlib import patches from matplotlib import patches
from mpl_toolkits.mplot3d import Axes3D from mpl_toolkits.mplot3d import Axes3D # pylint: disable=unused-import
import gtsam import gtsam
@ -318,47 +320,36 @@ def plot_trajectory(fignum, values, scale=1, marginals=None,
title (string): The title of the plot. title (string): The title of the plot.
axis_labels (iterable[string]): List of axis labels to set. axis_labels (iterable[string]): List of axis labels to set.
""" """
pose3Values = gtsam.utilities.allPose3s(values) fig = plt.figure(fignum)
keys = gtsam.KeyVector(pose3Values.keys()) axes = fig.gca(projection='3d')
lastKey = None
for key in keys: axes.set_xlabel(axis_labels[0])
try: axes.set_ylabel(axis_labels[1])
pose = pose3Values.atPose3(key) axes.set_zlabel(axis_labels[2])
except:
print("Warning: no Pose3 at key: {0}".format(key))
if lastKey is not None: # Plot 2D poses, if any
try: poses = gtsam.utilities.allPose2s(values)
lastPose = pose3Values.atPose3(lastKey) for key in poses.keys():
except: pose = poses.atPose2(key)
print("Warning: no Pose3 at key: {0}".format(lastKey)) if marginals:
pass covariance = marginals.marginalCovariance(key)
else:
covariance = None
if marginals: plot_pose2_on_axes(axes, pose, covariance=covariance,
covariance = marginals.marginalCovariance(lastKey) axis_length=scale)
else:
covariance = None
fig = plot_pose3(fignum, lastPose, P=covariance, # Then 3D poses, if any
axis_length=scale, axis_labels=axis_labels) poses = gtsam.utilities.allPose3s(values)
for key in poses.keys():
pose = poses.atPose3(key)
if marginals:
covariance = marginals.marginalCovariance(key)
else:
covariance = None
lastKey = key plot_pose3_on_axes(axes, pose, P=covariance,
axis_length=scale)
# Draw final pose
if lastKey is not None:
try:
lastPose = pose3Values.atPose3(lastKey)
if marginals:
covariance = marginals.marginalCovariance(lastKey)
else:
covariance = None
fig = plot_pose3(fignum, lastPose, P=covariance,
axis_length=scale, axis_labels=axis_labels)
except:
pass
fig.suptitle(title) fig.suptitle(title)
fig.canvas.set_window_title(title.lower()) fig.canvas.set_window_title(title.lower())
@ -383,8 +374,8 @@ def plot_incremental_trajectory(fignum, values, start=0,
fig = plt.figure(fignum) fig = plt.figure(fignum)
axes = fig.gca(projection='3d') axes = fig.gca(projection='3d')
pose3Values = gtsam.utilities.allPose3s(values) poses = gtsam.utilities.allPose3s(values)
keys = gtsam.KeyVector(pose3Values.keys()) keys = gtsam.KeyVector(poses.keys())
for key in keys[start:]: for key in keys[start:]:
if values.exists(key): if values.exists(key):