12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added notebook with SenselessDev code.

release/4.3a0
Frank Dellaert 2022-05-10 22:50:10 -04:00
parent 638d391a01
commit de1827ca40
2 changed files with 37 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
python/gtsam/notebooks/ellipses.ipynb
View File

@ -13,17 +13,18 @@
},
{
"cell_type": "code",
"execution_count": 3,
"execution_count": 5,
"metadata": {},
"outputs": [],
"source": [
"import scipy\n",
"import scipy.stats"
"import scipy.stats\n",
"import numpy as np"
]
},
{
"cell_type": "code",
"execution_count": 9,
"execution_count": 6,
"metadata": {},
"outputs": [],
"source": [
@ -36,7 +37,7 @@
},
{
"cell_type": "code",
"execution_count": 23,
"execution_count": 7,
"metadata": {},
"outputs": [
{
@ -51,17 +52,17 @@
}
],
"source": [
"for dim in range(0, 4):\n",
" print(\"{}D\".format(dim), end=\"\")\n",
"for dof in range(0, 4):\n",
" print(\"{}D\".format(dof), end=\"\")\n",
" for sigma in range(1, 6):\n",
" if dim == 0: print(\"\\t {} \".format(sigma), end=\"\")\n",
" else: print(\"\\t{:.5f}%\".format(sigma_to_pct(sigma, dim)), end=\"\")\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": 29,
"execution_count": 12,
"metadata": {},
"outputs": [
{
@ -70,34 +71,30 @@
"text": [
"1D\n",
"\n",
"sigma=1.0 -> 68.26895%\n",
"sigma=2.0 -> 95.44997%\n",
"sigma=2.5 -> 98.75807%\n",
"sigma=5.0 -> 99.99994%\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",
"sigma=1.0 -> 39.34693%\n",
"sigma=2.0 -> 86.46647%\n",
"sigma=2.5 -> 95.60631%\n",
"sigma=5.0 -> 99.99963%\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",
"sigma=1.0 -> 19.87480%\n",
"sigma=2.0 -> 73.85359%\n",
"sigma=2.5 -> 89.99392%\n",
"sigma=5.0 -> 99.99846%\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 dim in range(1, 4):\n",
" print(\"{}D\\n\".format(dim))\n",
" for sigma in [1, 2, 2.5, 5]:\n",
" print(\"sigma={:.1f} -> {:.5f}%\".format(sigma, sigma_to_pct(sigma, dim)), end=\"\")\n",
" print()\n",
"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()"
]
},
@ -111,10 +108,10 @@
],
"metadata": {
"interpreter": {
"hash": "341996cd3f3db7b5e0d1eaea072c5502d80452314e72e6b77c40445f6e9ba101"
"hash": "4d608302ba82e7596903db5446e6fa05f049271852e8cc6e1cafaafe5fbd9fed"
},
"kernelspec": {
"display_name": "Python 3 (ipykernel)",
"display_name": "Python 3.8.13 ('gtsfm-v1')",
"language": "python",
"name": "python3"
},
@ -128,7 +125,7 @@
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.10.4"
"version": "3.8.13"
}
},
"nbformat": 4,

38
python/gtsam/utils/plot.py
View File

@ -13,41 +13,25 @@ import gtsam
from gtsam import Marginals, Point2, Point3, Pose2, Pose3, Values
# 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.
# 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:
# 2D: pct_to_sigma(95, 2) -> 2.447746830680816
# 3D: pct_to_sigma(95, 3) -> 2.7954834829151074
# 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/
# 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:
"""
@ -123,7 +107,7 @@ def plot_covariance_ellipse_3d(axes,
alpha: Transparency value for the plotted surface in the range [0, 1].
"""
# this corresponds to 95%, see note above
k = 2.7954834829151074
k = 2.795483482915
U, S, _ = np.linalg.svd(P)
radii = k * np.sqrt(S)
@ -160,10 +144,10 @@ def plot_covariance_ellipse_2d(axes,
which will be represented as an ellipse.
"""
w, v = np.linalg.eig(covariance)
w, v = np.linalg.eigh(covariance)
# this corresponds to 95%, see note above
k = 2.447746830680816
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