From 1c82ac345fe5de8ca91dab389e5493bfc8e03426 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 8 Jun 2020 09:24:14 -0500 Subject: [PATCH 1/2] Plotting updates - Fixed plotting functions - Renamed `P` to more descriptive `covariance` - Added docstrings --- cython/gtsam/utils/plot.py | 156 +++++++++++++++++++++++++++++-------- 1 file changed, 124 insertions(+), 32 deletions(-) diff --git a/cython/gtsam/utils/plot.py b/cython/gtsam/utils/plot.py index 902ded3e0..51333a59c 100644 --- a/cython/gtsam/utils/plot.py +++ b/cython/gtsam/utils/plot.py @@ -13,8 +13,9 @@ def set_axes_equal(fignum): Make axes of 3D plot have equal scale so that spheres appear as spheres, cubes as cubes, etc.. This is one possible solution to Matplotlib's ax.set_aspect('equal') and ax.axis('equal') not working for 3D. - Input - ax: a matplotlib axis, e.g., as output from plt.gca(). + + Args: + fignum (int): An integer representing the figure number for Matplotlib. """ fig = plt.figure(fignum) ax = fig.gca(projection='3d') @@ -34,7 +35,21 @@ def set_axes_equal(fignum): def ellipsoid(xc, yc, zc, rx, ry, rz, n): - """Numpy equivalent of Matlab's ellipsoid function""" + """ + Numpy equivalent of Matlab's ellipsoid function. + + Args: + xc (double): Center of ellipsoid in X-axis. + yc (double): Center of ellipsoid in Y-axis. + zc (double): Center of ellipsoid in Z-axis. + rx (double): Radius of ellipsoid in X-axis. + ry (double): Radius of ellipsoid in Y-axis. + rz (double): Radius of ellipsoid in Z-axis. + n (int): The granularity of the ellipsoid plotted. + + Returns: + tuple[numpy.ndarray]: 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) x = -rx * np.outer(np.cos(u), np.sin(v)).T @@ -44,16 +59,24 @@ def ellipsoid(xc, yc, zc, rx, ry, rz, n): return x, y, z -def plot_covariance_ellipse_3d(axes, origin, P, scale=1, n=8, alpha=0.5): +def plot_covariance_ellipse_3d(axes, origin, covariance, scale=1, n=8, alpha=0.5): """ Plots a Gaussian as an uncertainty ellipse Based on Maybeck Vol 1, page 366 k=2.296 corresponds to 1 std, 68.26% of all probability k=11.82 corresponds to 3 std, 99.74% of all probability + + Args: + axes (matplotlib.axes.Axes): Matplotlib axes. + origin (gtsam.Point3): The origin in the world frame. + covariance (numpy.ndarray): The marginal covariance matrix of the 3D point which will be represented as an ellipse. + scale (float): Scaling factor of the radii of the covariance ellipse. + n (int): Defines the granularity of the ellipse. Higher values indicate finer ellipses. + alpha (float): Transparency value for the plotted surface in the range [0, 1]. """ k = 11.82 - U, S, _ = np.linalg.svd(P) + U, S, _ = np.linalg.svd(covariance) radii = k * np.sqrt(S) radii = radii * scale @@ -74,7 +97,15 @@ def plot_covariance_ellipse_3d(axes, origin, P, scale=1, n=8, alpha=0.5): def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=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`. + + Args: + axes (matplotlib.axes.Axes): Matplotlib axes. + pose (gtsam.Pose2): The pose to be plotted. + axis_length (float): The length of the camera axes. + covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. + """ # get rotation and translation (center) gRp = pose.rotation().matrix() # rotation from pose to global t = pose.translation() @@ -105,33 +136,64 @@ def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None): def plot_pose2(fignum, pose, axis_length=0.1, covariance=None): - """Plot a 2D pose on given figure with given 'axis_length'.""" + """ + Plot a 2D pose on given figure with given `axis_length`. + + Args: + fignum (int): Integer representing the figure number to use for plotting. + pose (gtsam.Pose2): The pose to be plotted. + axis_length (float): The length of the camera axes. + covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. + """ # get figure object fig = plt.figure(fignum) axes = fig.gca() - plot_pose2_on_axes(axes, pose, axis_length, covariance) + plot_pose2_on_axes(axes, pose, axis_length=axis_length, + covariance=covariance) -def plot_point3_on_axes(axes, point, linespec, P=None): - """Plot a 3D point on given axis 'axes' with given 'linespec'.""" +def plot_point3_on_axes(axes, point, linespec, covariance=None): + """ + Plot a 3D point on given axis `axes` with given `linespec`. + + Args: + axes (matplotlib.axes.Axes): Matplotlib axes. + point (gtsam.Point3): The point to be plotted. + linespec (string): String representing formatting options for Matplotlib. + covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. + """ axes.plot([point.x()], [point.y()], [point.z()], linespec) - if P is not None: - plot_covariance_ellipse_3d(axes, point.vector(), P) + if covariance is not None: + plot_covariance_ellipse_3d(axes, point.vector(), covariance) -def plot_point3(fignum, point, linespec, P=None): - """Plot a 3D point on given figure with given 'linespec'.""" +def plot_point3(fignum, point, linespec, covariance=None): + """ + Plot a 3D point on given figure with given `linespec`. + + Args: + fignum (int): Integer representing the figure number to use for plotting. + point (gtsam.Point3): The point to be plotted. + linespec (string): String representing formatting options for Matplotlib. + covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. + """ fig = plt.figure(fignum) axes = fig.gca(projection='3d') - plot_point3_on_axes(axes, point, linespec, P) + plot_point3_on_axes(axes, point, linespec, covariance) def plot_3d_points(fignum, values, linespec="g*", marginals=None): """ - 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. If a Marginals object is given, this function will also plot marginal covariance ellipses for each point. + + Args: + fignum (int): Integer representing the figure number to use for plotting. + values (gtsam.Values): Values dictionary consisting of points to be plotted. + linespec (string): String representing formatting options for Matplotlib. + covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. """ keys = values.keys() @@ -142,19 +204,27 @@ def plot_3d_points(fignum, values, linespec="g*", marginals=None): key = keys.at(i) point = values.atPoint3(key) if marginals is not None: - P = marginals.marginalCovariance(key); + covariance = marginals.marginalCovariance(key) else: - P = None + covariance = None - plot_point3(fignum, point, linespec, P) + plot_point3(fignum, point, linespec, covariance) except RuntimeError: continue # I guess it's not a Point3 -def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1): - """Plot a 3D pose on given axis 'axes' with given 'axis_length'.""" +def plot_pose3_on_axes(axes, pose, axis_length=0.1, covariance=None, scale=1): + """ + Plot a 3D pose on given axis `axes` with given `axis_length`. + + Args: + axes (matplotlib.axes.Axes): Matplotlib axes. + point (gtsam.Point3): The point to be plotted. + linespec (string): String representing formatting options for Matplotlib. + covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. + """ # get rotation and translation (center) gRp = pose.rotation().matrix() # rotation from pose to global origin = pose.translation().vector() @@ -173,22 +243,42 @@ def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1): axes.plot(line[:, 0], line[:, 1], line[:, 2], 'b-') # plot the covariance - if P is not None: + if covariance is not None: # covariance matrix in pose coordinate frame - pPp = P[3:6, 3:6] + pPp = covariance[3:6, 3:6] # convert the covariance matrix to global coordinate frame gPp = gRp @ pPp @ gRp.T plot_covariance_ellipse_3d(axes, origin, gPp) -def plot_pose3(fignum, pose, axis_length=0.1, P=None): - """Plot a 3D pose on given figure with given 'axis_length'.""" +def plot_pose3(fignum, pose, axis_length=0.1, covariance=None): + """ + Plot a 3D pose on given figure with given `axis_length`. + + Args: + fignum (int): Integer representing the figure number to use for plotting. + pose (gtsam.Pose3): 3D pose to be plotted. + linespec (string): String representing formatting options for Matplotlib. + covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. + """ # 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, covariance=covariance, + axis_length=axis_length) + def plot_trajectory(fignum, values, scale=1, marginals=None): + """ + Plot a complete 3D trajectory using poses in `values`. + + Args: + fignum (int): Integer representing the figure number to use for plotting. + values (gtsam.Values): Values dict containing the poses. + scale (float): Value to scale the poses by. + marginals (gtsam.Marginals): Marginalized probability values of the estimation. + Used to plot uncertainty bounds. + """ pose3Values = gtsam.allPose3s(values) keys = gtsam.KeyVector(pose3Values.keys()) lastIndex = None @@ -209,11 +299,12 @@ def plot_trajectory(fignum, values, scale=1, marginals=None): pass if marginals: - P = marginals.marginalCovariance(lastKey) + covariance = marginals.marginalCovariance(lastKey) else: - P = None + covariance = None - plot_pose3(fignum, lastPose, P, scale) + plot_pose3(fignum, lastPose, covariance=covariance, + axis_length=scale) lastIndex = i @@ -223,11 +314,12 @@ def plot_trajectory(fignum, values, scale=1, marginals=None): try: lastPose = pose3Values.atPose3(lastKey) if marginals: - P = marginals.marginalCovariance(lastKey) + covariance = marginals.marginalCovariance(lastKey) else: - P = None + covariance = None - plot_pose3(fignum, lastPose, P, scale) + plot_pose3(fignum, lastPose, covariance=covariance, + axis_length=scale) except: pass From 7a089ba79f13b1d6194fc6f632273b3e522c6bf6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 8 Jun 2020 13:46:12 -0500 Subject: [PATCH 2/2] Undo API changes --- cython/gtsam/utils/plot.py | 38 +++++++++++++++++++------------------- 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/cython/gtsam/utils/plot.py b/cython/gtsam/utils/plot.py index 51333a59c..bd6b5ab60 100644 --- a/cython/gtsam/utils/plot.py +++ b/cython/gtsam/utils/plot.py @@ -59,7 +59,7 @@ def ellipsoid(xc, yc, zc, rx, ry, rz, n): return x, y, z -def plot_covariance_ellipse_3d(axes, origin, covariance, scale=1, n=8, alpha=0.5): +def plot_covariance_ellipse_3d(axes, origin, P, scale=1, n=8, alpha=0.5): """ Plots a Gaussian as an uncertainty ellipse @@ -70,13 +70,13 @@ def plot_covariance_ellipse_3d(axes, origin, covariance, scale=1, n=8, alpha=0.5 Args: axes (matplotlib.axes.Axes): Matplotlib axes. origin (gtsam.Point3): The origin in the world frame. - covariance (numpy.ndarray): The marginal covariance matrix of the 3D point which will be represented as an ellipse. + P (numpy.ndarray): The marginal covariance matrix of the 3D point which will be represented as an ellipse. scale (float): Scaling factor of the radii of the covariance ellipse. n (int): Defines the granularity of the ellipse. Higher values indicate finer ellipses. alpha (float): Transparency value for the plotted surface in the range [0, 1]. """ k = 11.82 - U, S, _ = np.linalg.svd(covariance) + U, S, _ = np.linalg.svd(P) radii = k * np.sqrt(S) radii = radii * scale @@ -152,7 +152,7 @@ def plot_pose2(fignum, pose, axis_length=0.1, covariance=None): covariance=covariance) -def plot_point3_on_axes(axes, point, linespec, covariance=None): +def plot_point3_on_axes(axes, point, linespec, P=None): """ Plot a 3D point on given axis `axes` with given `linespec`. @@ -160,14 +160,14 @@ def plot_point3_on_axes(axes, point, linespec, covariance=None): axes (matplotlib.axes.Axes): Matplotlib axes. point (gtsam.Point3): The point to be plotted. linespec (string): String representing formatting options for Matplotlib. - covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. + P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. """ axes.plot([point.x()], [point.y()], [point.z()], linespec) - if covariance is not None: - plot_covariance_ellipse_3d(axes, point.vector(), covariance) + if P is not None: + plot_covariance_ellipse_3d(axes, point.vector(), P) -def plot_point3(fignum, point, linespec, covariance=None): +def plot_point3(fignum, point, linespec, P=None): """ Plot a 3D point on given figure with given `linespec`. @@ -175,11 +175,11 @@ def plot_point3(fignum, point, linespec, covariance=None): fignum (int): Integer representing the figure number to use for plotting. point (gtsam.Point3): The point to be plotted. linespec (string): String representing formatting options for Matplotlib. - covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. + P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. """ fig = plt.figure(fignum) axes = fig.gca(projection='3d') - plot_point3_on_axes(axes, point, linespec, covariance) + plot_point3_on_axes(axes, point, linespec, P) def plot_3d_points(fignum, values, linespec="g*", marginals=None): @@ -215,7 +215,7 @@ def plot_3d_points(fignum, values, linespec="g*", marginals=None): # I guess it's not a Point3 -def plot_pose3_on_axes(axes, pose, axis_length=0.1, covariance=None, scale=1): +def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1): """ Plot a 3D pose on given axis `axes` with given `axis_length`. @@ -223,7 +223,7 @@ def plot_pose3_on_axes(axes, pose, axis_length=0.1, covariance=None, scale=1): axes (matplotlib.axes.Axes): Matplotlib axes. point (gtsam.Point3): The point to be plotted. linespec (string): String representing formatting options for Matplotlib. - covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. + P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. """ # get rotation and translation (center) gRp = pose.rotation().matrix() # rotation from pose to global @@ -243,15 +243,15 @@ def plot_pose3_on_axes(axes, pose, axis_length=0.1, covariance=None, scale=1): axes.plot(line[:, 0], line[:, 1], line[:, 2], 'b-') # plot the covariance - if covariance is not None: + if P is not None: # covariance matrix in pose coordinate frame - pPp = covariance[3:6, 3:6] + pPp = P[3:6, 3:6] # convert the covariance matrix to global coordinate frame gPp = gRp @ pPp @ gRp.T plot_covariance_ellipse_3d(axes, origin, gPp) -def plot_pose3(fignum, pose, axis_length=0.1, covariance=None): +def plot_pose3(fignum, pose, axis_length=0.1, P=None): """ Plot a 3D pose on given figure with given `axis_length`. @@ -259,12 +259,12 @@ def plot_pose3(fignum, pose, axis_length=0.1, covariance=None): fignum (int): Integer representing the figure number to use for plotting. pose (gtsam.Pose3): 3D pose to be plotted. linespec (string): String representing formatting options for Matplotlib. - covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. + P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation. """ # get figure object fig = plt.figure(fignum) axes = fig.gca(projection='3d') - plot_pose3_on_axes(axes, pose, covariance=covariance, + plot_pose3_on_axes(axes, pose, P=P, axis_length=axis_length) @@ -303,7 +303,7 @@ def plot_trajectory(fignum, values, scale=1, marginals=None): else: covariance = None - plot_pose3(fignum, lastPose, covariance=covariance, + plot_pose3(fignum, lastPose, P=covariance, axis_length=scale) lastIndex = i @@ -318,7 +318,7 @@ def plot_trajectory(fignum, values, scale=1, marginals=None): else: covariance = None - plot_pose3(fignum, lastPose, covariance=covariance, + plot_pose3(fignum, lastPose, P=covariance, axis_length=scale) except: