Skip to content

Commit

Permalink
Merge pull request #1193 from borglab/fix/ellipses
Browse files Browse the repository at this point in the history
  • Loading branch information
@dellaert
dellaert authored May 11, 2022
2 parents e362906 + ce2cf72 commit e5c87a8
Show file tree
Hide file tree
Showing 3 changed files with 218 additions and 38 deletions.
1 change: 1 addition & 0 deletions .gitignore
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,3 +18,4 @@
CMakeLists.txt.user*
xcode/
/Dockerfile
/python/gtsam/notebooks/.ipynb_checkpoints/ellipses-checkpoint.ipynb
133 changes: 133 additions & 0 deletions python/gtsam/notebooks/ellipses.ipynb
View file
Original file line number Diff line number Diff line change
@@ -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
}
122 changes: 84 additions & 38 deletions python/gtsam/utils/plot.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:
"""
Expand Down Expand Up @@ -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.
Expand All @@ -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)
Expand All @@ -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.
Expand All @@ -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,
Expand All @@ -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.
Expand Down Expand Up @@ -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.
Expand All @@ -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(
Expand All @@ -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.
Expand Down Expand Up @@ -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.
Expand All @@ -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.
Expand Down Expand Up @@ -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.
Expand Down Expand Up @@ -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.
Expand Down

0 comments on commit e5c87a8

Please # to comment.