-
Notifications
You must be signed in to change notification settings - Fork 765
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.
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
Updated scaling factors for proper geometric representation of uncertainty ellipses #1067
Changes from 2 commits
d9a00de
1b81776
e524e28
df7fb47
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -75,8 +75,11 @@ 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 | ||
For the 3D case: | ||
k = 3.527 corresponds to 1 std, 68.26% of all probability | ||
k = 14.157 corresponds to 3 std, 99.74% of all probability | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. I think these (3.5 and 14.1) should still be changed to standard deviation |
||
|
||
We choose k = 5 which corresponds to 99.99846% of all probability in 3D | ||
|
||
Args: | ||
axes (matplotlib.axes.Axes): Matplotlib axes. | ||
|
@@ -87,7 +90,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 | ||
# Sigma value corresponding to the covariance ellipse | ||
k = 5 | ||
U, S, _ = np.linalg.svd(P) | ||
|
||
radii = k * np.sqrt(S) | ||
|
@@ -113,7 +117,16 @@ def plot_point2_on_axes(axes, | |
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`. | ||
|
||
Based on Stochastic Models, Estimation, and Control Vol 1 by Maybeck, | ||
page 366 | ||
For the 2D case: | ||
k = 2.296 corresponds to 1 std, 68.26% of all probability | ||
k = 11.820 corresponds to 3 std, 99.74% of all probability | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more.
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. same |
||
|
||
We choose k = 5 which corresponds to 99.99963% of all probability for 2D. | ||
|
||
Args: | ||
axes (matplotlib.axes.Axes): Matplotlib axes. | ||
|
@@ -125,16 +138,15 @@ def plot_point2_on_axes(axes, | |
if P is not None: | ||
w, v = np.linalg.eig(P) | ||
|
||
# "Sigma" value for drawing the uncertainty ellipse. 5 sigma corresponds | ||
# to a 99.9999% confidence, i.e. assuming the estimation has been | ||
# computed properly, there is a 99.999% chance that the true position | ||
# of the point will lie within the uncertainty ellipse. | ||
k = 5.0 | ||
# Sigma value corresponding to the covariance ellipse | ||
k = 5 | ||
|
||
angle = np.arctan2(v[1, 0], v[0, 0]) | ||
# We multiply k by 2 since k corresponds to the radius but Ellipse uses | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Wow. I guess we were looking at the wrong ellipses all that time! There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. On the bright side, your numbers would have still been correct! The requested changes have been made. Please let me know if you need anything else. Thanks! |
||
# the diameter. | ||
e1 = patches.Ellipse(point, | ||
np.sqrt(w[0] * k), | ||
np.sqrt(w[1] * k), | ||
np.sqrt(w[0]) * 2 * k, | ||
np.sqrt(w[1]) * 2 * k, | ||
np.rad2deg(angle), | ||
fill=False) | ||
axes.add_patch(e1) | ||
|
@@ -178,6 +190,14 @@ 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 = 2.296 corresponds to 1 std, 68.26% of all probability | ||
k = 11.820 corresponds to 3 std, 99.74% of all probability | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. same? |
||
|
||
We choose k = 5 which corresponds to 99.99963% of all probability for 2D. | ||
|
||
Args: | ||
axes (matplotlib.axes.Axes): Matplotlib axes. | ||
pose: The pose to be plotted. | ||
|
@@ -205,13 +225,15 @@ def plot_pose2_on_axes(axes, | |
|
||
w, v = np.linalg.eig(gPp) | ||
|
||
# k = 2.296 | ||
k = 5.0 | ||
# Sigma value corresponding to the covariance ellipse | ||
k = 5 | ||
|
||
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] * k), | ||
np.sqrt(w[1] * k), | ||
np.sqrt(w[0]) * 2 * k, | ||
np.sqrt(w[1]) * 2 * k, | ||
np.rad2deg(angle), | ||
fill=False) | ||
axes.add_patch(e1) | ||
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Do you have an online reference?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
With code from other comment:
So seems about right if we want this behavior.