plot.py
Go to the documentation of this file.
1 """Various plotting utlities."""
2 
3 # pylint: disable=no-member, invalid-name
4 
5 from typing import Iterable, Optional, Tuple
6 
7 import matplotlib.pyplot as plt
8 import numpy as np
9 from matplotlib import patches
10 from mpl_toolkits.mplot3d import Axes3D # pylint: disable=unused-import
11 
12 import gtsam
13 from gtsam import Marginals, Point2, Point3, Pose2, Pose3, Values
14 
15 # For translation between a scaling of the uncertainty ellipse and the
16 # percentage of inliers see discussion in
17 # [PR 1067](https://github.com/borglab/gtsam/pull/1067)
18 # and the notebook python/gtsam/notebooks/ellipses.ipynb (needs scipy).
19 #
20 # In the following, the default scaling is chosen for 95% inliers, which
21 # translates to the following sigma values:
22 # 1D: 1.959963984540
23 # 2D: 2.447746830681
24 # 3D: 2.795483482915
25 #
26 # Further references are Stochastic Models, Estimation, and Control Vol 1 by Maybeck,
27 # page 366 and https://www.xarg.org/2018/04/how-to-plot-a-covariance-error-ellipse/
28 #
29 # For reference, here are the inlier percentages for some sigma values:
30 # 1 2 3 4 5
31 # 1D 68.26895 95.44997 99.73002 99.99367 99.99994
32 # 2D 39.34693 86.46647 98.88910 99.96645 99.99963
33 # 3D 19.87480 73.85359 97.07091 99.88660 99.99846
34 
35 
36 def set_axes_equal(fignum: int) -> None:
37  """
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.
41 
42  Args:
43  fignum: An integer representing the figure number for Matplotlib.
44  """
45  fig = plt.figure(fignum)
46  if not fig.axes:
47  ax = fig.add_subplot(projection='3d')
48  else:
49  ax = fig.axes[0]
50 
51  limits = np.array([
52  ax.get_xlim3d(),
53  ax.get_ylim3d(),
54  ax.get_zlim3d(),
55  ])
56 
57  origin = np.mean(limits, axis=1)
58  radius = 0.5 * np.max(np.abs(limits[:, 1] - limits[:, 0]))
59 
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])
63 
64 
65 def ellipsoid(rx: float, ry: float, rz: float,
66  n: int) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
67  """
68  Numpy equivalent of Matlab's ellipsoid function.
69 
70  Args:
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.
75 
76  Returns:
77  The points in the x, y and z axes to use for the surface plot.
78  """
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
84 
85  return x, y, z
86 
87 
89  origin: Point3,
90  P: np.ndarray,
91  scale: float = 1,
92  n: int = 8,
93  alpha: float = 0.5) -> None:
94  """
95  Plots a Gaussian as an uncertainty ellipse
96 
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.
99 
100  Args:
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].
108  """
109  # this corresponds to 95%, see note above
110  k = 2.795483482915
111  U, S, _ = np.linalg.svd(P)
112 
113  radii = k * np.sqrt(S)
114  radii = radii * scale
115  rx, ry, rz = radii
116 
117  # generate data for "unrotated" ellipsoid
118  xc, yc, zc = ellipsoid(rx, ry, rz, n)
119 
120  # rotate data with orientation matrix U and center c
121  data = np.kron(U[:, 0:1], xc) + np.kron(U[:, 1:2], yc) + \
122  np.kron(U[:, 2:3], zc)
123  n = data.shape[1]
124  x = data[0:n, :] + origin[0]
125  y = data[n:2 * n, :] + origin[1]
126  z = data[2 * n:, :] + origin[2]
127 
128  axes.plot_surface(x, y, z, alpha=alpha, cmap='hot')
129 
130 
131 def plot_covariance_ellipse_2d(axes, origin: Point2,
132  covariance: np.ndarray) -> None:
133  """
134  Plots a Gaussian as an uncertainty ellipse
135 
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.
138 
139  Args:
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.
144  """
145 
146  w, v = np.linalg.eigh(covariance)
147 
148  # this corresponds to 95%, see note above
149  k = 2.447746830681
150 
151  angle = np.arctan2(v[1, 0], v[0, 0])
152  # We multiply k by 2 since k corresponds to the radius but Ellipse uses
153  # the diameter.
154  e1 = patches.Ellipse(origin,
155  np.sqrt(w[0]) * 2 * k,
156  np.sqrt(w[1]) * 2 * k,
157  angle=np.rad2deg(angle),
158  fill=False)
159  axes.add_patch(e1)
160 
161 
163  point: Point2,
164  linespec: str,
165  P: Optional[np.ndarray] = None) -> None:
166  """
167  Plot a 2D point and its corresponding uncertainty ellipse on given axis
168  `axes` with given `linespec`.
169 
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`.
172 
173  Args:
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.
178  """
179  axes.plot([point[0]], [point[1]], linespec, marker='.', markersize=10)
180  if P is not None:
181  plot_covariance_ellipse_2d(axes, point, P)
182 
183 
185  fignum: int,
186  point: Point2,
187  linespec: str,
188  P: np.ndarray = None,
189  axis_labels: Iterable[str] = ("X axis", "Y axis"),
190 ) -> plt.Figure:
191  """
192  Plot a 2D point on given figure with given `linespec`.
193 
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`.
196 
197  Args:
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.
203 
204  Returns:
205  fig: The matplotlib figure.
206 
207  """
208  fig = plt.figure(fignum)
209  axes = fig.gca()
210  plot_point2_on_axes(axes, point, linespec, P)
211 
212  axes.set_xlabel(axis_labels[0])
213  axes.set_ylabel(axis_labels[1])
214 
215  return fig
216 
217 
219  pose: Pose2,
220  axis_length: float = 0.1,
221  covariance: np.ndarray = None) -> None:
222  """
223  Plot a 2D pose on given axis `axes` with given `axis_length`.
224 
225  The ellipse is scaled in such a way that 95% of drawn samples are inliers,
226  see `plot_covariance_ellipse_2d`.
227 
228  Args:
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.
234  """
235  # get rotation and translation (center)
236  gRp = pose.rotation().matrix() # rotation from pose to global
237  t = pose.translation()
238  origin = t
239 
240  # draw the camera axes
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-')
244 
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-')
248 
249  if covariance is not None:
250  pPp = covariance[0:2, 0:2]
251  gPp = np.matmul(np.matmul(gRp, pPp), gRp.T)
252  plot_covariance_ellipse_2d(axes, origin, gPp)
253 
254 
256  fignum: int,
257  pose: Pose2,
258  axis_length: float = 0.1,
259  covariance: np.ndarray = None,
260  axis_labels=("X axis", "Y axis", "Z axis"),
261 ) -> plt.Figure:
262  """
263  Plot a 2D pose on given figure with given `axis_length`.
264 
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`.
267 
268  Args:
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.
275  """
276  # get figure object
277  fig = plt.figure(fignum)
278  axes = fig.gca()
279  plot_pose2_on_axes(axes,
280  pose,
281  axis_length=axis_length,
282  covariance=covariance)
283 
284  axes.set_xlabel(axis_labels[0])
285  axes.set_ylabel(axis_labels[1])
286 
287  return fig
288 
289 
291  point: Point3,
292  linespec: str,
293  P: Optional[np.ndarray] = None) -> None:
294  """
295  Plot a 3D point on given axis `axes` with given `linespec`.
296 
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`.
299 
300  Args:
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.
305  """
306  axes.plot([point[0]], [point[1]], [point[2]], linespec)
307  if P is not None:
308  plot_covariance_ellipse_3d(axes, point, P)
309 
310 
312  fignum: int,
313  point: Point3,
314  linespec: str,
315  P: np.ndarray = None,
316  axis_labels: Iterable[str] = ("X axis", "Y axis", "Z axis"),
317 ) -> plt.Figure:
318  """
319  Plot a 3D point on given figure with given `linespec`.
320 
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`.
323 
324  Args:
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.
330 
331  Returns:
332  fig: The matplotlib figure.
333 
334  """
335  fig = plt.figure(fignum)
336  if not fig.axes:
337  axes = fig.add_subplot(projection='3d')
338  else:
339  axes = fig.axes[0]
340  plot_point3_on_axes(axes, point, linespec, P)
341 
342  axes.set_xlabel(axis_labels[0])
343  axes.set_ylabel(axis_labels[1])
344  axes.set_zlabel(axis_labels[2])
345 
346  return fig
347 
348 
349 def plot_3d_points(fignum,
350  values,
351  linespec="g*",
352  marginals=None,
353  title="3D Points",
354  axis_labels=('X axis', 'Y axis', 'Z axis')):
355  """
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.
360 
361  Args:
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.
369  """
370 
371  keys = values.keys()
372 
373  # Plot points and covariance matrices
374  for key in keys:
375  try:
376  point = values.atPoint3(key)
377  if marginals is not None:
378  covariance = marginals.marginalCovariance(key)
379  else:
380  covariance = None
381 
382  fig = plot_point3(fignum,
383  point,
384  linespec,
385  covariance,
386  axis_labels=axis_labels)
387 
388  except RuntimeError:
389  continue
390  # I guess it's not a Point3
391 
392  fig = plt.figure(fignum)
393  fig.suptitle(title)
394  fig.canvas.manager.set_window_title(title.lower())
395 
396 
397 def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1):
398  """
399  Plot a 3D pose on given axis `axes` with given `axis_length`.
400 
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`.
403 
404  Args:
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.
409  """
410  # get rotation and translation (center)
411  gRp = pose.rotation().matrix() # rotation from pose to global
412  origin = pose.translation()
413 
414  # draw the camera axes
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-')
418 
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-')
422 
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-')
426 
427  # plot the covariance
428  if P is not None:
429  # covariance matrix in pose coordinate frame
430  pPp = P[3:6, 3:6]
431  # convert the covariance matrix to global coordinate frame
432  gPp = gRp @ pPp @ gRp.T
433  plot_covariance_ellipse_3d(axes, origin, gPp)
434 
435 
437  fignum: int,
438  pose: Pose3,
439  axis_length: float = 0.1,
440  P: np.ndarray = None,
441  axis_labels: Iterable[str] = ("X axis", "Y axis", "Z axis"),
442 ) -> plt.Figure:
443  """
444  Plot a 3D pose on given figure with given `axis_length`.
445 
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`.
448 
449  Args:
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.
455 
456  Returns:
457  fig: The matplotlib figure.
458  """
459  # get figure object
460  fig = plt.figure(fignum)
461  if not fig.axes:
462  axes = fig.add_subplot(projection='3d')
463  else:
464  axes = fig.axes[0]
465 
466  plot_pose3_on_axes(axes, pose, P=P, axis_length=axis_length)
467 
468  axes.set_xlabel(axis_labels[0])
469  axes.set_ylabel(axis_labels[1])
470  axes.set_zlabel(axis_labels[2])
471 
472  return fig
473 
474 
476  fignum: int,
477  values: Values,
478  scale: float = 1,
479  marginals: Marginals = None,
480  title: str = "Plot Trajectory",
481  axis_labels: Iterable[str] = ("X axis", "Y axis", "Z axis"),
482 ) -> None:
483  """
484  Plot a complete 2D/3D trajectory using poses in `values`.
485 
486  Args:
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.
494  """
495  fig = plt.figure(fignum)
496  if not fig.axes:
497  axes = fig.add_subplot(projection='3d')
498  else:
499  axes = fig.axes[0]
500 
501  axes.set_xlabel(axis_labels[0])
502  axes.set_ylabel(axis_labels[1])
503  axes.set_zlabel(axis_labels[2])
504 
505  # Plot 2D poses, if any
506  poses = gtsam.utilities.allPose2s(values)
507  for key in poses.keys():
508  pose = poses.atPose2(key)
509  if marginals:
510  covariance = marginals.marginalCovariance(key)
511  else:
512  covariance = None
513 
514  plot_pose2_on_axes(axes,
515  pose,
516  covariance=covariance,
517  axis_length=scale)
518 
519  # Then 3D poses, if any
520  poses = gtsam.utilities.allPose3s(values)
521  for key in poses.keys():
522  pose = poses.atPose3(key)
523  if marginals:
524  covariance = marginals.marginalCovariance(key)
525  else:
526  covariance = None
527 
528  plot_pose3_on_axes(axes, pose, P=covariance, axis_length=scale)
529 
530  fig.suptitle(title)
531  fig.canvas.manager.set_window_title(title.lower())
532 
533 
535  values: Values,
536  start: int = 0,
537  scale: float = 1,
538  marginals: Optional[Marginals] = None,
539  time_interval: float = 0.0) -> None:
540  """
541  Incrementally plot a complete 3D trajectory using poses in `values`.
542 
543  Args:
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.
552  """
553  fig = plt.figure(fignum)
554  if not fig.axes:
555  axes = fig.add_subplot(projection='3d')
556  else:
557  axes = fig.axes[0]
558 
559  poses = gtsam.utilities.allPose3s(values)
560  keys = poses.keys()
561 
562  for key in keys[start:]:
563  if values.exists(key):
564  pose_i = values.atPose3(key)
565  plot_pose3(fignum, pose_i, scale)
566 
567  # Update the plot space to encompass all plotted points
568  axes.autoscale()
569 
570  # Set the 3 axes equal
571  set_axes_equal(fignum)
572 
573  # Pause for a fixed amount of seconds
574  plt.pause(time_interval)
gtsam::utils.plot.plot_point2
plt.Figure plot_point2(int fignum, Point2 point, str linespec, np.ndarray P=None, Iterable[str] axis_labels=("X axis", "Y axis"))
Definition: plot.py:184
gtsam::utils.plot.plot_pose3
plt.Figure plot_pose3(int fignum, Pose3 pose, float axis_length=0.1, np.ndarray P=None, Iterable[str] axis_labels=("X axis", "Y axis", "Z axis"))
Definition: plot.py:436
gtsam::utilities::allPose3s
Values allPose3s(const Values &values)
Extract all Pose3 values.
Definition: nonlinear/utilities.h:150
gtsam::utilities::allPose2s
Values allPose2s(const Values &values)
Extract all Pose3 values.
Definition: nonlinear/utilities.h:132
gtsam::utils.plot.plot_covariance_ellipse_3d
None plot_covariance_ellipse_3d(axes, Point3 origin, np.ndarray P, float scale=1, int n=8, float alpha=0.5)
Definition: plot.py:88
gtsam::utils.plot.plot_incremental_trajectory
None plot_incremental_trajectory(int fignum, Values values, int start=0, float scale=1, Optional[Marginals] marginals=None, float time_interval=0.0)
Definition: plot.py:534
gtsam::utils.plot.plot_trajectory
None plot_trajectory(int fignum, Values values, float scale=1, Marginals marginals=None, str title="Plot Trajectory", Iterable[str] axis_labels=("X axis", "Y axis", "Z axis"))
Definition: plot.py:475
gtsam::utils.plot.plot_point2_on_axes
None plot_point2_on_axes(axes, Point2 point, str linespec, Optional[np.ndarray] P=None)
Definition: plot.py:162
gtsam::utils.plot.ellipsoid
Tuple[np.ndarray, np.ndarray, np.ndarray] ellipsoid(float rx, float ry, float rz, int n)
Definition: plot.py:65
gtsam::utils.plot.set_axes_equal
None set_axes_equal(int fignum)
Definition: plot.py:36
gtsam::utils.plot.plot_pose2
plt.Figure plot_pose2(int fignum, Pose2 pose, float axis_length=0.1, np.ndarray covariance=None, axis_labels=("X axis", "Y axis", "Z axis"))
Definition: plot.py:255
gtsam::utils.plot.plot_point3_on_axes
None plot_point3_on_axes(axes, Point3 point, str linespec, Optional[np.ndarray] P=None)
Definition: plot.py:290
gtsam::utils.plot.plot_point3
plt.Figure plot_point3(int fignum, Point3 point, str linespec, np.ndarray P=None, Iterable[str] axis_labels=("X axis", "Y axis", "Z axis"))
Definition: plot.py:311
gtsam::utils.plot.plot_pose3_on_axes
def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1)
Definition: plot.py:397
matrix
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
Definition: gtsam/3rdparty/Eigen/blas/common.h:110
gtsam::utils.plot.plot_3d_points
def plot_3d_points(fignum, values, linespec="g*", marginals=None, title="3D Points", axis_labels=('X axis', 'Y axis', 'Z axis'))
Definition: plot.py:349
gtsam::utils.plot.plot_pose2_on_axes
None plot_pose2_on_axes(axes, Pose2 pose, float axis_length=0.1, np.ndarray covariance=None)
Definition: plot.py:218
gtsam::utils.plot.plot_covariance_ellipse_2d
None plot_covariance_ellipse_2d(axes, Point2 origin, np.ndarray covariance)
Definition: plot.py:131


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:34:23