Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add missing import of Pose3 #858

Merged
merged 1 commit into from
Aug 24, 2021
Merged

Add missing import of Pose3 #858

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
106 changes: 61 additions & 45 deletions python/gtsam/utils/plot.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=unused-import

import gtsam
from gtsam import Marginals, Point3, Pose2, Values
from gtsam import Marginals, Point3, Pose2, Pose3, Values


def set_axes_equal(fignum: int) -> None:
Expand Down Expand Up @@ -39,9 +39,8 @@ def set_axes_equal(fignum: int) -> None:
ax.set_zlim3d([origin[2] - radius, origin[2] + radius])


def ellipsoid(
rx: float, ry: float, rz: float, n: int
) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
def ellipsoid(rx: float, ry: float, rz: float,
n: int) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
"""
Numpy equivalent of Matlab's ellipsoid function.

Expand All @@ -54,18 +53,21 @@ def ellipsoid(
Returns:
The points in the x, y and z axes to use for the surface plot.
"""
u = np.linspace(0, 2*np.pi, n+1)
v = np.linspace(0, np.pi, n+1)
u = np.linspace(0, 2 * np.pi, n + 1)
v = np.linspace(0, np.pi, n + 1)
x = -rx * np.outer(np.cos(u), np.sin(v)).T
y = -ry * np.outer(np.sin(u), np.sin(v)).T
z = -rz * np.outer(np.ones_like(u), np.cos(v)).T

return x, y, z


def plot_covariance_ellipse_3d(
axes, origin: Point3, P: np.ndarray, scale: float = 1, n: int = 8, alpha: float = 0.5
) -> None:
def plot_covariance_ellipse_3d(axes,
origin: Point3,
P: np.ndarray,
scale: float = 1,
n: int = 8,
alpha: float = 0.5) -> None:
"""
Plots a Gaussian as an uncertainty ellipse

Expand Down Expand Up @@ -97,15 +99,16 @@ def plot_covariance_ellipse_3d(
np.kron(U[:, 2:3], zc)
n = data.shape[1]
x = data[0:n, :] + origin[0]
y = data[n:2*n, :] + origin[1]
z = data[2*n:, :] + origin[2]
y = data[n:2 * n, :] + origin[1]
z = data[2 * n:, :] + origin[2]

axes.plot_surface(x, y, z, alpha=alpha, cmap='hot')


def plot_pose2_on_axes(
axes, pose: Pose2, axis_length: float = 0.1, covariance: np.ndarray = None
) -> None:
def plot_pose2_on_axes(axes,
pose: Pose2,
axis_length: float = 0.1,
covariance: np.ndarray = None) -> None:
"""
Plot a 2D pose on given axis `axes` with given `axis_length`.

Expand Down Expand Up @@ -140,17 +143,20 @@ def plot_pose2_on_axes(
k = 5.0

angle = np.arctan2(v[1, 0], v[0, 0])
e1 = patches.Ellipse(origin, np.sqrt(w[0]*k), np.sqrt(w[1]*k),
np.rad2deg(angle), fill=False)
e1 = patches.Ellipse(origin,
np.sqrt(w[0] * k),
np.sqrt(w[1] * k),
np.rad2deg(angle),
fill=False)
axes.add_patch(e1)


def plot_pose2(
fignum: int,
pose: Pose2,
axis_length: float = 0.1,
covariance: np.ndarray = None,
axis_labels=("X axis", "Y axis", "Z axis"),
fignum: int,
pose: Pose2,
axis_length: float = 0.1,
covariance: np.ndarray = None,
axis_labels=("X axis", "Y axis", "Z axis"),
) -> plt.Figure:
"""
Plot a 2D pose on given figure with given `axis_length`.
Expand All @@ -166,7 +172,9 @@ def plot_pose2(
# get figure object
fig = plt.figure(fignum)
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)

axes.set_xlabel(axis_labels[0])
Expand All @@ -175,7 +183,10 @@ def plot_pose2(
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`.

Expand Down Expand Up @@ -222,8 +233,12 @@ def plot_point3(
return fig


def plot_3d_points(fignum, values, linespec="g*", marginals=None,
title="3D Points", axis_labels=('X axis', 'Y axis', 'Z axis')):
def plot_3d_points(fignum,
values,
linespec="g*",
marginals=None,
title="3D Points",
axis_labels=('X axis', 'Y axis', 'Z axis')):
"""
Plots the Point3s in `values`, with optional covariances.
Finds all the Point3 objects in the given Values object and plots them.
Expand Down Expand Up @@ -251,7 +266,10 @@ def plot_3d_points(fignum, values, linespec="g*", marginals=None,
else:
covariance = None

fig = plot_point3(fignum, point, linespec, covariance,
fig = plot_point3(fignum,
point,
linespec,
covariance,
axis_labels=axis_labels)

except RuntimeError:
Expand Down Expand Up @@ -322,8 +340,7 @@ def plot_pose3(
# get figure object
fig = plt.figure(fignum)
axes = fig.gca(projection='3d')
plot_pose3_on_axes(axes, pose, P=P,
axis_length=axis_length)
plot_pose3_on_axes(axes, pose, P=P, axis_length=axis_length)

axes.set_xlabel(axis_labels[0])
axes.set_ylabel(axis_labels[1])
Expand All @@ -333,12 +350,12 @@ def plot_pose3(


def plot_trajectory(
fignum: int,
values: Values,
scale: float = 1,
marginals: Marginals = None,
title: str = "Plot Trajectory",
axis_labels: Iterable[str] = ("X axis", "Y axis", "Z axis"),
fignum: int,
values: Values,
scale: float = 1,
marginals: Marginals = None,
title: str = "Plot Trajectory",
axis_labels: Iterable[str] = ("X axis", "Y axis", "Z axis"),
) -> None:
"""
Plot a complete 2D/3D trajectory using poses in `values`.
Expand Down Expand Up @@ -368,7 +385,9 @@ def plot_trajectory(
else:
covariance = None

plot_pose2_on_axes(axes, pose, covariance=covariance,
plot_pose2_on_axes(axes,
pose,
covariance=covariance,
axis_length=scale)

# Then 3D poses, if any
Expand All @@ -380,21 +399,18 @@ def plot_trajectory(
else:
covariance = None

plot_pose3_on_axes(axes, pose, P=covariance,
axis_length=scale)
plot_pose3_on_axes(axes, pose, P=covariance, axis_length=scale)

fig.suptitle(title)
fig.canvas.set_window_title(title.lower())


def plot_incremental_trajectory(
fignum: int,
values: Values,
start: int = 0,
scale: float = 1,
marginals: Optional[Marginals] = None,
time_interval: float = 0.0
) -> None:
def plot_incremental_trajectory(fignum: int,
values: Values,
start: int = 0,
scale: float = 1,
marginals: Optional[Marginals] = None,
time_interval: float = 0.0) -> None:
"""
Incrementally plot a complete 3D trajectory using poses in `values`.

Expand Down