Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix uncertainty ellipses in python helpers #1177

Merged
merged 1 commit into from
May 10, 2022
Merged

Fix uncertainty ellipses in python helpers #1177

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
155 changes: 96 additions & 59 deletions python/gtsam/utils/plot.py
View file
Open in desktop
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)
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Actually, changing that to eigh is even better I think. This would prevent from getting complex results due to float precision issues. Comment?


# 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