From de1827ca40b100cd08bfdb69341e9e7d44ed10a4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 10 May 2022 22:50:10 -0400 Subject: [PATCH] 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