update explanation of uncertainty ellipses, try to fix #1067
senselessDev
parent
0ef4d26850
commit
7fe577b553
|
|
@ -12,13 +12,42 @@ 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/
|
||||
# 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:
|
||||
"""
|
||||
|
|
@ -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.
|
||||
|
|
@ -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)
|
||||
|
|
@ -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,
|
||||
|
|
@ -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.
|
||||
|
|
@ -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,
|
||||
|
|
@ -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.
|
||||
|
|
@ -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.
|
||||
|
|
@ -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(
|
||||
|
|
@ -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.
|
||||
|
|
@ -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.
|
||||
|
|
@ -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.
|
||||
|
|
@ -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.
|
||||
|
|
@ -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.
|
||||
|
|
|
|||
Loading…
Reference in New Issue