From d9a00ded231ea074cd64bdf5cc87690ea5dc0f0a Mon Sep 17 00:00:00 2001 From: Calvin Date: Tue, 25 Jan 2022 16:39:05 -0600 Subject: [PATCH 1/7] 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 a632b852a..8060de2fb 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/7] 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 8060de2fb..820cefb7c 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/7] 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 820cefb7c..f409968a8 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. From d7fbaaab639416fc08d881c58751dd8022f02c62 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 27 Mar 2022 11:24:45 -0400 Subject: [PATCH 4/7] Bump version --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b86598663..cfb251663 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,7 +10,7 @@ endif() set (GTSAM_VERSION_MAJOR 4) set (GTSAM_VERSION_MINOR 2) set (GTSAM_VERSION_PATCH 0) -set (GTSAM_PRERELEASE_VERSION "a5") +set (GTSAM_PRERELEASE_VERSION "a6") math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}") if (${GTSAM_VERSION_PATCH} EQUAL 0) From 938d4093956335351a5f841bb14d70557876dd25 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 13 Apr 2022 21:34:54 -0400 Subject: [PATCH 5/7] Notebook --- .gitignore | 1 + python/gtsam/notebooks/ellipses.ipynb | 136 ++++++++++++++++++++++++++ 2 files changed, 137 insertions(+) create mode 100644 python/gtsam/notebooks/ellipses.ipynb diff --git a/.gitignore b/.gitignore index 0e34eed34..e3f7613fe 100644 --- a/.gitignore +++ b/.gitignore @@ -18,3 +18,4 @@ CMakeLists.txt.user* xcode/ /Dockerfile +/python/gtsam/notebooks/.ipynb_checkpoints/ellipses-checkpoint.ipynb diff --git a/python/gtsam/notebooks/ellipses.ipynb b/python/gtsam/notebooks/ellipses.ipynb new file mode 100644 index 000000000..d1ce9a015 --- /dev/null +++ b/python/gtsam/notebooks/ellipses.ipynb @@ -0,0 +1,136 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Ellipse Scaling\n", + "\n", + "The code to calculate the percentages included in ellipses with various values of \"k\" in `plot.py`.\n", + "\n", + "Thanks to @senselessDev, January 26, for providing the code in [PR #1067](https://github.com/borglab/gtsam/pull/1067)." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [], + "source": [ + "import scipy\n", + "import scipy.stats" + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "metadata": {}, + "outputs": [], + "source": [ + "def pct_to_sigma(pct, dof):\n", + " return np.sqrt(scipy.stats.chi2.ppf(pct / 100., df=dof))\n", + "\n", + "def sigma_to_pct(sigma, dof):\n", + " return scipy.stats.chi2.cdf(sigma**2, df=dof) * 100." + ] + }, + { + "cell_type": "code", + "execution_count": 23, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "0D\t 1 \t 2 \t 3 \t 4 \t 5 \n", + "1D\t68.26895%\t95.44997%\t99.73002%\t99.99367%\t99.99994%\n", + "2D\t39.34693%\t86.46647%\t98.88910%\t99.96645%\t99.99963%\n", + "3D\t19.87480%\t73.85359%\t97.07091%\t99.88660%\t99.99846%\n" + ] + } + ], + "source": [ + "for dim in range(0, 4):\n", + " print(\"{}D\".format(dim), end=\"\")\n", + " for sigma in range(1, 6):\n", + " if dim == 0: print(\"\\t {} \".format(sigma), end=\"\")\n", + " else: print(\"\\t{:.5f}%\".format(sigma_to_pct(sigma, dim)), end=\"\")\n", + " print()" + ] + }, + { + "cell_type": "code", + "execution_count": 29, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "1D\n", + "\n", + "sigma=1.0 -> 68.26895%\n", + "sigma=2.0 -> 95.44997%\n", + "sigma=2.5 -> 98.75807%\n", + "sigma=5.0 -> 99.99994%\n", + "\n", + "2D\n", + "\n", + "sigma=1.0 -> 39.34693%\n", + "sigma=2.0 -> 86.46647%\n", + "sigma=2.5 -> 95.60631%\n", + "sigma=5.0 -> 99.99963%\n", + "\n", + "3D\n", + "\n", + "sigma=1.0 -> 19.87480%\n", + "sigma=2.0 -> 73.85359%\n", + "sigma=2.5 -> 89.99392%\n", + "sigma=5.0 -> 99.99846%\n", + "\n" + ] + } + ], + "source": [ + "for dim in range(1, 4):\n", + " print(\"{}D\\n\".format(dim))\n", + " for sigma in [1, 2, 2.5, 5]:\n", + " print(\"sigma={:.1f} -> {:.5f}%\".format(sigma, sigma_to_pct(sigma, dim)), end=\"\")\n", + " print()\n", + " print()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "interpreter": { + "hash": "341996cd3f3db7b5e0d1eaea072c5502d80452314e72e6b77c40445f6e9ba101" + }, + "kernelspec": { + "display_name": "Python 3 (ipykernel)", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.10.4" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} From 7fe577b553f92232bce23e049bc089fdb765e702 Mon Sep 17 00:00:00 2001 From: senselessDev Date: Fri, 22 Apr 2022 18:39:51 +0200 Subject: [PATCH 6/7] 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 abadf62aa..db78135b7 100644 --- a/python/gtsam/utils/plot.py +++ b/python/gtsam/utils/plot.py @@ -12,13 +12,42 @@ from mpl_toolkits.mplot3d import Axes3D # pylint: disable=unused-import 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. From de1827ca40b100cd08bfdb69341e9e7d44ed10a4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 10 May 2022 22:50:10 -0400 Subject: [PATCH 7/7] Added notebook with SenselessDev code. --- python/gtsam/notebooks/ellipses.ipynb | 55 +++++++++++++-------------- python/gtsam/utils/plot.py | 38 ++++++------------ 2 files changed, 37 insertions(+), 56 deletions(-) diff --git a/python/gtsam/notebooks/ellipses.ipynb b/python/gtsam/notebooks/ellipses.ipynb index d1ce9a015..06938f696 100644 --- a/python/gtsam/notebooks/ellipses.ipynb +++ b/python/gtsam/notebooks/ellipses.ipynb @@ -13,17 +13,18 @@ }, { "cell_type": "code", - "execution_count": 3, + "execution_count": 5, "metadata": {}, "outputs": [], "source": [ "import scipy\n", - "import scipy.stats" + "import scipy.stats\n", + "import numpy as np" ] }, { "cell_type": "code", - "execution_count": 9, + "execution_count": 6, "metadata": {}, "outputs": [], "source": [ @@ -36,7 +37,7 @@ }, { "cell_type": "code", - "execution_count": 23, + "execution_count": 7, "metadata": {}, "outputs": [ { @@ -51,17 +52,17 @@ } ], "source": [ - "for dim in range(0, 4):\n", - " print(\"{}D\".format(dim), end=\"\")\n", + "for dof in range(0, 4):\n", + " print(\"{}D\".format(dof), end=\"\")\n", " for sigma in range(1, 6):\n", - " if dim == 0: print(\"\\t {} \".format(sigma), end=\"\")\n", - " else: print(\"\\t{:.5f}%\".format(sigma_to_pct(sigma, dim)), end=\"\")\n", + " if dof == 0: print(\"\\t {} \".format(sigma), end=\"\")\n", + " else: print(\"\\t{:.5f}%\".format(sigma_to_pct(sigma, dof)), end=\"\")\n", " print()" ] }, { "cell_type": "code", - "execution_count": 29, + "execution_count": 12, "metadata": {}, "outputs": [ { @@ -70,34 +71,30 @@ "text": [ "1D\n", "\n", - "sigma=1.0 -> 68.26895%\n", - "sigma=2.0 -> 95.44997%\n", - "sigma=2.5 -> 98.75807%\n", - "sigma=5.0 -> 99.99994%\n", + "pct=50.0 -> sigma=0.674489750196\n", + "pct=95.0 -> sigma=1.959963984540\n", + "pct=99.0 -> sigma=2.575829303549\n", "\n", "2D\n", "\n", - "sigma=1.0 -> 39.34693%\n", - "sigma=2.0 -> 86.46647%\n", - "sigma=2.5 -> 95.60631%\n", - "sigma=5.0 -> 99.99963%\n", + "pct=50.0 -> sigma=1.177410022515\n", + "pct=95.0 -> sigma=2.447746830681\n", + "pct=99.0 -> sigma=3.034854258770\n", "\n", "3D\n", "\n", - "sigma=1.0 -> 19.87480%\n", - "sigma=2.0 -> 73.85359%\n", - "sigma=2.5 -> 89.99392%\n", - "sigma=5.0 -> 99.99846%\n", + "pct=50.0 -> sigma=1.538172254455\n", + "pct=95.0 -> sigma=2.795483482915\n", + "pct=99.0 -> sigma=3.368214175219\n", "\n" ] } ], "source": [ - "for dim in range(1, 4):\n", - " print(\"{}D\\n\".format(dim))\n", - " for sigma in [1, 2, 2.5, 5]:\n", - " print(\"sigma={:.1f} -> {:.5f}%\".format(sigma, sigma_to_pct(sigma, dim)), end=\"\")\n", - " print()\n", + "for dof in range(1, 4):\n", + " print(\"{}D\\n\".format(dof))\n", + " for pct in [50, 95, 99]:\n", + " print(\"pct={:.1f} -> sigma={:.12f}\".format(pct, pct_to_sigma(pct, dof)))\n", " print()" ] }, @@ -111,10 +108,10 @@ ], "metadata": { "interpreter": { - "hash": "341996cd3f3db7b5e0d1eaea072c5502d80452314e72e6b77c40445f6e9ba101" + "hash": "4d608302ba82e7596903db5446e6fa05f049271852e8cc6e1cafaafe5fbd9fed" }, "kernelspec": { - "display_name": "Python 3 (ipykernel)", + "display_name": "Python 3.8.13 ('gtsfm-v1')", "language": "python", "name": "python3" }, @@ -128,7 +125,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.10.4" + "version": "3.8.13" } }, "nbformat": 4, diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py index db78135b7..a4d19f72b 100644 --- a/python/gtsam/utils/plot.py +++ b/python/gtsam/utils/plot.py @@ -13,41 +13,25 @@ import gtsam from gtsam import Marginals, Point2, Point3, Pose2, Pose3, Values -# 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. +# For translation between a scaling of the uncertainty ellipse and the +# percentage of inliers see discussion in +# [PR 1067](https://github.com/borglab/gtsam/pull/1067) +# and the notebook python/gtsam/notebooks/ellipses.ipynb (needs scipy). # # 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 +# 1D: 1.959963984540 +# 2D: 2.447746830681 +# 3D: 2.795483482915 # # 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/ +# 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: """ @@ -123,7 +107,7 @@ def plot_covariance_ellipse_3d(axes, alpha: Transparency value for the plotted surface in the range [0, 1]. """ # this corresponds to 95%, see note above - k = 2.7954834829151074 + k = 2.795483482915 U, S, _ = np.linalg.svd(P) radii = k * np.sqrt(S) @@ -160,10 +144,10 @@ def plot_covariance_ellipse_2d(axes, which will be represented as an ellipse. """ - w, v = np.linalg.eig(covariance) + w, v = np.linalg.eigh(covariance) # this corresponds to 95%, see note above - k = 2.447746830680816 + k = 2.447746830681 angle = np.arctan2(v[1, 0], v[0, 0]) # We multiply k by 2 since k corresponds to the radius but Ellipse uses