Merge pull request #1193 from borglab/fix/ellipses

.gitignore

@@ -18,3 +18,4 @@
 CMakeLists.txt.user*
 xcode/
 /Dockerfile
+/python/gtsam/notebooks/.ipynb_checkpoints/ellipses-checkpoint.ipynb

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
+}

python/gtsam/utils/plot.py

@@ -12,13 +12,26 @@ 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/
+# For translation between a scaling of the uncertainty ellipse and the 
-# we have, in 2D:
+# percentage of inliers see discussion in 
-# def kk(p): return math.sqrt(-2*math.log(1-p)) # k to get p probability mass
+#   [PR 1067](https://github.com/borglab/gtsam/pull/1067)
-# def pp(k): return 1-math.exp(-float(k**2)/2.0) # p as a function of k
+# and the notebook python/gtsam/notebooks/ellipses.ipynb (needs scipy).
-# Some values:
+#
-# k = 5 => p = 99.9996 %
+# 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
+    The ellipse is scaled in such a way that 95% of drawn samples are inliers.
-    k=2.296 corresponds to 1 std, 68.26% of all probability
+    Derivation of the scaling factor is explained at the beginning of this file.
-    k=11.82 corresponds to 3 std, 99.74% of all probability
 
     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)
+        plot_covariance_ellipse_2d(axes, point, 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)
-
 
 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)
-
+        plot_covariance_ellipse_2d(axes, origin, gPp)
-        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)
 
 
 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.