12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add missing import of Pose3

release/4.3a0
Varun Agrawal 2021-08-23 18:45:37 -04:00
parent 3d17e74eec
commit d4202a23ec
1 changed files with 61 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
python/gtsam/utils/plot.py
View File

@ -10,7 +10,7 @@ from matplotlib import patches
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=unused-import from mpl_toolkits.mplot3d import Axes3D # pylint: disable=unused-import
import gtsam import gtsam
from gtsam import Marginals, Point3, Pose2, Values from gtsam import Marginals, Point3, Pose2, Pose3, Values
def set_axes_equal(fignum: int) -> None: def set_axes_equal(fignum: int) -> None:
@ -39,9 +39,8 @@ def set_axes_equal(fignum: int) -> None:
ax.set_zlim3d([origin[2] - radius, origin[2] + radius]) ax.set_zlim3d([origin[2] - radius, origin[2] + radius])
def ellipsoid( def ellipsoid(rx: float, ry: float, rz: float,
rx: float, ry: float, rz: float, n: int n: int) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
""" """
Numpy equivalent of Matlab's ellipsoid function. Numpy equivalent of Matlab's ellipsoid function.
@ -63,9 +62,12 @@ def ellipsoid(
return x, y, z return x, y, z
def plot_covariance_ellipse_3d( def plot_covariance_ellipse_3d(axes,
axes, origin: Point3, P: np.ndarray, scale: float = 1, n: int = 8, alpha: float = 0.5 origin: Point3,
) -> None: P: np.ndarray,
scale: float = 1,
n: int = 8,
alpha: float = 0.5) -> None:
""" """
Plots a Gaussian as an uncertainty ellipse Plots a Gaussian as an uncertainty ellipse
@ -103,9 +105,10 @@ def plot_covariance_ellipse_3d(
axes.plot_surface(x, y, z, alpha=alpha, cmap='hot') axes.plot_surface(x, y, z, alpha=alpha, cmap='hot')
def plot_pose2_on_axes( def plot_pose2_on_axes(axes,
axes, pose: Pose2, axis_length: float = 0.1, covariance: np.ndarray = None pose: Pose2,
) -> None: axis_length: float = 0.1,
covariance: np.ndarray = None) -> None:
""" """
Plot a 2D pose on given axis `axes` with given `axis_length`. Plot a 2D pose on given axis `axes` with given `axis_length`.
@ -140,8 +143,11 @@ def plot_pose2_on_axes(
k = 5.0 k = 5.0
angle = np.arctan2(v[1, 0], v[0, 0]) angle = np.arctan2(v[1, 0], v[0, 0])
e1 = patches.Ellipse(origin, np.sqrt(w[0]*k), np.sqrt(w[1]*k), e1 = patches.Ellipse(origin,
np.rad2deg(angle), fill=False) np.sqrt(w[0] * k),
np.sqrt(w[1] * k),
np.rad2deg(angle),
fill=False)
axes.add_patch(e1) axes.add_patch(e1)
@ -166,7 +172,9 @@ def plot_pose2(
# get figure object # get figure object
fig = plt.figure(fignum) fig = plt.figure(fignum)
axes = fig.gca() axes = fig.gca()
plot_pose2_on_axes(axes, pose, axis_length=axis_length, plot_pose2_on_axes(axes,
pose,
axis_length=axis_length,
covariance=covariance) covariance=covariance)
axes.set_xlabel(axis_labels[0]) axes.set_xlabel(axis_labels[0])
@ -175,7 +183,10 @@ def plot_pose2(
return fig return fig
def plot_point3_on_axes(axes, point: Point3, linespec: str, P: Optional[np.ndarray] = None) -> None: def plot_point3_on_axes(axes,
point: Point3,
linespec: str,
P: Optional[np.ndarray] = None) -> None:
""" """
Plot a 3D point on given axis `axes` with given `linespec`. Plot a 3D point on given axis `axes` with given `linespec`.
@ -222,8 +233,12 @@ def plot_point3(
return fig return fig
def plot_3d_points(fignum, values, linespec="g*", marginals=None, def plot_3d_points(fignum,
title="3D Points", axis_labels=('X axis', 'Y axis', 'Z axis')): values,
linespec="g*",
marginals=None,
title="3D Points",
axis_labels=('X axis', 'Y axis', 'Z axis')):
""" """
Plots the Point3s in `values`, with optional covariances. Plots the Point3s in `values`, with optional covariances.
Finds all the Point3 objects in the given Values object and plots them. Finds all the Point3 objects in the given Values object and plots them.
@ -251,7 +266,10 @@ def plot_3d_points(fignum, values, linespec="g*", marginals=None,
else: else:
covariance = None covariance = None
fig = plot_point3(fignum, point, linespec, covariance, fig = plot_point3(fignum,
point,
linespec,
covariance,
axis_labels=axis_labels) axis_labels=axis_labels)
except RuntimeError: except RuntimeError:
@ -322,8 +340,7 @@ def plot_pose3(
# get figure object # get figure object
fig = plt.figure(fignum) fig = plt.figure(fignum)
axes = fig.gca(projection='3d') axes = fig.gca(projection='3d')
plot_pose3_on_axes(axes, pose, P=P, plot_pose3_on_axes(axes, pose, P=P, axis_length=axis_length)
axis_length=axis_length)
axes.set_xlabel(axis_labels[0]) axes.set_xlabel(axis_labels[0])
axes.set_ylabel(axis_labels[1]) axes.set_ylabel(axis_labels[1])
@ -368,7 +385,9 @@ def plot_trajectory(
else: else:
covariance = None covariance = None
plot_pose2_on_axes(axes, pose, covariance=covariance, plot_pose2_on_axes(axes,
pose,
covariance=covariance,
axis_length=scale) axis_length=scale)
# Then 3D poses, if any # Then 3D poses, if any
@ -380,21 +399,18 @@ def plot_trajectory(
else: else:
covariance = None covariance = None
plot_pose3_on_axes(axes, pose, P=covariance, plot_pose3_on_axes(axes, pose, P=covariance, axis_length=scale)
axis_length=scale)
fig.suptitle(title) fig.suptitle(title)
fig.canvas.set_window_title(title.lower()) fig.canvas.set_window_title(title.lower())
def plot_incremental_trajectory( def plot_incremental_trajectory(fignum: int,
fignum: int,
values: Values, values: Values,
start: int = 0, start: int = 0,
scale: float = 1, scale: float = 1,
marginals: Optional[Marginals] = None, marginals: Optional[Marginals] = None,
time_interval: float = 0.0 time_interval: float = 0.0) -> None:
) -> None:
""" """
Incrementally plot a complete 3D trajectory using poses in `values`. Incrementally plot a complete 3D trajectory using poses in `values`.