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
37 Make axes of 3D plot have equal scale so that spheres appear as spheres, 38 cubes as cubes, etc.. This is one possible solution to Matplotlib's 39 ax.set_aspect('equal') and ax.axis('equal') not working for 3D. 42 fignum: An integer representing the figure number for Matplotlib. 44 fig = plt.figure(fignum)
46 ax = fig.add_subplot(projection=
'3d')
56 origin = np.mean(limits, axis=1)
57 radius = 0.5 * np.max(np.abs(limits[:, 1] - limits[:, 0]))
59 ax.set_xlim3d([origin[0] - radius, origin[0] + radius])
60 ax.set_ylim3d([origin[1] - radius, origin[1] + radius])
61 ax.set_zlim3d([origin[2] - radius, origin[2] + radius])
65 n: int) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
67 Numpy equivalent of Matlab's ellipsoid function. 70 rx: Radius of ellipsoid in X-axis. 71 ry: Radius of ellipsoid in Y-axis. 72 rz: Radius of ellipsoid in Z-axis. 73 n: The granularity of the ellipsoid plotted. 76 The points in the x, y and z axes to use for the surface plot. 78 u = np.linspace(0, 2 * np.pi, n + 1)
79 v = np.linspace(0, np.pi, n + 1)
80 x = -rx * np.outer(np.cos(u), np.sin(v)).T
81 y = -ry * np.outer(np.sin(u), np.sin(v)).T
82 z = -rz * np.outer(np.ones_like(u), np.cos(v)).T
92 alpha: float = 0.5) ->
None:
94 Plots a Gaussian as an uncertainty ellipse 96 The ellipse is scaled in such a way that 95% of drawn samples are inliers. 97 Derivation of the scaling factor is explained at the beginning of this file. 100 axes (matplotlib.axes.Axes): Matplotlib axes. 101 origin: The origin in the world frame. 102 P: The marginal covariance matrix of the 3D point 103 which will be represented as an ellipse. 104 scale: Scaling factor of the radii of the covariance ellipse. 105 n: Defines the granularity of the ellipse. Higher values indicate finer ellipses. 106 alpha: Transparency value for the plotted surface in the range [0, 1]. 110 U, S, _ = np.linalg.svd(P)
112 radii = k * np.sqrt(S)
113 radii = radii * scale
120 data = np.kron(U[:, 0:1], xc) + np.kron(U[:, 1:2], yc) + \
121 np.kron(U[:, 2:3], zc)
123 x = data[0:n, :] + origin[0]
124 y = data[n:2 * n, :] + origin[1]
125 z = data[2 * n:, :] + origin[2]
127 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,
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)
187 P: np.ndarray =
None,
188 axis_labels: Iterable[str] = (
"X axis",
"Y axis"),
191 Plot a 2D point on given figure with given `linespec`. 193 The uncertainty ellipse (if covariance is given) is scaled in such a way 194 that 95% of drawn samples are inliers, see `plot_covariance_ellipse_2d`. 197 fignum: Integer representing the figure number to use for plotting. 198 point: The point to be plotted. 199 linespec: String representing formatting options for Matplotlib. 200 P: Marginal covariance matrix to plot the uncertainty of the estimation. 201 axis_labels: List of axis labels to set. 204 fig: The matplotlib figure. 207 fig = plt.figure(fignum)
211 axes.set_xlabel(axis_labels[0])
212 axes.set_ylabel(axis_labels[1])
219 axis_length: float = 0.1,
220 covariance: np.ndarray =
None) ->
None:
222 Plot a 2D pose on given axis `axes` with given `axis_length`. 224 The ellipse is scaled in such a way that 95% of drawn samples are inliers, 225 see `plot_covariance_ellipse_2d`. 228 axes (matplotlib.axes.Axes): Matplotlib axes. 229 pose: The pose to be plotted. 230 axis_length: The length of the camera axes. 231 covariance (numpy.ndarray): Marginal covariance matrix to plot 232 the uncertainty of the estimation. 235 gRp = pose.rotation().
matrix()
236 t = pose.translation()
240 x_axis = origin + gRp[:, 0] * axis_length
241 line = np.append(origin[np.newaxis], x_axis[np.newaxis], axis=0)
242 axes.plot(line[:, 0], line[:, 1],
'r-')
244 y_axis = origin + gRp[:, 1] * axis_length
245 line = np.append(origin[np.newaxis], y_axis[np.newaxis], axis=0)
246 axes.plot(line[:, 0], line[:, 1],
'g-')
248 if covariance
is not None:
249 pPp = covariance[0:2, 0:2]
250 gPp = np.matmul(np.matmul(gRp, pPp), gRp.T)
257 axis_length: float = 0.1,
258 covariance: np.ndarray =
None,
259 axis_labels=(
"X axis",
"Y axis",
"Z axis"),
262 Plot a 2D pose on given figure with given `axis_length`. 264 The uncertainty ellipse (if covariance is given) is scaled in such a way 265 that 95% of drawn samples are inliers, see `plot_covariance_ellipse_2d`. 268 fignum: Integer representing the figure number to use for plotting. 269 pose: The pose to be plotted. 270 axis_length: The length of the camera axes. 271 covariance: Marginal covariance matrix to plot 272 the uncertainty of the estimation. 273 axis_labels (iterable[string]): List of axis labels to set. 276 fig = plt.figure(fignum)
280 axis_length=axis_length,
281 covariance=covariance)
283 axes.set_xlabel(axis_labels[0])
284 axes.set_ylabel(axis_labels[1])
292 P: Optional[np.ndarray] =
None) ->
None:
294 Plot a 3D point on given axis `axes` with given `linespec`. 296 The uncertainty ellipse (if covariance is given) is scaled in such a way 297 that 95% of drawn samples are inliers, see `plot_covariance_ellipse_3d`. 300 axes (matplotlib.axes.Axes): Matplotlib axes. 301 point: The point to be plotted. 302 linespec: String representing formatting options for Matplotlib. 303 P: Marginal covariance matrix to plot the uncertainty of the estimation. 305 axes.plot([point[0]], [point[1]], [point[2]], linespec)
314 P: np.ndarray =
None,
315 axis_labels: Iterable[str] = (
"X axis",
"Y axis",
"Z axis"),
318 Plot a 3D point on given figure with given `linespec`. 320 The uncertainty ellipse (if covariance is given) is scaled in such a way 321 that 95% of drawn samples are inliers, see `plot_covariance_ellipse_3d`. 324 fignum: Integer representing the figure number to use for plotting. 325 point: The point to be plotted. 326 linespec: String representing formatting options for Matplotlib. 327 P: Marginal covariance matrix to plot the uncertainty of the estimation. 328 axis_labels: List of axis labels to set. 331 fig: The matplotlib figure. 334 fig = plt.figure(fignum)
336 axes = fig.add_subplot(projection=
'3d')
341 axes.set_xlabel(axis_labels[0])
342 axes.set_ylabel(axis_labels[1])
343 axes.set_zlabel(axis_labels[2])
353 axis_labels=(
'X axis',
'Y axis',
'Z axis')):
355 Plots the Point3s in `values`, with optional covariances. 356 Finds all the Point3 objects in the given Values object and plots them. 357 If a Marginals object is given, this function will also plot marginal 358 covariance ellipses for each point. 361 fignum (int): Integer representing the figure number to use for plotting. 362 values (gtsam.Values): Values dictionary consisting of points to be plotted. 363 linespec (string): String representing formatting options for Matplotlib. 364 marginals (numpy.ndarray): Marginal covariance matrix to plot the 365 uncertainty of the estimation. 366 title (string): The title of the plot. 367 axis_labels (iterable[string]): List of axis labels to set. 375 point = values.atPoint3(key)
376 if marginals
is not None:
377 covariance = marginals.marginalCovariance(key)
385 axis_labels=axis_labels)
391 fig = plt.figure(fignum)
393 fig.canvas.manager.set_window_title(title.lower())
398 Plot a 3D pose on given axis `axes` with given `axis_length`. 400 The uncertainty ellipse (if covariance is given) is scaled in such a way 401 that 95% of drawn samples are inliers, see `plot_covariance_ellipse_3d`. 404 axes (matplotlib.axes.Axes): Matplotlib axes. 405 point (gtsam.Point3): The point to be plotted. 406 linespec (string): String representing formatting options for Matplotlib. 407 P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. 410 gRp = pose.rotation().
matrix()
411 origin = pose.translation()
414 x_axis = origin + gRp[:, 0] * axis_length
415 line = np.append(origin[np.newaxis], x_axis[np.newaxis], axis=0)
416 axes.plot(line[:, 0], line[:, 1], line[:, 2],
'r-')
418 y_axis = origin + gRp[:, 1] * axis_length
419 line = np.append(origin[np.newaxis], y_axis[np.newaxis], axis=0)
420 axes.plot(line[:, 0], line[:, 1], line[:, 2],
'g-')
422 z_axis = origin + gRp[:, 2] * axis_length
423 line = np.append(origin[np.newaxis], z_axis[np.newaxis], axis=0)
424 axes.plot(line[:, 0], line[:, 1], line[:, 2],
'b-')
431 gPp = gRp @ pPp @ gRp.T
438 axis_length: float = 0.1,
439 P: np.ndarray =
None,
440 axis_labels: Iterable[str] = (
"X axis",
"Y axis",
"Z axis"),
443 Plot a 3D pose on given figure with given `axis_length`. 445 The uncertainty ellipse (if covariance is given) is scaled in such a way 446 that 95% of drawn samples are inliers, see `plot_covariance_ellipse_3d`. 449 fignum: Integer representing the figure number to use for plotting. 450 pose (gtsam.Pose3): 3D pose to be plotted. 451 axis_length: The length of the camera axes. 452 P: Marginal covariance matrix to plot the uncertainty of the estimation. 453 axis_labels: List of axis labels to set. 456 fig: The matplotlib figure. 459 fig = plt.figure(fignum)
461 axes = fig.add_subplot(projection=
'3d')
467 axes.set_xlabel(axis_labels[0])
468 axes.set_ylabel(axis_labels[1])
469 axes.set_zlabel(axis_labels[2])
478 marginals: Marginals =
None,
479 title: str =
"Plot Trajectory",
480 axis_labels: Iterable[str] = (
"X axis",
"Y axis",
"Z axis"),
483 Plot a complete 2D/3D trajectory using poses in `values`. 486 fignum: Integer representing the figure number to use for plotting. 487 values: Values containing some Pose2 and/or Pose3 values. 488 scale: Value to scale the poses by. 489 marginals: Marginalized probability values of the estimation. 490 Used to plot uncertainty bounds. 491 title: The title of the plot. 492 axis_labels (iterable[string]): List of axis labels to set. 494 fig = plt.figure(fignum)
496 axes = fig.add_subplot(projection=
'3d')
500 axes.set_xlabel(axis_labels[0])
501 axes.set_ylabel(axis_labels[1])
502 axes.set_zlabel(axis_labels[2])
506 for key
in poses.keys():
507 pose = poses.atPose2(key)
509 covariance = marginals.marginalCovariance(key)
515 covariance=covariance,
520 for key
in poses.keys():
521 pose = poses.atPose3(key)
523 covariance = marginals.marginalCovariance(key)
530 fig.canvas.manager.set_window_title(title.lower())
537 marginals: Optional[Marginals] =
None,
538 time_interval: float = 0.0) ->
None:
540 Incrementally plot a complete 3D trajectory using poses in `values`. 543 fignum: Integer representing the figure number to use for plotting. 544 values: Values dict containing the poses. 545 start: Starting index to start plotting from. 546 scale: Value to scale the poses by. 547 marginals: Marginalized probability values of the estimation. 548 Used to plot uncertainty bounds. 549 time_interval: Time in seconds to pause between each rendering. 550 Used to create animation effect. 552 fig = plt.figure(fignum)
554 axes = fig.add_subplot(projection=
'3d')
561 for key
in keys[start:]:
562 if values.exists(key):
563 pose_i = values.atPose3(key)
573 plt.pause(time_interval)
def plot_3d_points(fignum, values, linespec="g*", marginals=None, title="3D Points", axis_labels=('X axis', 'Y axis', 'Z axis'))
def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1)
def plot_incremental_trajectory
Values allPose3s(const Values &values)
Extract all Pose3 values.
Values allPose2s(const Values &values)
Extract all Pose3 values.
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
def plot_covariance_ellipse_3d
def plot_covariance_ellipse_2d