From d9a00ded231ea074cd64bdf5cc87690ea5dc0f0a Mon Sep 17 00:00:00 2001 From: Calvin Date: Tue, 25 Jan 2022 16:39:05 -0600 Subject: [PATCH 1/3] Modified the scaling values for plotting uncertainty ellipses to properly display 3 standard deviations for both 2D and 3D cases. --- python/gtsam/utils/plot.py | 42 ++++++++++++++++++++++++-------------- 1 file changed, 27 insertions(+), 15 deletions(-) diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py index a632b852a2..8060de2fb5 100644 --- a/python/gtsam/utils/plot.py +++ b/python/gtsam/utils/plot.py @@ -75,8 +75,9 @@ def plot_covariance_ellipse_3d(axes, 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 + For the 3D case: + k = 3.527 corresponds to 1 std, 68.26% of all probability + k = 14.157 corresponds to 3 std, 99.74% of all probability Args: axes (matplotlib.axes.Axes): Matplotlib axes. @@ -87,7 +88,7 @@ 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]. """ - k = 11.82 + k = np.sqrt(14.157) U, S, _ = np.linalg.svd(P) radii = k * np.sqrt(S) @@ -113,7 +114,14 @@ def plot_point2_on_axes(axes, linespec: str, P: Optional[np.ndarray] = None) -> None: """ - Plot a 2D point on given axis `axes` with given `linespec`. + 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 = 2.296 corresponds to 1 std, 68.26% of all probability + k = 11.820 corresponds to 3 std, 99.74% of all probability Args: axes (matplotlib.axes.Axes): Matplotlib axes. @@ -125,16 +133,15 @@ def plot_point2_on_axes(axes, 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 + # Scaling value for the uncertainty ellipse, we multiply by 2 because + # matplotlib takes the diameter and not the radius of the major and + # minor axes of the ellipse. + k = 2*np.sqrt(11.820) angle = np.arctan2(v[1, 0], v[0, 0]) e1 = patches.Ellipse(point, - np.sqrt(w[0] * k), - np.sqrt(w[1] * k), + np.sqrt(w[0]) * k, + np.sqrt(w[1]) * k, np.rad2deg(angle), fill=False) axes.add_patch(e1) @@ -178,6 +185,12 @@ 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 = 2.296 corresponds to 1 std, 68.26% of all probability + k = 11.820 corresponds to 3 std, 99.74% of all probability + Args: axes (matplotlib.axes.Axes): Matplotlib axes. pose: The pose to be plotted. @@ -205,13 +218,12 @@ def plot_pose2_on_axes(axes, w, v = np.linalg.eig(gPp) - # k = 2.296 - k = 5.0 + k = 2*np.sqrt(11.820) angle = np.arctan2(v[1, 0], v[0, 0]) e1 = patches.Ellipse(origin, - np.sqrt(w[0] * k), - np.sqrt(w[1] * k), + np.sqrt(w[0]) * k, + np.sqrt(w[1]) * k, np.rad2deg(angle), fill=False) axes.add_patch(e1) From 1b817760dd10d2d08579f4021968eeda262f34c1 Mon Sep 17 00:00:00 2001 From: Calvin Date: Fri, 28 Jan 2022 13:31:11 -0600 Subject: [PATCH 2/3] Changed all instances of the Sigma value, k, to 5 for plotting the covariance ellipse. --- python/gtsam/utils/plot.py | 30 ++++++++++++++++++++---------- 1 file changed, 20 insertions(+), 10 deletions(-) diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py index 8060de2fb5..820cefb7c7 100644 --- a/python/gtsam/utils/plot.py +++ b/python/gtsam/utils/plot.py @@ -79,6 +79,8 @@ def plot_covariance_ellipse_3d(axes, k = 3.527 corresponds to 1 std, 68.26% of all probability k = 14.157 corresponds to 3 std, 99.74% of all probability + We choose k = 5 which corresponds to 99.99846% of all probability in 3D + Args: axes (matplotlib.axes.Axes): Matplotlib axes. origin: The origin in the world frame. @@ -88,7 +90,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]. """ - k = np.sqrt(14.157) + # Sigma value corresponding to the covariance ellipse + k = 5 U, S, _ = np.linalg.svd(P) radii = k * np.sqrt(S) @@ -123,6 +126,8 @@ def plot_point2_on_axes(axes, k = 2.296 corresponds to 1 std, 68.26% of all probability k = 11.820 corresponds to 3 std, 99.74% of all probability + We choose k = 5 which corresponds to 99.99963% of all probability for 2D. + Args: axes (matplotlib.axes.Axes): Matplotlib axes. point: The point to be plotted. @@ -133,15 +138,15 @@ def plot_point2_on_axes(axes, if P is not None: w, v = np.linalg.eig(P) - # Scaling value for the uncertainty ellipse, we multiply by 2 because - # matplotlib takes the diameter and not the radius of the major and - # minor axes of the ellipse. - k = 2*np.sqrt(11.820) + # Sigma value corresponding to the covariance ellipse + k = 5 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]) * k, - np.sqrt(w[1]) * k, + np.sqrt(w[0]) * 2 * k, + np.sqrt(w[1]) * 2 * k, np.rad2deg(angle), fill=False) axes.add_patch(e1) @@ -191,6 +196,8 @@ def plot_pose2_on_axes(axes, k = 2.296 corresponds to 1 std, 68.26% of all probability k = 11.820 corresponds to 3 std, 99.74% of all probability + We choose k = 5 which corresponds to 99.99963% of all probability for 2D. + Args: axes (matplotlib.axes.Axes): Matplotlib axes. pose: The pose to be plotted. @@ -218,12 +225,15 @@ def plot_pose2_on_axes(axes, w, v = np.linalg.eig(gPp) - k = 2*np.sqrt(11.820) + # Sigma value corresponding to the covariance ellipse + k = 5 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]) * k, - np.sqrt(w[1]) * k, + np.sqrt(w[0]) * 2 * k, + np.sqrt(w[1]) * 2 * k, np.rad2deg(angle), fill=False) axes.add_patch(e1) From e524e2806b6a341053a7ab4a425a5fe0a4afe72e Mon Sep 17 00:00:00 2001 From: Calvin Date: Fri, 28 Jan 2022 14:16:30 -0600 Subject: [PATCH 3/3] Updated comments to reflect standard deviations instead of variances in the discussions regarding the values for k. --- python/gtsam/utils/plot.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py index 820cefb7c7..f409968a8d 100644 --- a/python/gtsam/utils/plot.py +++ b/python/gtsam/utils/plot.py @@ -76,8 +76,8 @@ def plot_covariance_ellipse_3d(axes, Based on Maybeck Vol 1, page 366 For the 3D case: - k = 3.527 corresponds to 1 std, 68.26% of all probability - k = 14.157 corresponds to 3 std, 99.74% of all probability + 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 @@ -123,8 +123,8 @@ def plot_point2_on_axes(axes, Based on Stochastic Models, Estimation, and Control Vol 1 by Maybeck, page 366 For the 2D case: - k = 2.296 corresponds to 1 std, 68.26% of all probability - k = 11.820 corresponds to 3 std, 99.74% of all probability + 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. @@ -193,8 +193,8 @@ def plot_pose2_on_axes(axes, Based on Stochastic Models, Estimation, and Control Vol 1 by Maybeck, page 366 For the 2D case: - k = 2.296 corresponds to 1 std, 68.26% of all probability - k = 11.820 corresponds to 3 std, 99.74% of all probability + 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.