From 7fe577b553f92232bce23e049bc089fdb765e702 Mon Sep 17 00:00:00 2001 From: senselessDev Date: Fri, 22 Apr 2022 18:39:51 +0200 Subject: [PATCH] update explanation of uncertainty ellipses, try to fix #1067 --- python/gtsam/utils/plot.py | 155 +++++++++++++++++++++++-------------- 1 file changed, 96 insertions(+), 59 deletions(-) diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py index abadf62aaf..db78135b76 100644 --- a/python/gtsam/utils/plot.py +++ b/python/gtsam/utils/plot.py @@ -12,13 +12,42 @@ import gtsam from gtsam import Marginals, Point2, Point3, Pose2, Pose3, Values -# For future reference: following -# https://www.xarg.org/2018/04/how-to-plot-a-covariance-error-ellipse/ -# we have, in 2D: -# def kk(p): return math.sqrt(-2*math.log(1-p)) # k to get p probability mass -# def pp(k): return 1-math.exp(-float(k**2)/2.0) # p as a function of k -# Some values: -# k = 5 => p = 99.9996 % + +# For translation between a scaling of the uncertainty ellipse and the percentage of +# inliers see discussion in https://github.com/borglab/gtsam/pull/1067 +# +# Both directions can be calculated by the following functions: +# def pct_to_sigma(pct, dof): +# return np.sqrt(scipy.stats.chi2.ppf(pct / 100., df=dof)) +# +# def sigma_to_pct(sigma, dof): +# return scipy.stats.chi2.cdf(sigma**2, df=dof) * 100. +# +# In the following, the default scaling is chosen for 95% inliers, which +# translates to the following sigma values: +# 2D: pct_to_sigma(95, 2) -> 2.447746830680816 +# 3D: pct_to_sigma(95, 3) -> 2.7954834829151074 +# +# Further references are Stochastic Models, Estimation, and Control Vol 1 by Maybeck, +# page 366 and https://www.xarg.org/2018/04/how-to-plot-a-covariance-error-ellipse/ +# +# For reference, here are the inlier percentages for some sigma values: +# 1 2 3 4 5 +# 1D 68.26895 95.44997 99.73002 99.99367 99.99994 +# 2D 39.34693 86.46647 98.88910 99.96645 99.99963 +# 3D 19.87480 73.85359 97.07091 99.88660 99.99846 +# +# This can be generated by: +# for dim in range(0, 4): +# print("{}D".format(dim), end="") +# for n_sigma in range(1, 6): +# if dim == 0: print("\t {} ".format(n_sigma), end="") +# else: print("\t{:.5f}".format(sigma_to_pct(n_sigma, dim)), end="") +# print() +# +# The translation routines are not included in GTSAM, because it would introduce +# scipy as a new dependency. + def set_axes_equal(fignum: int) -> None: """ @@ -81,12 +110,8 @@ def plot_covariance_ellipse_3d(axes, """ Plots a Gaussian as an uncertainty ellipse - Based on Maybeck Vol 1, page 366 - For the 3D case: - k = 1.878 corresponds to 1 std, 68.26% of all probability - k = 3.763 corresponds to 3 std, 99.74% of all probability - - We choose k = 5 which corresponds to 99.99846% of all probability in 3D + The ellipse is scaled in such a way that 95% of drawn samples are inliers. + Derivation of the scaling factor is explained at the beginning of this file. Args: axes (matplotlib.axes.Axes): Matplotlib axes. @@ -97,8 +122,8 @@ def plot_covariance_ellipse_3d(axes, n: Defines the granularity of the ellipse. Higher values indicate finer ellipses. alpha: Transparency value for the plotted surface in the range [0, 1]. """ - # Sigma value corresponding to the covariance ellipse - k = 5 + # this corresponds to 95%, see note above + k = 2.7954834829151074 U, S, _ = np.linalg.svd(P) radii = k * np.sqrt(S) @@ -119,6 +144,38 @@ def plot_covariance_ellipse_3d(axes, axes.plot_surface(x, y, z, alpha=alpha, cmap='hot') +def plot_covariance_ellipse_2d(axes, + origin: Point2, + covariance: np.ndarray) -> None: + """ + Plots a Gaussian as an uncertainty ellipse + + The ellipse is scaled in such a way that 95% of drawn samples are inliers. + Derivation of the scaling factor is explained at the beginning of this file. + + Args: + axes (matplotlib.axes.Axes): Matplotlib axes. + origin: The origin in the world frame. + covariance: The marginal covariance matrix of the 2D point + which will be represented as an ellipse. + """ + + w, v = np.linalg.eig(covariance) + + # this corresponds to 95%, see note above + k = 2.447746830680816 + + angle = np.arctan2(v[1, 0], v[0, 0]) + # We multiply k by 2 since k corresponds to the radius but Ellipse uses + # the diameter. + e1 = patches.Ellipse(origin, + np.sqrt(w[0]) * 2 * k, + np.sqrt(w[1]) * 2 * k, + np.rad2deg(angle), + fill=False) + axes.add_patch(e1) + + def plot_point2_on_axes(axes, point: Point2, linespec: str, @@ -127,13 +184,8 @@ def plot_point2_on_axes(axes, Plot a 2D point and its corresponding uncertainty ellipse on given axis `axes` with given `linespec`. - Based on Stochastic Models, Estimation, and Control Vol 1 by Maybeck, - page 366 - For the 2D case: - k = 1.515 corresponds to 1 std, 68.26% of all probability - k = 3.438 corresponds to 3 std, 99.74% of all probability - - We choose k = 5 which corresponds to 99.99963% of all probability for 2D. + The uncertainty ellipse (if covariance is given) is scaled in such a way + that 95% of drawn samples are inliers, see `plot_covariance_ellipse_2d`. Args: axes (matplotlib.axes.Axes): Matplotlib axes. @@ -143,21 +195,7 @@ def plot_point2_on_axes(axes, """ axes.plot([point[0]], [point[1]], linespec, marker='.', markersize=10) if P is not None: - w, v = np.linalg.eig(P) - - # 5 sigma corresponds to 99.9996%, see note above - k = 5.0 - - angle = np.arctan2(v[1, 0], v[0, 0]) - # We multiply k by 2 since k corresponds to the radius but Ellipse uses - # the diameter. - e1 = patches.Ellipse(point, - np.sqrt(w[0]) * 2 * k, - np.sqrt(w[1]) * 2 * k, - np.rad2deg(angle), - fill=False) - axes.add_patch(e1) - + plot_covariance_ellipse_2d(axes, point, P) def plot_point2( fignum: int, @@ -169,6 +207,9 @@ def plot_point2( """ Plot a 2D point on given figure with given `linespec`. + The uncertainty ellipse (if covariance is given) is scaled in such a way + that 95% of drawn samples are inliers, see `plot_covariance_ellipse_2d`. + Args: fignum: Integer representing the figure number to use for plotting. point: The point to be plotted. @@ -197,13 +238,8 @@ def plot_pose2_on_axes(axes, """ Plot a 2D pose on given axis `axes` with given `axis_length`. - Based on Stochastic Models, Estimation, and Control Vol 1 by Maybeck, - page 366 - For the 2D case: - k = 1.515 corresponds to 1 std, 68.26% of all probability - k = 3.438 corresponds to 3 std, 99.74% of all probability - - We choose k = 5 which corresponds to 99.99963% of all probability for 2D. + The ellipse is scaled in such a way that 95% of drawn samples are inliers, + see `plot_covariance_ellipse_2d`. Args: axes (matplotlib.axes.Axes): Matplotlib axes. @@ -229,21 +265,7 @@ def plot_pose2_on_axes(axes, if covariance is not None: pPp = covariance[0:2, 0:2] gPp = np.matmul(np.matmul(gRp, pPp), gRp.T) - - w, v = np.linalg.eig(gPp) - - # 5 sigma corresponds to 99.9996%, see note above - k = 5.0 - - angle = np.arctan2(v[1, 0], v[0, 0]) - # We multiply k by 2 since k corresponds to the radius but Ellipse uses - # the diameter. - e1 = patches.Ellipse(origin, - np.sqrt(w[0]) * 2 * k, - np.sqrt(w[1]) * 2 * k, - np.rad2deg(angle), - fill=False) - axes.add_patch(e1) + plot_covariance_ellipse_2d(axes, origin, gPp) def plot_pose2( @@ -256,6 +278,9 @@ def plot_pose2( """ Plot a 2D pose on given figure with given `axis_length`. + The uncertainty ellipse (if covariance is given) is scaled in such a way + that 95% of drawn samples are inliers, see `plot_covariance_ellipse_2d`. + Args: fignum: Integer representing the figure number to use for plotting. pose: The pose to be plotted. @@ -285,6 +310,9 @@ def plot_point3_on_axes(axes, """ Plot a 3D point on given axis `axes` with given `linespec`. + The uncertainty ellipse (if covariance is given) is scaled in such a way + that 95% of drawn samples are inliers, see `plot_covariance_ellipse_3d`. + Args: axes (matplotlib.axes.Axes): Matplotlib axes. point: The point to be plotted. @@ -306,6 +334,9 @@ def plot_point3( """ Plot a 3D point on given figure with given `linespec`. + The uncertainty ellipse (if covariance is given) is scaled in such a way + that 95% of drawn samples are inliers, see `plot_covariance_ellipse_3d`. + Args: fignum: Integer representing the figure number to use for plotting. point: The point to be plotted. @@ -380,6 +411,9 @@ 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`. + The uncertainty ellipse (if covariance is given) is scaled in such a way + that 95% of drawn samples are inliers, see `plot_covariance_ellipse_3d`. + Args: axes (matplotlib.axes.Axes): Matplotlib axes. point (gtsam.Point3): The point to be plotted. @@ -422,6 +456,9 @@ def plot_pose3( """ Plot a 3D pose on given figure with given `axis_length`. + The uncertainty ellipse (if covariance is given) is scaled in such a way + that 95% of drawn samples are inliers, see `plot_covariance_ellipse_3d`. + Args: fignum: Integer representing the figure number to use for plotting. pose (gtsam.Pose3): 3D pose to be plotted.