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", "cell_type": "code",
"execution_count": 3, "execution_count": 5,
"metadata": {}, "metadata": {},
"outputs": [], "outputs": [],
"source": [ "source": [
"import scipy\n", "import scipy\n",
"import scipy.stats" "import scipy.stats\n",
"import numpy as np"
] ]
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 9, "execution_count": 6,
"metadata": {}, "metadata": {},
"outputs": [], "outputs": [],
"source": [ "source": [
@ -36,7 +37,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 23, "execution_count": 7,
"metadata": {}, "metadata": {},
"outputs": [ "outputs": [
{ {
@ -51,17 +52,17 @@
} }
], ],
"source": [ "source": [
"for dim in range(0, 4):\n", "for dof in range(0, 4):\n",
" print(\"{}D\".format(dim), end=\"\")\n", " print(\"{}D\".format(dof), end=\"\")\n",
" for sigma in range(1, 6):\n", " for sigma in range(1, 6):\n",
" if dim == 0: print(\"\\t {} \".format(sigma), end=\"\")\n", " if dof == 0: print(\"\\t {} \".format(sigma), end=\"\")\n",
" else: print(\"\\t{:.5f}%\".format(sigma_to_pct(sigma, dim)), end=\"\")\n", " else: print(\"\\t{:.5f}%\".format(sigma_to_pct(sigma, dof)), end=\"\")\n",
" print()" " print()"
] ]
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 29, "execution_count": 12,
"metadata": {}, "metadata": {},
"outputs": [ "outputs": [
{ {
@ -70,34 +71,30 @@
"text": [ "text": [
"1D\n", "1D\n",
"\n", "\n",
"sigma=1.0 -> 68.26895%\n", "pct=50.0 -> sigma=0.674489750196\n",
"sigma=2.0 -> 95.44997%\n", "pct=95.0 -> sigma=1.959963984540\n",
"sigma=2.5 -> 98.75807%\n", "pct=99.0 -> sigma=2.575829303549\n",
"sigma=5.0 -> 99.99994%\n",
"\n", "\n",
"2D\n", "2D\n",
"\n", "\n",
"sigma=1.0 -> 39.34693%\n", "pct=50.0 -> sigma=1.177410022515\n",
"sigma=2.0 -> 86.46647%\n", "pct=95.0 -> sigma=2.447746830681\n",
"sigma=2.5 -> 95.60631%\n", "pct=99.0 -> sigma=3.034854258770\n",
"sigma=5.0 -> 99.99963%\n",
"\n", "\n",
"3D\n", "3D\n",
"\n", "\n",
"sigma=1.0 -> 19.87480%\n", "pct=50.0 -> sigma=1.538172254455\n",
"sigma=2.0 -> 73.85359%\n", "pct=95.0 -> sigma=2.795483482915\n",
"sigma=2.5 -> 89.99392%\n", "pct=99.0 -> sigma=3.368214175219\n",
"sigma=5.0 -> 99.99846%\n",
"\n" "\n"
] ]
} }
], ],
"source": [ "source": [
"for dim in range(1, 4):\n", "for dof in range(1, 4):\n",
" print(\"{}D\\n\".format(dim))\n", " print(\"{}D\\n\".format(dof))\n",
" for sigma in [1, 2, 2.5, 5]:\n", " for pct in [50, 95, 99]:\n",
" print(\"sigma={:.1f} -> {:.5f}%\".format(sigma, sigma_to_pct(sigma, dim)), end=\"\")\n", " print(\"pct={:.1f} -> sigma={:.12f}\".format(pct, pct_to_sigma(pct, dof)))\n",
" print()\n",
" print()" " print()"
] ]
}, },
@ -111,10 +108,10 @@
], ],
"metadata": { "metadata": {
"interpreter": { "interpreter": {
"hash": "341996cd3f3db7b5e0d1eaea072c5502d80452314e72e6b77c40445f6e9ba101" "hash": "4d608302ba82e7596903db5446e6fa05f049271852e8cc6e1cafaafe5fbd9fed"
}, },
"kernelspec": { "kernelspec": {
"display_name": "Python 3 (ipykernel)", "display_name": "Python 3.8.13 ('gtsfm-v1')",
"language": "python", "language": "python",
"name": "python3" "name": "python3"
}, },
@ -128,7 +125,7 @@
"name": "python", "name": "python",
"nbconvert_exporter": "python", "nbconvert_exporter": "python",
"pygments_lexer": "ipython3", "pygments_lexer": "ipython3",
"version": "3.10.4" "version": "3.8.13"
} }
}, },
"nbformat": 4, "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 from gtsam import Marginals, Point2, Point3, Pose2, Pose3, Values
# For translation between a scaling of the uncertainty ellipse and the percentage of # For translation between a scaling of the uncertainty ellipse and the
# inliers see discussion in https://github.com/borglab/gtsam/pull/1067 # percentage of inliers see discussion in
# # [PR 1067](https://github.com/borglab/gtsam/pull/1067)
# Both directions can be calculated by the following functions: # and the notebook python/gtsam/notebooks/ellipses.ipynb (needs scipy).
# 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 # In the following, the default scaling is chosen for 95% inliers, which
# translates to the following sigma values: # translates to the following sigma values:
# 2D: pct_to_sigma(95, 2) -> 2.447746830680816 # 1D: 1.959963984540
# 3D: pct_to_sigma(95, 3) -> 2.7954834829151074 # 2D: 2.447746830681
# 3D: 2.795483482915
# #
# Further references are Stochastic Models, Estimation, and Control Vol 1 by Maybeck, # 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: # For reference, here are the inlier percentages for some sigma values:
# 1 2 3 4 5 # 1 2 3 4 5
# 1D 68.26895 95.44997 99.73002 99.99367 99.99994 # 1D 68.26895 95.44997 99.73002 99.99367 99.99994
# 2D 39.34693 86.46647 98.88910 99.96645 99.99963 # 2D 39.34693 86.46647 98.88910 99.96645 99.99963
# 3D 19.87480 73.85359 97.07091 99.88660 99.99846 # 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: 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]. alpha: Transparency value for the plotted surface in the range [0, 1].
""" """
# this corresponds to 95%, see note above # this corresponds to 95%, see note above
k = 2.7954834829151074 k = 2.795483482915
U, S, _ = np.linalg.svd(P) U, S, _ = np.linalg.svd(P)
radii = k * np.sqrt(S) radii = k * np.sqrt(S)
@ -160,10 +144,10 @@ def plot_covariance_ellipse_2d(axes,
which will be represented as an ellipse. 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 # this corresponds to 95%, see note above
k = 2.447746830680816 k = 2.447746830681
angle = np.arctan2(v[1, 0], v[0, 0]) angle = np.arctan2(v[1, 0], v[0, 0])
# We multiply k by 2 since k corresponds to the radius but Ellipse uses # We multiply k by 2 since k corresponds to the radius but Ellipse uses