1 """Various plotting utlities."""
5 from typing
import Iterable, Optional, Tuple
7 import matplotlib.pyplot
as plt
9 from matplotlib
import patches
10 from mpl_toolkits.mplot3d
import Axes3D
13 from gtsam
import Marginals, Point2, Point3, Pose2, Pose3, Values
38 Make axes of 3D plot have equal scale so that spheres appear as spheres,
39 cubes as cubes, etc.. This is one possible solution to Matplotlib's
40 ax.set_aspect('equal') and ax.axis('equal') not working for 3D.
43 fignum: An integer representing the figure number for Matplotlib.
45 fig = plt.figure(fignum)
47 ax = fig.add_subplot(projection=
'3d')
57 origin = np.mean(limits, axis=1)
58 radius = 0.5 * np.max(np.abs(limits[:, 1] - limits[:, 0]))
60 ax.set_xlim3d([origin[0] - radius, origin[0] + radius])
61 ax.set_ylim3d([origin[1] - radius, origin[1] + radius])
62 ax.set_zlim3d([origin[2] - radius, origin[2] + radius])
66 n: int) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
68 Numpy equivalent of Matlab's ellipsoid function.
71 rx: Radius of ellipsoid in X-axis.
72 ry: Radius of ellipsoid in Y-axis.
73 rz: Radius of ellipsoid in Z-axis.
74 n: The granularity of the ellipsoid plotted.
77 The points in the x, y and z axes to use for the surface plot.
79 u = np.linspace(0, 2 * np.pi, n + 1)
80 v = np.linspace(0, np.pi, n + 1)
81 x = -rx * np.outer(np.cos(u), np.sin(v)).T
82 y = -ry * np.outer(np.sin(u), np.sin(v)).T
83 z = -rz * np.outer(np.ones_like(u), np.cos(v)).T
93 alpha: float = 0.5) ->
None:
95 Plots a Gaussian as an uncertainty ellipse
97 The ellipse is scaled in such a way that 95% of drawn samples are inliers.
98 Derivation of the scaling factor is explained at the beginning of this file.
101 axes (matplotlib.axes.Axes): Matplotlib axes.
102 origin: The origin in the world frame.
103 P: The marginal covariance matrix of the 3D point
104 which will be represented as an ellipse.
105 scale: Scaling factor of the radii of the covariance ellipse.
106 n: Defines the granularity of the ellipse. Higher values indicate finer ellipses.
107 alpha: Transparency value for the plotted surface in the range [0, 1].
111 U, S, _ = np.linalg.svd(P)
113 radii = k * np.sqrt(S)
114 radii = radii * scale
121 data = np.kron(U[:, 0:1], xc) + np.kron(U[:, 1:2], yc) + \
122 np.kron(U[:, 2:3], zc)
124 x = data[0:n, :] + origin[0]
125 y = data[n:2 * n, :] + origin[1]
126 z = data[2 * n:, :] + origin[2]
128 axes.plot_surface(x, y, z, alpha=alpha, cmap=
'hot')
132 covariance: np.ndarray) ->
None:
134 Plots a Gaussian as an uncertainty ellipse
136 The ellipse is scaled in such a way that 95% of drawn samples are inliers.
137 Derivation of the scaling factor is explained at the beginning of this file.
140 axes (matplotlib.axes.Axes): Matplotlib axes.
141 origin: The origin in the world frame.
142 covariance: The marginal covariance matrix of the 2D point
143 which will be represented as an ellipse.
146 w, v = np.linalg.eigh(covariance)
151 angle = np.arctan2(v[1, 0], v[0, 0])
154 e1 = patches.Ellipse(origin,
155 np.sqrt(w[0]) * 2 * k,
156 np.sqrt(w[1]) * 2 * k,
157 angle=np.rad2deg(angle),
165 P: Optional[np.ndarray] =
None) ->
None:
167 Plot a 2D point and its corresponding uncertainty ellipse on given axis
168 `axes` with given `linespec`.
170 The uncertainty ellipse (if covariance is given) is scaled in such a way
171 that 95% of drawn samples are inliers, see `plot_covariance_ellipse_2d`.
174 axes (matplotlib.axes.Axes): Matplotlib axes.
175 point: The point to be plotted.
176 linespec: String representing formatting options for Matplotlib.
177 P: Marginal covariance matrix to plot the uncertainty of the estimation.
179 axes.plot([point[0]], [point[1]], linespec, marker=
'.', markersize=10)
188 P: np.ndarray =
None,
189 axis_labels: Iterable[str] = (
"X axis",
"Y axis"),
192 Plot a 2D point on given figure with given `linespec`.
194 The uncertainty ellipse (if covariance is given) is scaled in such a way
195 that 95% of drawn samples are inliers, see `plot_covariance_ellipse_2d`.
198 fignum: Integer representing the figure number to use for plotting.
199 point: The point to be plotted.
200 linespec: String representing formatting options for Matplotlib.
201 P: Marginal covariance matrix to plot the uncertainty of the estimation.
202 axis_labels: List of axis labels to set.
205 fig: The matplotlib figure.
208 fig = plt.figure(fignum)
212 axes.set_xlabel(axis_labels[0])
213 axes.set_ylabel(axis_labels[1])
220 axis_length: float = 0.1,
221 covariance: np.ndarray =
None) ->
None:
223 Plot a 2D pose on given axis `axes` with given `axis_length`.
225 The ellipse is scaled in such a way that 95% of drawn samples are inliers,
226 see `plot_covariance_ellipse_2d`.
229 axes (matplotlib.axes.Axes): Matplotlib axes.
230 pose: The pose to be plotted.
231 axis_length: The length of the camera axes.
232 covariance (numpy.ndarray): Marginal covariance matrix to plot
233 the uncertainty of the estimation.
236 gRp = pose.rotation().
matrix()
237 t = pose.translation()
241 x_axis = origin + gRp[:, 0] * axis_length
242 line = np.append(origin[np.newaxis], x_axis[np.newaxis], axis=0)
243 axes.plot(line[:, 0], line[:, 1],
'r-')
245 y_axis = origin + gRp[:, 1] * axis_length
246 line = np.append(origin[np.newaxis], y_axis[np.newaxis], axis=0)
247 axes.plot(line[:, 0], line[:, 1],
'g-')
249 if covariance
is not None:
250 pPp = covariance[0:2, 0:2]
251 gPp = np.matmul(np.matmul(gRp, pPp), gRp.T)
258 axis_length: float = 0.1,
259 covariance: np.ndarray =
None,
260 axis_labels=(
"X axis",
"Y axis",
"Z axis"),
263 Plot a 2D pose on given figure with given `axis_length`.
265 The uncertainty ellipse (if covariance is given) is scaled in such a way
266 that 95% of drawn samples are inliers, see `plot_covariance_ellipse_2d`.
269 fignum: Integer representing the figure number to use for plotting.
270 pose: The pose to be plotted.
271 axis_length: The length of the camera axes.
272 covariance: Marginal covariance matrix to plot
273 the uncertainty of the estimation.
274 axis_labels (iterable[string]): List of axis labels to set.
277 fig = plt.figure(fignum)
281 axis_length=axis_length,
282 covariance=covariance)
284 axes.set_xlabel(axis_labels[0])
285 axes.set_ylabel(axis_labels[1])
293 P: Optional[np.ndarray] =
None) ->
None:
295 Plot a 3D point on given axis `axes` with given `linespec`.
297 The uncertainty ellipse (if covariance is given) is scaled in such a way
298 that 95% of drawn samples are inliers, see `plot_covariance_ellipse_3d`.
301 axes (matplotlib.axes.Axes): Matplotlib axes.
302 point: The point to be plotted.
303 linespec: String representing formatting options for Matplotlib.
304 P: Marginal covariance matrix to plot the uncertainty of the estimation.
306 axes.plot([point[0]], [point[1]], [point[2]], linespec)
315 P: np.ndarray =
None,
316 axis_labels: Iterable[str] = (
"X axis",
"Y axis",
"Z axis"),
319 Plot a 3D point on given figure with given `linespec`.
321 The uncertainty ellipse (if covariance is given) is scaled in such a way
322 that 95% of drawn samples are inliers, see `plot_covariance_ellipse_3d`.
325 fignum: Integer representing the figure number to use for plotting.
326 point: The point to be plotted.
327 linespec: String representing formatting options for Matplotlib.
328 P: Marginal covariance matrix to plot the uncertainty of the estimation.
329 axis_labels: List of axis labels to set.
332 fig: The matplotlib figure.
335 fig = plt.figure(fignum)
337 axes = fig.add_subplot(projection=
'3d')
342 axes.set_xlabel(axis_labels[0])
343 axes.set_ylabel(axis_labels[1])
344 axes.set_zlabel(axis_labels[2])
354 axis_labels=(
'X axis',
'Y axis',
'Z axis')):
356 Plots the Point3s in `values`, with optional covariances.
357 Finds all the Point3 objects in the given Values object and plots them.
358 If a Marginals object is given, this function will also plot marginal
359 covariance ellipses for each point.
362 fignum (int): Integer representing the figure number to use for plotting.
363 values (gtsam.Values): Values dictionary consisting of points to be plotted.
364 linespec (string): String representing formatting options for Matplotlib.
365 marginals (numpy.ndarray): Marginal covariance matrix to plot the
366 uncertainty of the estimation.
367 title (string): The title of the plot.
368 axis_labels (iterable[string]): List of axis labels to set.
376 point = values.atPoint3(key)
377 if marginals
is not None:
378 covariance = marginals.marginalCovariance(key)
386 axis_labels=axis_labels)
392 fig = plt.figure(fignum)
394 fig.canvas.manager.set_window_title(title.lower())
399 Plot a 3D pose on given axis `axes` with given `axis_length`.
401 The uncertainty ellipse (if covariance is given) is scaled in such a way
402 that 95% of drawn samples are inliers, see `plot_covariance_ellipse_3d`.
405 axes (matplotlib.axes.Axes): Matplotlib axes.
406 point (gtsam.Point3): The point to be plotted.
407 linespec (string): String representing formatting options for Matplotlib.
408 P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
411 gRp = pose.rotation().
matrix()
412 origin = pose.translation()
415 x_axis = origin + gRp[:, 0] * axis_length
416 line = np.append(origin[np.newaxis], x_axis[np.newaxis], axis=0)
417 axes.plot(line[:, 0], line[:, 1], line[:, 2],
'r-')
419 y_axis = origin + gRp[:, 1] * axis_length
420 line = np.append(origin[np.newaxis], y_axis[np.newaxis], axis=0)
421 axes.plot(line[:, 0], line[:, 1], line[:, 2],
'g-')
423 z_axis = origin + gRp[:, 2] * axis_length
424 line = np.append(origin[np.newaxis], z_axis[np.newaxis], axis=0)
425 axes.plot(line[:, 0], line[:, 1], line[:, 2],
'b-')
432 gPp = gRp @ pPp @ gRp.T
439 axis_length: float = 0.1,
440 P: np.ndarray =
None,
441 axis_labels: Iterable[str] = (
"X axis",
"Y axis",
"Z axis"),
444 Plot a 3D pose on given figure with given `axis_length`.
446 The uncertainty ellipse (if covariance is given) is scaled in such a way
447 that 95% of drawn samples are inliers, see `plot_covariance_ellipse_3d`.
450 fignum: Integer representing the figure number to use for plotting.
451 pose (gtsam.Pose3): 3D pose to be plotted.
452 axis_length: The length of the camera axes.
453 P: Marginal covariance matrix to plot the uncertainty of the estimation.
454 axis_labels: List of axis labels to set.
457 fig: The matplotlib figure.
460 fig = plt.figure(fignum)
462 axes = fig.add_subplot(projection=
'3d')
468 axes.set_xlabel(axis_labels[0])
469 axes.set_ylabel(axis_labels[1])
470 axes.set_zlabel(axis_labels[2])
479 marginals: Marginals =
None,
480 title: str =
"Plot Trajectory",
481 axis_labels: Iterable[str] = (
"X axis",
"Y axis",
"Z axis"),
484 Plot a complete 2D/3D trajectory using poses in `values`.
487 fignum: Integer representing the figure number to use for plotting.
488 values: Values containing some Pose2 and/or Pose3 values.
489 scale: Value to scale the poses by.
490 marginals: Marginalized probability values of the estimation.
491 Used to plot uncertainty bounds.
492 title: The title of the plot.
493 axis_labels (iterable[string]): List of axis labels to set.
495 fig = plt.figure(fignum)
497 axes = fig.add_subplot(projection=
'3d')
501 axes.set_xlabel(axis_labels[0])
502 axes.set_ylabel(axis_labels[1])
503 axes.set_zlabel(axis_labels[2])
507 for key
in poses.keys():
508 pose = poses.atPose2(key)
510 covariance = marginals.marginalCovariance(key)
516 covariance=covariance,
521 for key
in poses.keys():
522 pose = poses.atPose3(key)
524 covariance = marginals.marginalCovariance(key)
531 fig.canvas.manager.set_window_title(title.lower())
538 marginals: Optional[Marginals] =
None,
539 time_interval: float = 0.0) ->
None:
541 Incrementally plot a complete 3D trajectory using poses in `values`.
544 fignum: Integer representing the figure number to use for plotting.
545 values: Values dict containing the poses.
546 start: Starting index to start plotting from.
547 scale: Value to scale the poses by.
548 marginals: Marginalized probability values of the estimation.
549 Used to plot uncertainty bounds.
550 time_interval: Time in seconds to pause between each rendering.
551 Used to create animation effect.
553 fig = plt.figure(fignum)
555 axes = fig.add_subplot(projection=
'3d')
562 for key
in keys[start:]:
563 if values.exists(key):
564 pose_i = values.atPose3(key)
574 plt.pause(time_interval)