diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py index 7ea393077..a632b852a 100644 --- a/python/gtsam/utils/plot.py +++ b/python/gtsam/utils/plot.py @@ -10,7 +10,7 @@ from matplotlib import patches from mpl_toolkits.mplot3d import Axes3D # pylint: disable=unused-import import gtsam -from gtsam import Marginals, Point3, Pose2, Pose3, Values +from gtsam import Marginals, Point2, Point3, Pose2, Pose3, Values def set_axes_equal(fignum: int) -> None: @@ -108,6 +108,69 @@ def plot_covariance_ellipse_3d(axes, axes.plot_surface(x, y, z, alpha=alpha, cmap='hot') +def plot_point2_on_axes(axes, + point: Point2, + linespec: str, + P: Optional[np.ndarray] = None) -> None: + """ + Plot a 2D point on given axis `axes` with given `linespec`. + + Args: + axes (matplotlib.axes.Axes): Matplotlib axes. + point: The point to be plotted. + linespec: String representing formatting options for Matplotlib. + P: Marginal covariance matrix to plot the uncertainty of the estimation. + """ + axes.plot([point[0]], [point[1]], linespec, marker='.', markersize=10) + if P is not None: + w, v = np.linalg.eig(P) + + # "Sigma" value for drawing the uncertainty ellipse. 5 sigma corresponds + # to a 99.9999% confidence, i.e. assuming the estimation has been + # computed properly, there is a 99.999% chance that the true position + # of the point will lie within the uncertainty ellipse. + k = 5.0 + + angle = np.arctan2(v[1, 0], v[0, 0]) + e1 = patches.Ellipse(point, + np.sqrt(w[0] * k), + np.sqrt(w[1] * k), + np.rad2deg(angle), + fill=False) + axes.add_patch(e1) + + +def plot_point2( + fignum: int, + point: Point2, + linespec: str, + P: np.ndarray = None, + axis_labels: Iterable[str] = ("X axis", "Y axis"), +) -> plt.Figure: + """ + Plot a 2D point on given figure with given `linespec`. + + Args: + fignum: Integer representing the figure number to use for plotting. + point: The point to be plotted. + linespec: String representing formatting options for Matplotlib. + P: Marginal covariance matrix to plot the uncertainty of the estimation. + axis_labels: List of axis labels to set. + + Returns: + fig: The matplotlib figure. + + """ + fig = plt.figure(fignum) + axes = fig.gca() + plot_point2_on_axes(axes, point, linespec, P) + + axes.set_xlabel(axis_labels[0]) + axes.set_ylabel(axis_labels[1]) + + return fig + + def plot_pose2_on_axes(axes, pose: Pose2, axis_length: float = 0.1,