Skip to content

Commit

Permalink
Merge pull request #1177 from senselessDev/fix/ellipses
Browse files Browse the repository at this point in the history
  • Loading branch information
@dellaert
dellaert authored May 10, 2022
2 parents 938d409 + 7fe577b commit 638d391
Showing 1 changed file with 96 additions and 59 deletions.
155 changes: 96 additions & 59 deletions python/gtsam/utils/plot.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,13 +12,42 @@
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:
"""
Expand Down Expand Up @@ -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.
Expand All @@ -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)
Expand All @@ -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,
Expand All @@ -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.
Expand All @@ -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,
Expand All @@ -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.
Expand Down Expand Up @@ -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.
Expand All @@ -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(
Expand All @@ -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.
Expand Down Expand Up @@ -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.
Expand All @@ -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.
Expand Down Expand Up @@ -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.
Expand Down Expand Up @@ -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.
Expand Down

0 comments on commit 638d391

Please sign in to comment.