diff --git a/.gitignore b/.gitignore index 0e34eed347..e3f7613fee 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 0000000000..06938f6968 --- /dev/null +++ b/python/gtsam/notebooks/ellipses.ipynb @@ -0,0 +1,133 @@ +{ + "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": 5, + "metadata": {}, + "outputs": [], + "source": [ + "import scipy\n", + "import scipy.stats\n", + "import numpy as np" + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "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": 7, + "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 dof in range(0, 4):\n", + " print(\"{}D\".format(dof), end=\"\")\n", + " for sigma in range(1, 6):\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": 12, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "1D\n", + "\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", + "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", + "pct=50.0 -> sigma=1.538172254455\n", + "pct=95.0 -> sigma=2.795483482915\n", + "pct=99.0 -> sigma=3.368214175219\n", + "\n" + ] + } + ], + "source": [ + "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()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "interpreter": { + "hash": "4d608302ba82e7596903db5446e6fa05f049271852e8cc6e1cafaafe5fbd9fed" + }, + "kernelspec": { + "display_name": "Python 3.8.13 ('gtsfm-v1')", + "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.8.13" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py index 5ff7fd7aa7..a4d19f72be 100644 --- a/python/gtsam/utils/plot.py +++ b/python/gtsam/utils/plot.py @@ -12,13 +12,26 @@ 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 +# [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: +# 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/ +# +# 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 def set_axes_equal(fignum: int) -> None: """ @@ -81,9 +94,8 @@ 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 + 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. @@ -94,7 +106,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 = 11.82 + # this corresponds to 95%, see note above + k = 2.795483482915 U, S, _ = np.linalg.svd(P) radii = k * np.sqrt(S) @@ -115,12 +128,48 @@ 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.eigh(covariance) + + # this corresponds to 95%, see note above + 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 + # 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, 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`. + + 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. @@ -130,19 +179,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]) - e1 = patches.Ellipse(point, - np.sqrt(w[0] * k), - np.sqrt(w[1] * k), - np.rad2deg(angle), - fill=False) - axes.add_patch(e1) - + plot_covariance_ellipse_2d(axes, point, P) def plot_point2( fignum: int, @@ -154,6 +191,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. @@ -182,6 +222,9 @@ def plot_pose2_on_axes(axes, """ Plot a 2D pose on given axis `axes` with given `axis_length`. + 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. pose: The pose to be plotted. @@ -206,19 +249,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]) - e1 = patches.Ellipse(origin, - np.sqrt(w[0] * k), - np.sqrt(w[1] * k), - np.rad2deg(angle), - fill=False) - axes.add_patch(e1) + plot_covariance_ellipse_2d(axes, origin, gPp) def plot_pose2( @@ -231,6 +262,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. @@ -260,6 +294,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. @@ -281,6 +318,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. @@ -355,6 +395,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. @@ -397,6 +440,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.